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