1/*
2---------------------------------------------------------------------------
3Open Asset Import Library (assimp)
4---------------------------------------------------------------------------
5
6Copyright (c) 2006-2019, assimp team
7
8
9
10All rights reserved.
11
12Redistribution and use of this software in source and binary forms,
13with or without modification, are permitted provided that the following
14conditions 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
30THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
31"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
32LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
33A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
34OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
35SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
36LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
37DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
38THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
39(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
40OF 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// ---------------------------------------------------------------------------
57template<typename TReal>
58bool 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// ---------------------------------------------------------------------------
64template<typename TReal>
65bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const
66{
67 return !(*this == o);
68}
69
70// ---------------------------------------------------------------------------
71template<typename TReal>
72inline 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
82template<typename TReal>
83inline 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
126template<typename TReal>
127inline 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
145template<typename TReal>
146inline 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
164template<typename TReal>
165inline 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
178template<typename TReal>
179inline 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!
197template<typename TReal>
198inline 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// ---------------------------------------------------------------------------
238template<typename TReal>
239inline 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// ---------------------------------------------------------------------------
255template<typename TReal>
256inline 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// ---------------------------------------------------------------------------
265template<typename TReal>
266inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Conjugate ()
267{
268 x = -x;
269 y = -y;
270 z = -z;
271 return *this;
272}
273
274// ---------------------------------------------------------------------------
275template<typename TReal>
276inline 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

source code of qt3d/src/3rdparty/assimp/src/include/assimp/quaternion.inl