Warning, /include/assimp/quaternion.inl is written in an unsupported language. File is not indexed.
0001 /*
0002 ---------------------------------------------------------------------------
0003 Open Asset Import Library (assimp)
0004 ---------------------------------------------------------------------------
0005
0006 Copyright (c) 2006-2024, assimp team
0007
0008
0009
0010 All rights reserved.
0011
0012 Redistribution and use of this software in source and binary forms,
0013 with or without modification, are permitted provided that the following
0014 conditions are met:
0015
0016 * Redistributions of source code must retain the above
0017 copyright notice, this list of conditions and the
0018 following disclaimer.
0019
0020 * Redistributions in binary form must reproduce the above
0021 copyright notice, this list of conditions and the
0022 following disclaimer in the documentation and/or other
0023 materials provided with the distribution.
0024
0025 * Neither the name of the assimp team, nor the names of its
0026 contributors may be used to endorse or promote products
0027 derived from this software without specific prior
0028 written permission of the assimp team.
0029
0030 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
0031 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
0032 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
0033 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
0034 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
0035 SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
0036 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
0037 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
0038 THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
0039 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
0040 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
0041 ---------------------------------------------------------------------------
0042 */
0043
0044 /** @file quaternion.inl
0045 * @brief Inline implementation of aiQuaterniont<TReal> operators
0046 */
0047 #pragma once
0048 #ifndef AI_QUATERNION_INL_INC
0049 #define AI_QUATERNION_INL_INC
0050
0051 #ifdef __GNUC__
0052 # pragma GCC system_header
0053 #endif
0054
0055 #ifdef __cplusplus
0056 #include <assimp/quaternion.h>
0057
0058 #include <cmath>
0059
0060 // ------------------------------------------------------------------------------------------------
0061 /** Transformation of a quaternion by a 4x4 matrix */
0062 template <typename TReal>
0063 AI_FORCE_INLINE
0064 aiQuaterniont<TReal> operator * (const aiMatrix4x4t<TReal>& pMatrix, const aiQuaterniont<TReal>& pQuaternion) {
0065 aiQuaterniont<TReal> res;
0066 res.x = pMatrix.a1 * pQuaternion.x + pMatrix.a2 * pQuaternion.y + pMatrix.a3 * pQuaternion.z + pMatrix.a4 * pQuaternion.w;
0067 res.y = pMatrix.b1 * pQuaternion.x + pMatrix.b2 * pQuaternion.y + pMatrix.b3 * pQuaternion.z + pMatrix.b4 * pQuaternion.w;
0068 res.z = pMatrix.c1 * pQuaternion.x + pMatrix.c2 * pQuaternion.y + pMatrix.c3 * pQuaternion.z + pMatrix.c4 * pQuaternion.w;
0069 res.w = pMatrix.d1 * pQuaternion.x + pMatrix.d2 * pQuaternion.y + pMatrix.d3 * pQuaternion.z + pMatrix.d4 * pQuaternion.w;
0070 return res;
0071 }
0072 // ---------------------------------------------------------------------------
0073 template<typename TReal>
0074 bool aiQuaterniont<TReal>::operator== (const aiQuaterniont& o) const
0075 {
0076 return x == o.x && y == o.y && z == o.z && w == o.w;
0077 }
0078
0079 // ---------------------------------------------------------------------------
0080 template<typename TReal>
0081 bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const
0082 {
0083 return !(*this == o);
0084 }
0085
0086 // ------------------------------------------------------------------------------------------------
0087 template <typename TReal>
0088 AI_FORCE_INLINE
0089 aiQuaterniont<TReal>& aiQuaterniont<TReal>::operator *= (const aiMatrix4x4t<TReal>& mat){
0090 return (*this = mat * (*this));
0091 }
0092 // ------------------------------------------------------------------------------------------------
0093
0094 // ---------------------------------------------------------------------------
0095 template<typename TReal>
0096 inline bool aiQuaterniont<TReal>::Equal(const aiQuaterniont& o, TReal epsilon) const {
0097 return
0098 std::abs(x - o.x) <= epsilon &&
0099 std::abs(y - o.y) <= epsilon &&
0100 std::abs(z - o.z) <= epsilon &&
0101 std::abs(w - o.w) <= epsilon;
0102 }
0103
0104 // ---------------------------------------------------------------------------
0105 // Constructs a quaternion from a rotation matrix
0106 template<typename TReal>
0107 inline aiQuaterniont<TReal>::aiQuaterniont( const aiMatrix3x3t<TReal> &pRotMatrix)
0108 {
0109 TReal t = pRotMatrix.a1 + pRotMatrix.b2 + pRotMatrix.c3;
0110
0111 // large enough
0112 if( t > static_cast<TReal>(0))
0113 {
0114 TReal s = std::sqrt(1 + t) * static_cast<TReal>(2.0);
0115 x = (pRotMatrix.c2 - pRotMatrix.b3) / s;
0116 y = (pRotMatrix.a3 - pRotMatrix.c1) / s;
0117 z = (pRotMatrix.b1 - pRotMatrix.a2) / s;
0118 w = static_cast<TReal>(0.25) * s;
0119 } // else we have to check several cases
0120 else if( pRotMatrix.a1 > pRotMatrix.b2 && pRotMatrix.a1 > pRotMatrix.c3 )
0121 {
0122 // Column 0:
0123 TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.a1 - pRotMatrix.b2 - pRotMatrix.c3) * static_cast<TReal>(2.0);
0124 x = static_cast<TReal>(0.25) * s;
0125 y = (pRotMatrix.b1 + pRotMatrix.a2) / s;
0126 z = (pRotMatrix.a3 + pRotMatrix.c1) / s;
0127 w = (pRotMatrix.c2 - pRotMatrix.b3) / s;
0128 }
0129 else if( pRotMatrix.b2 > pRotMatrix.c3)
0130 {
0131 // Column 1:
0132 TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.b2 - pRotMatrix.a1 - pRotMatrix.c3) * static_cast<TReal>(2.0);
0133 x = (pRotMatrix.b1 + pRotMatrix.a2) / s;
0134 y = static_cast<TReal>(0.25) * s;
0135 z = (pRotMatrix.c2 + pRotMatrix.b3) / s;
0136 w = (pRotMatrix.a3 - pRotMatrix.c1) / s;
0137 } else
0138 {
0139 // Column 2:
0140 TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.c3 - pRotMatrix.a1 - pRotMatrix.b2) * static_cast<TReal>(2.0);
0141 x = (pRotMatrix.a3 + pRotMatrix.c1) / s;
0142 y = (pRotMatrix.c2 + pRotMatrix.b3) / s;
0143 z = static_cast<TReal>(0.25) * s;
0144 w = (pRotMatrix.b1 - pRotMatrix.a2) / s;
0145 }
0146 }
0147
0148 // ---------------------------------------------------------------------------
0149 // Construction from euler angles
0150 template<typename TReal>
0151 inline aiQuaterniont<TReal>::aiQuaterniont( TReal fPitch, TReal fYaw, TReal fRoll )
0152 {
0153 const TReal fSinPitch(std::sin(fPitch*static_cast<TReal>(0.5)));
0154 const TReal fCosPitch(std::cos(fPitch*static_cast<TReal>(0.5)));
0155 const TReal fSinYaw(std::sin(fYaw*static_cast<TReal>(0.5)));
0156 const TReal fCosYaw(std::cos(fYaw*static_cast<TReal>(0.5)));
0157 const TReal fSinRoll(std::sin(fRoll*static_cast<TReal>(0.5)));
0158 const TReal fCosRoll(std::cos(fRoll*static_cast<TReal>(0.5)));
0159 const TReal fCosPitchCosYaw(fCosPitch*fCosYaw);
0160 const TReal fSinPitchSinYaw(fSinPitch*fSinYaw);
0161 x = fSinRoll * fCosPitchCosYaw - fCosRoll * fSinPitchSinYaw;
0162 y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
0163 z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
0164 w = fCosRoll * fCosPitchCosYaw + fSinRoll * fSinPitchSinYaw;
0165 }
0166
0167 // ---------------------------------------------------------------------------
0168 // Returns a matrix representation of the quaternion
0169 template<typename TReal>
0170 inline aiMatrix3x3t<TReal> aiQuaterniont<TReal>::GetMatrix() const
0171 {
0172 aiMatrix3x3t<TReal> resMatrix;
0173 resMatrix.a1 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (y * y + z * z);
0174 resMatrix.a2 = static_cast<TReal>(2.0) * (x * y - z * w);
0175 resMatrix.a3 = static_cast<TReal>(2.0) * (x * z + y * w);
0176 resMatrix.b1 = static_cast<TReal>(2.0) * (x * y + z * w);
0177 resMatrix.b2 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + z * z);
0178 resMatrix.b3 = static_cast<TReal>(2.0) * (y * z - x * w);
0179 resMatrix.c1 = static_cast<TReal>(2.0) * (x * z - y * w);
0180 resMatrix.c2 = static_cast<TReal>(2.0) * (y * z + x * w);
0181 resMatrix.c3 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + y * y);
0182
0183 return resMatrix;
0184 }
0185
0186 // ---------------------------------------------------------------------------
0187 // Construction from an axis-angle pair
0188 template<typename TReal>
0189 inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> axis, TReal angle)
0190 {
0191 axis.Normalize();
0192
0193 const TReal sin_a = std::sin( angle / 2 );
0194 const TReal cos_a = std::cos( angle / 2 );
0195 x = axis.x * sin_a;
0196 y = axis.y * sin_a;
0197 z = axis.z * sin_a;
0198 w = cos_a;
0199 }
0200 // ---------------------------------------------------------------------------
0201 // Construction from am existing, normalized quaternion
0202 template<typename TReal>
0203 inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> normalized)
0204 {
0205 x = normalized.x;
0206 y = normalized.y;
0207 z = normalized.z;
0208
0209 const TReal t = static_cast<TReal>(1.0) - (x*x) - (y*y) - (z*z);
0210
0211 if (t < static_cast<TReal>(0.0)) {
0212 w = static_cast<TReal>(0.0);
0213 }
0214 else w = std::sqrt (t);
0215 }
0216
0217 // ---------------------------------------------------------------------------
0218 // Performs a spherical interpolation between two quaternions
0219 // Implementation adopted from the gmtl project. All others I found on the net fail in some cases.
0220 // Congrats, gmtl!
0221 template<typename TReal>
0222 inline void aiQuaterniont<TReal>::Interpolate( aiQuaterniont& pOut, const aiQuaterniont& pStart, const aiQuaterniont& pEnd, TReal pFactor)
0223 {
0224 // calc cosine theta
0225 TReal cosom = pStart.x * pEnd.x + pStart.y * pEnd.y + pStart.z * pEnd.z + pStart.w * pEnd.w;
0226
0227 // adjust signs (if necessary)
0228 aiQuaterniont end = pEnd;
0229 if( cosom < static_cast<TReal>(0.0))
0230 {
0231 cosom = -cosom;
0232 end.x = -end.x; // Reverse all signs
0233 end.y = -end.y;
0234 end.z = -end.z;
0235 end.w = -end.w;
0236 }
0237
0238 // Calculate coefficients
0239 TReal sclp, sclq;
0240
0241 if ((static_cast<TReal>(1.0) - cosom) > ai_epsilon) // 0.0001 -> some epsillon
0242 {
0243 // Standard case (slerp)
0244 TReal omega, sinom;
0245 omega = std::acos( cosom); // extract theta from dot product's cos theta
0246 sinom = std::sin( omega);
0247 sclp = std::sin( (static_cast<TReal>(1.0) - pFactor) * omega) / sinom;
0248 sclq = std::sin( pFactor * omega) / sinom;
0249 } else
0250 {
0251 // Very close, do linear interp (because it's faster)
0252 sclp = static_cast<TReal>(1.0) - pFactor;
0253 sclq = pFactor;
0254 }
0255
0256 pOut.x = sclp * pStart.x + sclq * end.x;
0257 pOut.y = sclp * pStart.y + sclq * end.y;
0258 pOut.z = sclp * pStart.z + sclq * end.z;
0259 pOut.w = sclp * pStart.w + sclq * end.w;
0260 }
0261
0262 // ---------------------------------------------------------------------------
0263 template<typename TReal>
0264 inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Normalize()
0265 {
0266 // compute the magnitude and divide through it
0267 const TReal mag = std::sqrt(x*x + y*y + z*z + w*w);
0268 if (mag)
0269 {
0270 const TReal invMag = static_cast<TReal>(1.0)/mag;
0271 x *= invMag;
0272 y *= invMag;
0273 z *= invMag;
0274 w *= invMag;
0275 }
0276 return *this;
0277 }
0278
0279 // ---------------------------------------------------------------------------
0280 template<typename TReal>
0281 inline aiQuaterniont<TReal> aiQuaterniont<TReal>::operator* (const aiQuaterniont& t) const
0282 {
0283 return aiQuaterniont(w*t.w - x*t.x - y*t.y - z*t.z,
0284 w*t.x + x*t.w + y*t.z - z*t.y,
0285 w*t.y + y*t.w + z*t.x - x*t.z,
0286 w*t.z + z*t.w + x*t.y - y*t.x);
0287 }
0288
0289 // ---------------------------------------------------------------------------
0290 template<typename TReal>
0291 inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Conjugate ()
0292 {
0293 x = -x;
0294 y = -y;
0295 z = -z;
0296 return *this;
0297 }
0298
0299 // ---------------------------------------------------------------------------
0300 template<typename TReal>
0301 inline aiVector3t<TReal> aiQuaterniont<TReal>::Rotate (const aiVector3t<TReal>& v) const
0302 {
0303 aiQuaterniont q2(0.f,v.x,v.y,v.z), q = *this, qinv = q;
0304 qinv.Conjugate();
0305
0306 q = q*q2*qinv;
0307 return aiVector3t<TReal>(q.x,q.y,q.z);
0308 }
0309
0310 #endif
0311 #endif // AI_QUATERNION_INL_INC
