1+ module ForwardDiffStaticArraysExt
2+
3+ using ForwardDiff, StaticArrays
4+ using ForwardDiff. LinearAlgebra
5+ using ForwardDiff. DiffResults
6+ using ForwardDiff: Dual, partials, GradientConfig, JacobianConfig, HessianConfig, Tag, Chunk,
7+ gradient, hessian, jacobian, gradient!, hessian!, jacobian!,
8+ extract_gradient!, extract_jacobian!, extract_value!,
9+ vector_mode_gradient, vector_mode_gradient!,
10+ vector_mode_jacobian, vector_mode_jacobian!, valtype, value, _lyap_div!
11+ using DiffResults: DiffResult, ImmutableDiffResult, MutableDiffResult
12+
13+ @generated function dualize (:: Type{T} , x:: StaticArray ) where T
14+ N = length (x)
15+ dx = Expr (:tuple , [:(Dual {T} (x[$ i], chunk, Val {$i} ())) for i in 1 : N]. .. )
16+ V = StaticArrays. similar_type (x, Dual{T,eltype (x),N})
17+ return quote
18+ chunk = Chunk {$N} ()
19+ $ (Expr (:meta , :inline ))
20+ return $ V ($ (dx))
21+ end
22+ end
23+
24+ @inline static_dual_eval (:: Type{T} , f, x:: StaticArray ) where T = f (dualize (T, x))
25+
26+ function LinearAlgebra. eigvals (A:: Symmetric{<:Dual{Tg,T,N}, <:StaticArrays.StaticMatrix} ) where {Tg,T<: Real ,N}
27+ λ,Q = eigen (Symmetric (value .(parent (A))))
28+ parts = ntuple (j -> diag (Q' * getindex .(partials .(A), j) * Q), N)
29+ Dual {Tg} .(λ, tuple .(parts... ))
30+ end
31+
32+ function LinearAlgebra. eigen (A:: Symmetric{<:Dual{Tg,T,N}, <:StaticArrays.StaticMatrix} ) where {Tg,T<: Real ,N}
33+ λ = eigvals (A)
34+ _,Q = eigen (Symmetric (value .(parent (A))))
35+ parts = ntuple (j -> Q* ForwardDiff. _lyap_div! (Q' * getindex .(partials .(A), j) * Q - Diagonal (getindex .(partials .(λ), j)), value .(λ)), N)
36+ Eigen (λ,Dual {Tg} .(Q, tuple .(parts... )))
37+ end
38+
39+ # Gradient
40+ @inline ForwardDiff. gradient (f, x:: StaticArray ) = vector_mode_gradient (f, x)
41+ @inline ForwardDiff. gradient (f, x:: StaticArray , cfg:: GradientConfig ) = gradient (f, x)
42+ @inline ForwardDiff. gradient (f, x:: StaticArray , cfg:: GradientConfig , :: Val ) = gradient (f, x)
43+
44+ @inline ForwardDiff. gradient! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray ) = vector_mode_gradient! (result, f, x)
45+ @inline ForwardDiff. gradient! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray , cfg:: GradientConfig ) = gradient! (result, f, x)
46+ @inline ForwardDiff. gradient! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray , cfg:: GradientConfig , :: Val ) = gradient! (result, f, x)
47+
48+ @generated function extract_gradient (:: Type{T} , y:: Real , x:: S ) where {T,S<: StaticArray }
49+ result = Expr (:tuple , [:(partials (T, y, $ i)) for i in 1 : length (x)]. .. )
50+ return quote
51+ $ (Expr (:meta , :inline ))
52+ V = StaticArrays. similar_type (S, valtype ($ y))
53+ return V ($ result)
54+ end
55+ end
56+
57+ @inline function ForwardDiff. vector_mode_gradient (f, x:: StaticArray )
58+ T = typeof (Tag (f, eltype (x)))
59+ return extract_gradient (T, static_dual_eval (T, f, x), x)
60+ end
61+
62+ @inline function ForwardDiff. vector_mode_gradient! (result, f, x:: StaticArray )
63+ T = typeof (Tag (f, eltype (x)))
64+ return extract_gradient! (T, result, static_dual_eval (T, f, x))
65+ end
66+
67+ # Jacobian
68+ @inline ForwardDiff. jacobian (f, x:: StaticArray ) = vector_mode_jacobian (f, x)
69+ @inline ForwardDiff. jacobian (f, x:: StaticArray , cfg:: JacobianConfig ) = jacobian (f, x)
70+ @inline ForwardDiff. jacobian (f, x:: StaticArray , cfg:: JacobianConfig , :: Val ) = jacobian (f, x)
71+
72+ @inline ForwardDiff. jacobian! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray ) = vector_mode_jacobian! (result, f, x)
73+ @inline ForwardDiff. jacobian! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray , cfg:: JacobianConfig ) = jacobian! (result, f, x)
74+ @inline ForwardDiff. jacobian! (result:: Union{AbstractArray,DiffResult} , f, x:: StaticArray , cfg:: JacobianConfig , :: Val ) = jacobian! (result, f, x)
75+
76+ @generated function extract_jacobian (:: Type{T} , ydual:: StaticArray , x:: S ) where {T,S<: StaticArray }
77+ M, N = length (ydual), length (x)
78+ result = Expr (:tuple , [:(partials (T, ydual[$ i], $ j)) for i in 1 : M, j in 1 : N]. .. )
79+ return quote
80+ $ (Expr (:meta , :inline ))
81+ V = StaticArrays. similar_type (S, valtype (eltype ($ ydual)), Size ($ M, $ N))
82+ return V ($ result)
83+ end
84+ end
85+
86+ function extract_jacobian (:: Type{T} , ydual:: AbstractArray , x:: StaticArray ) where T
87+ result = similar (ydual, valtype (eltype (ydual)), length (ydual), length (x))
88+ return extract_jacobian! (T, result, ydual, length (x))
89+ end
90+
91+ @inline function ForwardDiff. vector_mode_jacobian (f, x:: StaticArray )
92+ T = typeof (Tag (f, eltype (x)))
93+ return extract_jacobian (T, static_dual_eval (T, f, x), x)
94+ end
95+
96+ @inline function ForwardDiff. vector_mode_jacobian! (result, f, x:: StaticArray )
97+ T = typeof (Tag (f, eltype (x)))
98+ ydual = static_dual_eval (T, f, x)
99+ result = extract_jacobian! (T, result, ydual, length (x))
100+ result = extract_value! (T, result, ydual)
101+ return result
102+ end
103+
104+ @inline function ForwardDiff. vector_mode_jacobian! (result:: ImmutableDiffResult , f, x:: StaticArray )
105+ T = typeof (Tag (f, eltype (x)))
106+ ydual = static_dual_eval (T, f, x)
107+ result = DiffResults. jacobian! (result, extract_jacobian (T, ydual, x))
108+ result = DiffResults. value! (d -> value (T,d), result, ydual)
109+ return result
110+ end
111+
112+ # Hessian
113+ ForwardDiff. hessian (f, x:: StaticArray ) = jacobian (y -> gradient (f, y), x)
114+ ForwardDiff. hessian (f, x:: StaticArray , cfg:: HessianConfig ) = hessian (f, x)
115+ ForwardDiff. hessian (f, x:: StaticArray , cfg:: HessianConfig , :: Val ) = hessian (f, x)
116+
117+ ForwardDiff. hessian! (result:: AbstractArray , f, x:: StaticArray ) = jacobian! (result, y -> gradient (f, y), x)
118+
119+ ForwardDiff. hessian! (result:: MutableDiffResult , f, x:: StaticArray ) = hessian! (result, f, x, HessianConfig (f, result, x))
120+
121+ ForwardDiff. hessian! (result:: ImmutableDiffResult , f, x:: StaticArray , cfg:: HessianConfig ) = hessian! (result, f, x)
122+ ForwardDiff. hessian! (result:: ImmutableDiffResult , f, x:: StaticArray , cfg:: HessianConfig , :: Val ) = hessian! (result, f, x)
123+
124+ function ForwardDiff. hessian! (result:: ImmutableDiffResult , f, x:: StaticArray )
125+ T = typeof (Tag (f, eltype (x)))
126+ d1 = dualize (T, x)
127+ d2 = dualize (T, d1)
128+ fd2 = f (d2)
129+ val = value (T,value (T,fd2))
130+ grad = extract_gradient (T,value (T,fd2), x)
131+ hess = extract_jacobian (T,partials (T,fd2), x)
132+ result = DiffResults. hessian! (result, hess)
133+ result = DiffResults. gradient! (result, grad)
134+ result = DiffResults. value! (result, val)
135+ return result
136+ end
137+
138+ end
0 commit comments