|
| 1 | +using SymbolicUtils: istree |
| 2 | + |
| 3 | +function nterms(t) |
| 4 | + if istree(t) |
| 5 | + return reduce(+, map(nterms, arguments(t)), init=0) |
| 6 | + else |
| 7 | + return 1 |
| 8 | + end |
| 9 | +end |
| 10 | +# Soft pivoted |
| 11 | +# Note: we call this function with a matrix of Union{SymbolicUtils.Symbolic, Any} |
| 12 | +# It should work as-is with Operation type too. |
| 13 | +function sym_lu(A) |
| 14 | + m, n = size(A) |
| 15 | + L = fill!(Array{Any}(undef, size(A)),0) # TODO: make sparse? |
| 16 | + for i=1:min(m, n) |
| 17 | + L[i,i] = 1 |
| 18 | + end |
| 19 | + U = copy!(Array{Any}(undef, size(A)),A) |
| 20 | + p = BlasInt[1:m;] |
| 21 | + for k = 1:m-1 |
| 22 | + _, i = findmin(map(ii->iszero(U[ii, k]) ? Inf : nterms(U[ii,k]), k:n)) |
| 23 | + i += k - 1 |
| 24 | + # swap |
| 25 | + U[k, k:end], U[i, k:end] = U[i, k:end], U[k, k:end] |
| 26 | + L[k, 1:k-1], L[i, 1:k-1] = L[i, 1:k-1], L[k, 1:k-1] |
| 27 | + p[k] = i |
| 28 | + |
| 29 | + for j = k+1:m |
| 30 | + L[j,k] = U[j, k] / U[k, k] |
| 31 | + U[j,k:m] .= U[j,k:m] .- L[j,k] .* U[k,k:m] |
| 32 | + end |
| 33 | + end |
| 34 | + for j=1:m |
| 35 | + for i=j+1:n |
| 36 | + U[i,j] = 0 |
| 37 | + end |
| 38 | + end |
| 39 | + |
| 40 | + (L, U, LinearAlgebra.ipiv2perm(p, m)) |
| 41 | +end |
| 42 | + |
| 43 | +# Given a vector of equations and a |
| 44 | +# list of independent variables, |
| 45 | +# return the coefficient matrix `A` and a |
| 46 | +# vector of constants (possibly symbolic) `b` such that |
| 47 | +# A \ b will solve the equations for the vars |
| 48 | +function A_b(eqs, vars) |
| 49 | + exprs = rhss(eqs) .- lhss(eqs) |
| 50 | + for ex in exprs |
| 51 | + @assert islinear(ex, vars) |
| 52 | + end |
| 53 | + A = jacobian(exprs, vars) |
| 54 | + b = A * vars - exprs |
| 55 | + A, b |
| 56 | +end |
| 57 | + |
| 58 | +""" |
| 59 | + solve_for(eqs::Vector, vars::Vector) |
| 60 | +
|
| 61 | +Solve the vector of equations `eqs` for a set of variables `vars`. |
| 62 | +
|
| 63 | +Assumes `length(eqs) == length(vars)` |
| 64 | +
|
| 65 | +Currently only works if all equations are linear. |
| 66 | +""" |
| 67 | +function solve_for(eqs, vars) |
| 68 | + A, b = A_b(eqs, vars) |
| 69 | + _solve(A, b) |
| 70 | +end |
| 71 | + |
| 72 | +function _solve(A, b) |
| 73 | + A = SymbolicUtils.simplify.(to_symbolic.(A), polynorm=true) |
| 74 | + b = SymbolicUtils.simplify.(to_symbolic.(b), polynorm=true) |
| 75 | + map(to_mtk, SymbolicUtils.simplify.(ldiv(sym_lu(A), b))) |
| 76 | +end |
| 77 | + |
| 78 | +LinearAlgebra.:(\)(A::AbstractMatrix{<:Expression}, b::AbstractVector{<:Expression}) = _solve(A, b) |
| 79 | +LinearAlgebra.:(\)(A::AbstractMatrix{<:Expression}, b::AbstractVector) = _solve(A, b) |
| 80 | +LinearAlgebra.:(\)(A::AbstractMatrix, b::AbstractVector{<:Expression}) = _solve(A, b) |
| 81 | + |
| 82 | +# ldiv below |
| 83 | + |
| 84 | +_iszero(x::Number) = iszero(x) |
| 85 | +_isone(x::Number) = isone(x) |
| 86 | +_iszero(::Term) = false |
| 87 | +_isone(::Term) = false |
| 88 | + |
| 89 | +function simplifying_dot(x,y) |
| 90 | + isempty(x) && return 0 |
| 91 | + muls = map(x,y) do xi,yi |
| 92 | + _isone(xi) ? yi : _isone(yi) ? xi : _iszero(xi) ? 0 : _iszero(yi) ? 0 : xi * yi |
| 93 | + end |
| 94 | + |
| 95 | + reduce(muls) do acc, x |
| 96 | + _iszero(acc) ? x : _iszero(x) ? acc : acc + x |
| 97 | + end |
| 98 | +end |
| 99 | + |
| 100 | +function ldiv((L,U,p), b) |
| 101 | + m, n = size(L) |
| 102 | + x = Vector{Any}(undef, length(b)) |
| 103 | + b = b[p] |
| 104 | + |
| 105 | + for i=n:-1:1 |
| 106 | + sub = simplifying_dot(x[i+1:end], U[i,i+1:end]) |
| 107 | + den = U[i,i] |
| 108 | + x[i] = _iszero(sub) ? b[i] : b[i] - sub |
| 109 | + x[i] = _isone(den) ? x[i] : _isone(-den) ? -x[i] : x[i] / den |
| 110 | + end |
| 111 | + |
| 112 | + # unit lower triangular solve first: |
| 113 | + for i=1:n |
| 114 | + sub = simplifying_dot(b[1:i-1], L[i, 1:i-1]) # this should be `b` not x |
| 115 | + x[i] = _iszero(sub) ? x[i] : x[i] - sub |
| 116 | + end |
| 117 | + x |
| 118 | +end |
0 commit comments