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