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