|
1 | 1 | using DualNumbers |
2 | 2 |
|
3 | | -immutable DualQuaternion{T<:Real} <: Number |
| 3 | +struct DualQuaternion{T<:Real} <: Number |
4 | 4 | q0::Quaternion{T} |
5 | 5 | qe::Quaternion{T} |
6 | 6 | norm::Bool |
7 | 7 | end |
8 | 8 |
|
9 | | -DualQuaternion( q0::Quaternion, qe::Quaternion, n::Bool = false) = |
10 | | - DualQuaternion( promote( q0, qe )..., n ) |
| 9 | +DualQuaternion(q0::Quaternion, qe::Quaternion, n::Bool = false) = |
| 10 | + DualQuaternion(promote(q0, qe)..., n) |
11 | 11 |
|
12 | | -DualQuaternion( d1::Dual, d2::Dual, d3::Dual, d4::Dual, n::Bool = false) = |
13 | | - DualQuaternion( Quaternion( d1.re, d2.re, d3.re, d4.re, n ), |
14 | | - Quaternion( d1.du, d2.du, d3.du, d4.du ), n ) |
| 12 | +DualQuaternion(d1::Dual, d2::Dual, d3::Dual, d4::Dual, n::Bool = false) = |
| 13 | + DualQuaternion(Quaternion(d1.re, d2.re, d3.re, d4.re, n), |
| 14 | + Quaternion(d1.du, d2.du, d3.du, d4.du), n) |
15 | 15 |
|
16 | | -DualQuaternion( x::Real ) = DualQuaternion( Quaternion( x ), Quaternion( zero(x) ), abs(x) == one(x) ) |
| 16 | +DualQuaternion(x::Real) = DualQuaternion(Quaternion(x), Quaternion(zero(x)), abs(x) == one(x)) |
17 | 17 |
|
18 | | -DualQuaternion( d::Dual ) = DualQuaternion( d, zero(d), zero(d), zero(d), abs(d) == one(d.re) ) |
| 18 | +DualQuaternion(d::Dual) = DualQuaternion(d, zero(d), zero(d), zero(d), abs(d) == one(d.re)) |
19 | 19 |
|
20 | | -DualQuaternion( q::Quaternion ) = DualQuaternion( q, zero(q), q.norm ) |
| 20 | +DualQuaternion(q::Quaternion) = DualQuaternion(q, zero(q), q.norm) |
21 | 21 |
|
22 | | -DualQuaternion( a::Vector ) = DualQuaternion( zero(Quaternion{typeof(a[1])}), Quaternion( a ) ) |
| 22 | +DualQuaternion(a::Vector) = DualQuaternion(zero(Quaternion{typeof(a[1])}), Quaternion(a)) |
23 | 23 |
|
24 | | -convert{T}(::Type{DualQuaternion{T}}, x::Real) = |
25 | | - DualQuaternion( convert( Quaternion{T}, x ), convert(Quaternion{T},0) ) |
| 24 | +convert(::Type{DualQuaternion{T}}, x::Real) where {T} = |
| 25 | + DualQuaternion(convert(Quaternion{T}, x), convert(Quaternion{T}, 0)) |
26 | 26 |
|
27 | | -convert{T}(::Type{DualQuaternion{T}}, d::Dual) = |
28 | | - DualQuaternion( convert( Dual{T}, d ), convert( Dual{T}, 0 ), convert( Dual{T}, 0 ), convert( Dual{T}, 0 ) ) |
| 27 | +convert(::Type{DualQuaternion{T}}, d::Dual) where {T} = |
| 28 | + DualQuaternion(convert(Dual{T}, d), convert(Dual{T}, 0), convert(Dual{T}, 0), convert(Dual{T}, 0)) |
29 | 29 |
|
30 | | -convert{T}(::Type{DualQuaternion{T}}, q::Quaternion) = |
31 | | - DualQuaternion( convert( Quaternion{T}, q ), convert( Quaternion{T}, 0 ), q.norm ) |
| 30 | +convert(::Type{DualQuaternion{T}}, q::Quaternion) where {T} = |
| 31 | + DualQuaternion(convert(Quaternion{T}, q), convert(Quaternion{T}, 0), q.norm) |
32 | 32 |
|
33 | | -convert{T<:Real}(::Type{DualQuaternion{T}}, q::DualQuaternion{T}) = q |
| 33 | +convert(::Type{DualQuaternion{T}}, q::DualQuaternion{T}) where {T <: Real} = q |
34 | 34 |
|
35 | | -convert{T}(::Type{DualQuaternion{T}}, dq::DualQuaternion) = |
36 | | - DualQuaternion( convert( Quaternion{T}, dq.q0 ), convert( Quaternion{T}, dq.qe ), dq.norm ) |
| 35 | +convert(::Type{DualQuaternion{T}}, dq::DualQuaternion) where {T} = |
| 36 | + DualQuaternion(convert(Quaternion{T}, dq.q0), convert(Quaternion{T}, dq.qe), dq.norm) |
37 | 37 |
|
38 | | -promote_rule{T<:Real}(::Type{DualQuaternion{T}}, ::Type{T}) = DualQuaternion{T} |
39 | | -promote_rule{T<:Real}(::Type{DualQuaternion}, ::Type{T}) = DualQuaternion |
40 | | -promote_rule{T<:Real,S<:Real}(::Type{DualQuaternion{T}}, ::Type{S}) = DualQuaternion{promote_type(T,S)} |
41 | | -promote_rule{T<:Real,S<:Real}(::Type{Quaternion{T}}, ::Type{DualQuaternion{S}}) = DualQuaternion{promote_type(T,S)} |
42 | | -promote_rule{T<:Real,S<:Real}(::Type{DualQuaternion{T}}, ::Type{DualQuaternion{S}}) = DualQuaternion{promote_type(T,S)} |
| 38 | +promote_rule(::Type{DualQuaternion{T}}, ::Type{T}) where {T <: Real} = DualQuaternion{T} |
| 39 | +promote_rule(::Type{DualQuaternion}, ::Type{T}) where {T <: Real} = DualQuaternion |
| 40 | +promote_rule(::Type{DualQuaternion{T}}, ::Type{S}) where {T <: Real, S <: Real} = DualQuaternion{promote_type(T, S)} |
| 41 | +promote_rule(::Type{Quaternion{T}}, ::Type{DualQuaternion{S}}) where {T <: Real, S <: Real} = DualQuaternion{promote_type(T, S)} |
| 42 | +promote_rule(::Type{DualQuaternion{T}}, ::Type{DualQuaternion{S}}) where {T <: Real, S <: Real} = DualQuaternion{promote_type(T, S)} |
43 | 43 |
|
44 | | -dualquat( q1, q2, n=false ) = DualQuaternion( q1, q2, n ) |
45 | | -dualquat( d1, d2, d3, d4, n=false ) = DualQuaternion( d1, d2, d3, d4, n ) |
46 | | -dualquat( x ) = DualQuaternion( x ) |
| 44 | +dualquat(q1, q2, n=false) = DualQuaternion(q1, q2, n) |
| 45 | +dualquat(d1, d2, d3, d4, n=false) = DualQuaternion(d1, d2, d3, d4, n) |
| 46 | +dualquat(x) = DualQuaternion(x) |
47 | 47 |
|
48 | 48 | function show(io::IO, dq::DualQuaternion) |
49 | | - show( io, dq.q0 ) |
50 | | - print( io, " + ( " ) |
51 | | - show( io, dq.qe ) |
52 | | - print( io, " )du" ) |
| 49 | + show(io, dq.q0) |
| 50 | + print(io, " + ( ") |
| 51 | + show(io, dq.qe) |
| 52 | + print(io, " )du") |
53 | 53 | end |
54 | 54 |
|
55 | | -Q0( dq::DualQuaternion ) = dq.q0 |
56 | | -Qe( dq::DualQuaternion ) = dq.qe |
| 55 | +Q0(dq::DualQuaternion) = dq.q0 |
| 56 | +Qe(dq::DualQuaternion) = dq.qe |
57 | 57 |
|
58 | | -(/)(dq::DualQuaternion, x::Real) = DualQuaternion( dq.q0 / x, dq.qe / x ) |
| 58 | +(/)(dq::DualQuaternion, x::Real) = DualQuaternion(dq.q0 / x, dq.qe / x) |
59 | 59 |
|
60 | 60 | (/)(dq::DualQuaternion, d::Dual) = |
61 | | - DualQuaternion( dual( dq.q0.s , dq.qe.s ) / d, |
62 | | - dual( dq.q0.v1, dq.qe.v1 ) / d, |
63 | | - dual( dq.q0.v2, dq.qe.v2 ) / d, |
64 | | - dual( dq.q0.v3, dq.qe.v3 ) / d ) |
65 | | - |
66 | | -abs2( dq::DualQuaternion ) = dq.norm ? dual( one( dq.q0.s ) ) : |
67 | | - dual( abs2( dq.q0 ) , |
68 | | - 2.0 * ( dq.q0.s * dq.qe.s + |
| 61 | + DualQuaternion(dual(dq.q0.s, dq.qe.s) / d, |
| 62 | + dual(dq.q0.v1, dq.qe.v1) / d, |
| 63 | + dual(dq.q0.v2, dq.qe.v2) / d, |
| 64 | + dual(dq.q0.v3, dq.qe.v3) / d) |
| 65 | + |
| 66 | +abs2(dq::DualQuaternion) = dq.norm ? dual(one(dq.q0.s)) : |
| 67 | + dual(abs2(dq.q0), |
| 68 | + 2.0 * (dq.q0.s * dq.qe.s + |
69 | 69 | dq.q0.v1 * dq.qe.v1 + |
70 | 70 | dq.q0.v2 * dq.qe.v2 + |
71 | | - dq.q0.v3 * dq.qe.v3 ) ) |
| 71 | + dq.q0.v3 * dq.qe.v3)) |
72 | 72 |
|
73 | | -abs( dq::DualQuaternion ) = dq.norm ? dual( one( dq.q0.s ) ) : sqrt( abs2( dq ) ) |
| 73 | +abs(dq::DualQuaternion) = dq.norm ? dual(one(dq.q0.s)) : sqrt(abs2(dq)) |
74 | 74 |
|
75 | | -conj( dq::DualQuaternion ) = DualQuaternion( conj( dq.q0 ), conj( dq.qe ), dq.norm ) |
76 | | -dconj( dq::DualQuaternion ) = DualQuaternion( dq.q0, -dq.qe, dq.norm ) |
| 75 | +conj(dq::DualQuaternion) = DualQuaternion(conj(dq.q0), conj(dq.qe), dq.norm) |
| 76 | +dconj(dq::DualQuaternion) = DualQuaternion(dq.q0, -dq.qe, dq.norm) |
77 | 77 |
|
78 | | -inv( dq::DualQuaternion ) = dq.norm ? conj( dq ) : conj( dq ) / abs2( dq ) |
| 78 | +inv(dq::DualQuaternion) = dq.norm ? conj(dq) : conj(dq) / abs2(dq) |
79 | 79 |
|
80 | | -function normalize( dq::DualQuaternion ) |
81 | | - if ( dq.norm ) |
| 80 | +function normalize(dq::DualQuaternion) |
| 81 | + if (dq.norm) |
82 | 82 | return dq |
83 | 83 | end |
84 | | - a = abs( dq ) |
85 | | - if abs( a ) > 0 |
| 84 | + a = abs(dq) |
| 85 | + if abs(a) > 0 |
86 | 86 | qa = dq / a |
87 | | - dualquat( qa.q0, qa.qe, true ) |
| 87 | + dualquat(qa.q0, qa.qe, true) |
88 | 88 | else |
89 | 89 | dq |
90 | 90 | end |
91 | 91 | end |
92 | 92 |
|
93 | | -function normalizea( dq::DualQuaternion ) |
94 | | - if ( dq.norm ) |
95 | | - return ( dq, one( dual ) ) |
| 93 | +function normalizea(dq::DualQuaternion) |
| 94 | + if (dq.norm) |
| 95 | + return (dq, one(dual)) |
96 | 96 | end |
97 | | - a = abs( dq ) |
98 | | - if abs( a ) > 0 |
| 97 | + a = abs(dq) |
| 98 | + if abs(a) > 0 |
99 | 99 | qa = dq / a |
100 | | - dualquat( qa.q0, qa.qe, true ), a |
| 100 | + dualquat(qa.q0, qa.qe, true), a |
101 | 101 | else |
102 | | - dq, zero( dual ) |
| 102 | + dq, zero(dual) |
103 | 103 | end |
104 | 104 | end |
105 | 105 |
|
106 | | -(-)(dq::DualQuaternion) = DualQuaternion( -dq.q0, -dq.qe, dq.norm) |
| 106 | +(-)(dq::DualQuaternion) = DualQuaternion(-dq.q0, -dq.qe, dq.norm) |
107 | 107 |
|
108 | | -(+)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion( dq.q0 + dw.q0, dq.qe + dw.qe ) |
109 | | -(-)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion( dq.q0 - dw.q0, dq.qe - dw.qe ) |
110 | | -(*)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion( dq.q0 * dw.q0, |
| 108 | +(+)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion(dq.q0 + dw.q0, dq.qe + dw.qe) |
| 109 | +(-)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion(dq.q0 - dw.q0, dq.qe - dw.qe) |
| 110 | +(*)(dq::DualQuaternion, dw::DualQuaternion) = DualQuaternion(dq.q0 * dw.q0, |
111 | 111 | dq.q0 * dw.qe + dq.qe * dw.q0, |
112 | | - dq.norm && dw.norm ) |
113 | | -(/)(dq::DualQuaternion, dw::DualQuaternion) = dq*inv(dw) |
| 112 | + dq.norm && dw.norm) |
| 113 | +(/)(dq::DualQuaternion, dw::DualQuaternion) = dq * inv(dw) |
114 | 114 |
|
115 | | -function angleaxis( dq::DualQuaternion ) |
116 | | - tq = dq.qe * conj( dq.q0 ) |
117 | | - t = [ 2.0 * tq.v1, 2.0 * tq.v2, 2.0 * tq.v3 ] |
| 115 | +function angleaxis(dq::DualQuaternion) |
| 116 | + tq = dq.qe * conj(dq.q0) |
| 117 | + t = [2.0 * tq.v1, 2.0 * tq.v2, 2.0 * tq.v3] |
118 | 118 | q0s = dq.q0.s |
119 | | - th0, s0 = angleaxis( dq.q0 ) |
120 | | - sq0 = quat( 0.0, s0 ) |
121 | | - if abs( abs( q0s ) - one( q0s ) ) == 0 |
122 | | - th = dual( th0, 0.5 * abs( quat( 0, t ) ) ) |
123 | | - th, dualquat( sq0 ) |
| 119 | + th0, s0 = angleaxis(dq.q0) |
| 120 | + sq0 = quat(0.0, s0) |
| 121 | + if abs(abs(q0s) - one(q0s)) == 0 |
| 122 | + th = dual(th0, 0.5 * abs(quat(0, t))) |
| 123 | + th, dualquat(sq0) |
124 | 124 | else |
125 | | - th = dual( th0, 0.5 * dot( t, s0 ) ) |
126 | | - s0c1 = cross( s0, t ) |
127 | | - tanth = tan( th0 ) |
128 | | - s0c2 = ( s0c1 / tanth + t ) * 0.5 |
129 | | - sqev = cross( s0c2, s0 ) |
130 | | - th, dualquat( sq0, quat( 0.0, sqev ) ) |
| 125 | + th = dual(th0, 0.5 * dot(t, s0)) |
| 126 | + s0c1 = cross(s0, t) |
| 127 | + tanth = tan(th0) |
| 128 | + s0c2 = (s0c1 / tanth + t) * 0.5 |
| 129 | + sqev = cross(s0c2, s0) |
| 130 | + th, dualquat(sq0, quat(0.0, sqev)) |
131 | 131 | end |
132 | 132 | end |
133 | 133 |
|
134 | | -function angle( dq::DualQuaternion ) |
135 | | - th, ax = angleaxis( dq ) |
| 134 | +function angle(dq::DualQuaternion) |
| 135 | + th, ax = angleaxis(dq) |
136 | 136 | th |
137 | 137 | end |
138 | 138 |
|
139 | | -function axis( dq::DualQuaternion ) |
140 | | - th, ax = angleaxis( dq ) |
| 139 | +function axis(dq::DualQuaternion) |
| 140 | + th, ax = angleaxis(dq) |
141 | 141 | ax |
142 | 142 | end |
143 | 143 |
|
144 | | -function exp( dq::DualQuaternion ) |
145 | | - se = dual( dq.q0.s, dq.qe.s ) |
146 | | - se = exp( se ) |
147 | | - dq = dualquat( quat( 0.0, imag( dq.q0 ) ), quat( 0.0, imag( dq.qe ) ) ) |
148 | | - dq, th = normalizea( dq ) |
| 144 | +function exp(dq::DualQuaternion) |
| 145 | + se = dual(dq.q0.s, dq.qe.s) |
| 146 | + se = exp(se) |
| 147 | + dq = dualquat(quat(0.0, imag(dq.q0)), quat(0.0, imag(dq.qe))) |
| 148 | + dq, th = normalizea(dq) |
149 | 149 | if dq.norm |
150 | | - dualquat( se ) * ( dualquat( cos( th ) ) + dq * dualquat( sin( th ) ) ) |
| 150 | + dualquat(se) * (dualquat(cos(th)) + dq * dualquat(sin(th))) |
151 | 151 | else |
152 | | - dualquat( se ) |
| 152 | + dualquat(se) |
153 | 153 | end |
154 | 154 | end |
155 | 155 |
|
156 | | -function log( dq::DualQuaternion ) |
157 | | - dq, a = normalizea( dq ) |
158 | | - sl = log( a ) |
159 | | - th, s = angleaxis( dq ) |
160 | | - s * dualquat( th ) + dualquat( sl ) |
| 156 | +function log(dq::DualQuaternion) |
| 157 | + dq, a = normalizea(dq) |
| 158 | + sl = log(a) |
| 159 | + th, s = angleaxis(dq) |
| 160 | + s * dualquat(th) + dualquat(sl) |
161 | 161 | end |
162 | 162 |
|
163 | | -(^)(dq::DualQuaternion, dw::DualQuaternion) = exp( dw * log( dq ) ) |
| 163 | +(^)(dq::DualQuaternion, dw::DualQuaternion) = exp(dw * log(dq)) |
164 | 164 |
|
165 | | -function sqrt( dq::DualQuaternion ) |
166 | | - exp( 0.5 * log( dq ) ) |
| 165 | +function sqrt(dq::DualQuaternion) |
| 166 | + exp(0.5 * log(dq)) |
167 | 167 | end |
168 | 168 |
|
169 | | -dualquatrand() = dualquat( quatrand(), quatrand() ) |
170 | | -ndualquatrand() = normalize( dualquatrand() ) |
| 169 | +dualquatrand() = dualquat(quatrand(), quatrand()) |
| 170 | +ndualquatrand() = normalize(dualquatrand()) |
0 commit comments