miniassimp
view include/miniassimp/quaternion.inl @ 0:879c81d94345
initial commit
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Mon, 28 Jan 2019 18:19:26 +0200 |
| parents | |
| children |
line source
1 /*
2 ---------------------------------------------------------------------------
3 Open Asset Import Library (assimp)
4 ---------------------------------------------------------------------------
6 Copyright (c) 2006-2018, assimp team
10 All rights reserved.
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:
16 * Redistributions of source code must retain the above
17 copyright notice, this list of conditions and the
18 following disclaimer.
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.
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.
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 */
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
51 #ifdef __cplusplus
52 #include "quaternion.h"
54 #include <cmath>
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 }
63 // ---------------------------------------------------------------------------
64 template<typename TReal>
65 bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const
66 {
67 return !(*this == o);
68 }
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 }
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;
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 }
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 }
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);
159 return resMatrix;
160 }
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();
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;
185 const TReal t = static_cast<TReal>(1.0) - (x*x) - (y*y) - (z*z);
187 if (t < static_cast<TReal>(0.0)) {
188 w = static_cast<TReal>(0.0);
189 }
190 else w = std::sqrt (t);
191 }
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;
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 }
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 }
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 }
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 }
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 }
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 }
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();
281 q = q*q2*qinv;
282 return aiVector3t<TReal>(q.x,q.y,q.z);
283 }
285 #endif
286 #endif // AI_QUATERNION_INL_INC