1 | /* |
2 | --------------------------------------------------------------------------- |
3 | Open Asset Import Library (assimp) |
4 | --------------------------------------------------------------------------- |
5 | |
6 | Copyright (c) 2006-2019, assimp team |
7 | |
8 | |
9 | |
10 | All rights reserved. |
11 | |
12 | Redistribution and use of this software in source and binary forms, |
13 | with or without modification, are permitted provided that the following |
14 | conditions are met: |
15 | |
16 | * Redistributions of source code must retain the above |
17 | copyright notice, this list of conditions and the |
18 | following disclaimer. |
19 | |
20 | * Redistributions in binary form must reproduce the above |
21 | copyright notice, this list of conditions and the |
22 | following disclaimer in the documentation and/or other |
23 | materials provided with the distribution. |
24 | |
25 | * Neither the name of the assimp team, nor the names of its |
26 | contributors may be used to endorse or promote products |
27 | derived from this software without specific prior |
28 | written permission of the assimp team. |
29 | |
30 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
31 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
32 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
33 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
34 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
35 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
36 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
37 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
38 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
39 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
40 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
41 | --------------------------------------------------------------------------- |
42 | */ |
43 | |
44 | /** @file quaternion.inl |
45 | * @brief Inline implementation of aiQuaterniont<TReal> operators |
46 | */ |
47 | #pragma once |
48 | #ifndef AI_QUATERNION_INL_INC |
49 | #define AI_QUATERNION_INL_INC |
50 | |
51 | #ifdef __cplusplus |
52 | #include "quaternion.h" |
53 | |
54 | #include <cmath> |
55 | |
56 | // --------------------------------------------------------------------------- |
57 | template<typename TReal> |
58 | bool aiQuaterniont<TReal>::operator== (const aiQuaterniont& o) const |
59 | { |
60 | return x == o.x && y == o.y && z == o.z && w == o.w; |
61 | } |
62 | |
63 | // --------------------------------------------------------------------------- |
64 | template<typename TReal> |
65 | bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const |
66 | { |
67 | return !(*this == o); |
68 | } |
69 | |
70 | // --------------------------------------------------------------------------- |
71 | template<typename TReal> |
72 | inline bool aiQuaterniont<TReal>::Equal(const aiQuaterniont& o, TReal epsilon) const { |
73 | return |
74 | std::abs(x - o.x) <= epsilon && |
75 | std::abs(y - o.y) <= epsilon && |
76 | std::abs(z - o.z) <= epsilon && |
77 | std::abs(w - o.w) <= epsilon; |
78 | } |
79 | |
80 | // --------------------------------------------------------------------------- |
81 | // Constructs a quaternion from a rotation matrix |
82 | template<typename TReal> |
83 | inline aiQuaterniont<TReal>::aiQuaterniont( const aiMatrix3x3t<TReal> &pRotMatrix) |
84 | { |
85 | TReal t = pRotMatrix.a1 + pRotMatrix.b2 + pRotMatrix.c3; |
86 | |
87 | // large enough |
88 | if( t > static_cast<TReal>(0)) |
89 | { |
90 | TReal s = std::sqrt(1 + t) * static_cast<TReal>(2.0); |
91 | x = (pRotMatrix.c2 - pRotMatrix.b3) / s; |
92 | y = (pRotMatrix.a3 - pRotMatrix.c1) / s; |
93 | z = (pRotMatrix.b1 - pRotMatrix.a2) / s; |
94 | w = static_cast<TReal>(0.25) * s; |
95 | } // else we have to check several cases |
96 | else if( pRotMatrix.a1 > pRotMatrix.b2 && pRotMatrix.a1 > pRotMatrix.c3 ) |
97 | { |
98 | // Column 0: |
99 | TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.a1 - pRotMatrix.b2 - pRotMatrix.c3) * static_cast<TReal>(2.0); |
100 | x = static_cast<TReal>(0.25) * s; |
101 | y = (pRotMatrix.b1 + pRotMatrix.a2) / s; |
102 | z = (pRotMatrix.a3 + pRotMatrix.c1) / s; |
103 | w = (pRotMatrix.c2 - pRotMatrix.b3) / s; |
104 | } |
105 | else if( pRotMatrix.b2 > pRotMatrix.c3) |
106 | { |
107 | // Column 1: |
108 | TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.b2 - pRotMatrix.a1 - pRotMatrix.c3) * static_cast<TReal>(2.0); |
109 | x = (pRotMatrix.b1 + pRotMatrix.a2) / s; |
110 | y = static_cast<TReal>(0.25) * s; |
111 | z = (pRotMatrix.c2 + pRotMatrix.b3) / s; |
112 | w = (pRotMatrix.a3 - pRotMatrix.c1) / s; |
113 | } else |
114 | { |
115 | // Column 2: |
116 | TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.c3 - pRotMatrix.a1 - pRotMatrix.b2) * static_cast<TReal>(2.0); |
117 | x = (pRotMatrix.a3 + pRotMatrix.c1) / s; |
118 | y = (pRotMatrix.c2 + pRotMatrix.b3) / s; |
119 | z = static_cast<TReal>(0.25) * s; |
120 | w = (pRotMatrix.b1 - pRotMatrix.a2) / s; |
121 | } |
122 | } |
123 | |
124 | // --------------------------------------------------------------------------- |
125 | // Construction from euler angles |
126 | template<typename TReal> |
127 | inline aiQuaterniont<TReal>::aiQuaterniont( TReal fPitch, TReal fYaw, TReal fRoll ) |
128 | { |
129 | const TReal fSinPitch(std::sin(fPitch*static_cast<TReal>(0.5))); |
130 | const TReal fCosPitch(std::cos(fPitch*static_cast<TReal>(0.5))); |
131 | const TReal fSinYaw(std::sin(fYaw*static_cast<TReal>(0.5))); |
132 | const TReal fCosYaw(std::cos(fYaw*static_cast<TReal>(0.5))); |
133 | const TReal fSinRoll(std::sin(fRoll*static_cast<TReal>(0.5))); |
134 | const TReal fCosRoll(std::cos(fRoll*static_cast<TReal>(0.5))); |
135 | const TReal fCosPitchCosYaw(fCosPitch*fCosYaw); |
136 | const TReal fSinPitchSinYaw(fSinPitch*fSinYaw); |
137 | x = fSinRoll * fCosPitchCosYaw - fCosRoll * fSinPitchSinYaw; |
138 | y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw; |
139 | z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw; |
140 | w = fCosRoll * fCosPitchCosYaw + fSinRoll * fSinPitchSinYaw; |
141 | } |
142 | |
143 | // --------------------------------------------------------------------------- |
144 | // Returns a matrix representation of the quaternion |
145 | template<typename TReal> |
146 | inline aiMatrix3x3t<TReal> aiQuaterniont<TReal>::GetMatrix() const |
147 | { |
148 | aiMatrix3x3t<TReal> resMatrix; |
149 | resMatrix.a1 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (y * y + z * z); |
150 | resMatrix.a2 = static_cast<TReal>(2.0) * (x * y - z * w); |
151 | resMatrix.a3 = static_cast<TReal>(2.0) * (x * z + y * w); |
152 | resMatrix.b1 = static_cast<TReal>(2.0) * (x * y + z * w); |
153 | resMatrix.b2 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + z * z); |
154 | resMatrix.b3 = static_cast<TReal>(2.0) * (y * z - x * w); |
155 | resMatrix.c1 = static_cast<TReal>(2.0) * (x * z - y * w); |
156 | resMatrix.c2 = static_cast<TReal>(2.0) * (y * z + x * w); |
157 | resMatrix.c3 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + y * y); |
158 | |
159 | return resMatrix; |
160 | } |
161 | |
162 | // --------------------------------------------------------------------------- |
163 | // Construction from an axis-angle pair |
164 | template<typename TReal> |
165 | inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> axis, TReal angle) |
166 | { |
167 | axis.Normalize(); |
168 | |
169 | const TReal sin_a = std::sin( angle / 2 ); |
170 | const TReal cos_a = std::cos( angle / 2 ); |
171 | x = axis.x * sin_a; |
172 | y = axis.y * sin_a; |
173 | z = axis.z * sin_a; |
174 | w = cos_a; |
175 | } |
176 | // --------------------------------------------------------------------------- |
177 | // Construction from am existing, normalized quaternion |
178 | template<typename TReal> |
179 | inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> normalized) |
180 | { |
181 | x = normalized.x; |
182 | y = normalized.y; |
183 | z = normalized.z; |
184 | |
185 | const TReal t = static_cast<TReal>(1.0) - (x*x) - (y*y) - (z*z); |
186 | |
187 | if (t < static_cast<TReal>(0.0)) { |
188 | w = static_cast<TReal>(0.0); |
189 | } |
190 | else w = std::sqrt (t); |
191 | } |
192 | |
193 | // --------------------------------------------------------------------------- |
194 | // Performs a spherical interpolation between two quaternions |
195 | // Implementation adopted from the gmtl project. All others I found on the net fail in some cases. |
196 | // Congrats, gmtl! |
197 | template<typename TReal> |
198 | inline void aiQuaterniont<TReal>::Interpolate( aiQuaterniont& pOut, const aiQuaterniont& pStart, const aiQuaterniont& pEnd, TReal pFactor) |
199 | { |
200 | // calc cosine theta |
201 | TReal cosom = pStart.x * pEnd.x + pStart.y * pEnd.y + pStart.z * pEnd.z + pStart.w * pEnd.w; |
202 | |
203 | // adjust signs (if necessary) |
204 | aiQuaterniont end = pEnd; |
205 | if( cosom < static_cast<TReal>(0.0)) |
206 | { |
207 | cosom = -cosom; |
208 | end.x = -end.x; // Reverse all signs |
209 | end.y = -end.y; |
210 | end.z = -end.z; |
211 | end.w = -end.w; |
212 | } |
213 | |
214 | // Calculate coefficients |
215 | TReal sclp, sclq; |
216 | if( (static_cast<TReal>(1.0) - cosom) > static_cast<TReal>(0.0001)) // 0.0001 -> some epsillon |
217 | { |
218 | // Standard case (slerp) |
219 | TReal omega, sinom; |
220 | omega = std::acos( cosom); // extract theta from dot product's cos theta |
221 | sinom = std::sin( omega); |
222 | sclp = std::sin( (static_cast<TReal>(1.0) - pFactor) * omega) / sinom; |
223 | sclq = std::sin( pFactor * omega) / sinom; |
224 | } else |
225 | { |
226 | // Very close, do linear interp (because it's faster) |
227 | sclp = static_cast<TReal>(1.0) - pFactor; |
228 | sclq = pFactor; |
229 | } |
230 | |
231 | pOut.x = sclp * pStart.x + sclq * end.x; |
232 | pOut.y = sclp * pStart.y + sclq * end.y; |
233 | pOut.z = sclp * pStart.z + sclq * end.z; |
234 | pOut.w = sclp * pStart.w + sclq * end.w; |
235 | } |
236 | |
237 | // --------------------------------------------------------------------------- |
238 | template<typename TReal> |
239 | inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Normalize() |
240 | { |
241 | // compute the magnitude and divide through it |
242 | const TReal mag = std::sqrt(x*x + y*y + z*z + w*w); |
243 | if (mag) |
244 | { |
245 | const TReal invMag = static_cast<TReal>(1.0)/mag; |
246 | x *= invMag; |
247 | y *= invMag; |
248 | z *= invMag; |
249 | w *= invMag; |
250 | } |
251 | return *this; |
252 | } |
253 | |
254 | // --------------------------------------------------------------------------- |
255 | template<typename TReal> |
256 | inline aiQuaterniont<TReal> aiQuaterniont<TReal>::operator* (const aiQuaterniont& t) const |
257 | { |
258 | return aiQuaterniont(w*t.w - x*t.x - y*t.y - z*t.z, |
259 | w*t.x + x*t.w + y*t.z - z*t.y, |
260 | w*t.y + y*t.w + z*t.x - x*t.z, |
261 | w*t.z + z*t.w + x*t.y - y*t.x); |
262 | } |
263 | |
264 | // --------------------------------------------------------------------------- |
265 | template<typename TReal> |
266 | inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Conjugate () |
267 | { |
268 | x = -x; |
269 | y = -y; |
270 | z = -z; |
271 | return *this; |
272 | } |
273 | |
274 | // --------------------------------------------------------------------------- |
275 | template<typename TReal> |
276 | inline aiVector3t<TReal> aiQuaterniont<TReal>::Rotate (const aiVector3t<TReal>& v) |
277 | { |
278 | aiQuaterniont q2(0.f,v.x,v.y,v.z), q = *this, qinv = q; |
279 | qinv.Conjugate(); |
280 | |
281 | q = q*q2*qinv; |
282 | return aiVector3t<TReal>(q.x,q.y,q.z); |
283 | } |
284 | |
285 | #endif |
286 | #endif // AI_QUATERNION_INL_INC |
287 | |