vrshoot
diff 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 diff
1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000 1.2 +++ b/libs/assimp/assimp/quaternion.inl Sat Feb 01 19:58:19 2014 +0200 1.3 @@ -0,0 +1,274 @@ 1.4 +/* 1.5 +--------------------------------------------------------------------------- 1.6 +Open Asset Import Library (assimp) 1.7 +--------------------------------------------------------------------------- 1.8 + 1.9 +Copyright (c) 2006-2012, assimp team 1.10 + 1.11 +All rights reserved. 1.12 + 1.13 +Redistribution and use of this software in source and binary forms, 1.14 +with or without modification, are permitted provided that the following 1.15 +conditions are met: 1.16 + 1.17 +* Redistributions of source code must retain the above 1.18 + copyright notice, this list of conditions and the 1.19 + following disclaimer. 1.20 + 1.21 +* Redistributions in binary form must reproduce the above 1.22 + copyright notice, this list of conditions and the 1.23 + following disclaimer in the documentation and/or other 1.24 + materials provided with the distribution. 1.25 + 1.26 +* Neither the name of the assimp team, nor the names of its 1.27 + contributors may be used to endorse or promote products 1.28 + derived from this software without specific prior 1.29 + written permission of the assimp team. 1.30 + 1.31 +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 1.32 +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 1.33 +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 1.34 +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 1.35 +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 1.36 +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 1.37 +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 1.38 +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 1.39 +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 1.40 +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 1.41 +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 1.42 +--------------------------------------------------------------------------- 1.43 +*/ 1.44 + 1.45 +/** @file aiQuaterniont.inl 1.46 + * @brief Inline implementation of aiQuaterniont<TReal> operators 1.47 + */ 1.48 +#ifndef AI_QUATERNION_INL_INC 1.49 +#define AI_QUATERNION_INL_INC 1.50 + 1.51 +#ifdef __cplusplus 1.52 +#include "quaternion.h" 1.53 + 1.54 +// --------------------------------------------------------------------------- 1.55 +template<typename TReal> 1.56 +bool aiQuaterniont<TReal>::operator== (const aiQuaterniont& o) const 1.57 +{ 1.58 + return x == o.x && y == o.y && z == o.z && w == o.w; 1.59 +} 1.60 + 1.61 +// --------------------------------------------------------------------------- 1.62 +template<typename TReal> 1.63 +bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const 1.64 +{ 1.65 + return !(*this == o); 1.66 +} 1.67 + 1.68 + 1.69 + 1.70 +// --------------------------------------------------------------------------- 1.71 +// Constructs a quaternion from a rotation matrix 1.72 +template<typename TReal> 1.73 +inline aiQuaterniont<TReal>::aiQuaterniont( const aiMatrix3x3t<TReal> &pRotMatrix) 1.74 +{ 1.75 + TReal t = pRotMatrix.a1 + pRotMatrix.b2 + pRotMatrix.c3; 1.76 + 1.77 + // large enough 1.78 + if( t > static_cast<TReal>(0)) 1.79 + { 1.80 + TReal s = sqrt(1 + t) * static_cast<TReal>(2.0); 1.81 + x = (pRotMatrix.c2 - pRotMatrix.b3) / s; 1.82 + y = (pRotMatrix.a3 - pRotMatrix.c1) / s; 1.83 + z = (pRotMatrix.b1 - pRotMatrix.a2) / s; 1.84 + w = static_cast<TReal>(0.25) * s; 1.85 + } // else we have to check several cases 1.86 + else if( pRotMatrix.a1 > pRotMatrix.b2 && pRotMatrix.a1 > pRotMatrix.c3 ) 1.87 + { 1.88 + // Column 0: 1.89 + TReal s = sqrt( static_cast<TReal>(1.0) + pRotMatrix.a1 - pRotMatrix.b2 - pRotMatrix.c3) * static_cast<TReal>(2.0); 1.90 + x = static_cast<TReal>(0.25) * s; 1.91 + y = (pRotMatrix.b1 + pRotMatrix.a2) / s; 1.92 + z = (pRotMatrix.a3 + pRotMatrix.c1) / s; 1.93 + w = (pRotMatrix.c2 - pRotMatrix.b3) / s; 1.94 + } 1.95 + else if( pRotMatrix.b2 > pRotMatrix.c3) 1.96 + { 1.97 + // Column 1: 1.98 + TReal s = sqrt( static_cast<TReal>(1.0) + pRotMatrix.b2 - pRotMatrix.a1 - pRotMatrix.c3) * static_cast<TReal>(2.0); 1.99 + x = (pRotMatrix.b1 + pRotMatrix.a2) / s; 1.100 + y = static_cast<TReal>(0.25) * s; 1.101 + z = (pRotMatrix.c2 + pRotMatrix.b3) / s; 1.102 + w = (pRotMatrix.a3 - pRotMatrix.c1) / s; 1.103 + } else 1.104 + { 1.105 + // Column 2: 1.106 + TReal s = sqrt( static_cast<TReal>(1.0) + pRotMatrix.c3 - pRotMatrix.a1 - pRotMatrix.b2) * static_cast<TReal>(2.0); 1.107 + x = (pRotMatrix.a3 + pRotMatrix.c1) / s; 1.108 + y = (pRotMatrix.c2 + pRotMatrix.b3) / s; 1.109 + z = static_cast<TReal>(0.25) * s; 1.110 + w = (pRotMatrix.b1 - pRotMatrix.a2) / s; 1.111 + } 1.112 +} 1.113 + 1.114 +// --------------------------------------------------------------------------- 1.115 +// Construction from euler angles 1.116 +template<typename TReal> 1.117 +inline aiQuaterniont<TReal>::aiQuaterniont( TReal fPitch, TReal fYaw, TReal fRoll ) 1.118 +{ 1.119 + const TReal fSinPitch(sin(fPitch*static_cast<TReal>(0.5))); 1.120 + const TReal fCosPitch(cos(fPitch*static_cast<TReal>(0.5))); 1.121 + const TReal fSinYaw(sin(fYaw*static_cast<TReal>(0.5))); 1.122 + const TReal fCosYaw(cos(fYaw*static_cast<TReal>(0.5))); 1.123 + const TReal fSinRoll(sin(fRoll*static_cast<TReal>(0.5))); 1.124 + const TReal fCosRoll(cos(fRoll*static_cast<TReal>(0.5))); 1.125 + const TReal fCosPitchCosYaw(fCosPitch*fCosYaw); 1.126 + const TReal fSinPitchSinYaw(fSinPitch*fSinYaw); 1.127 + x = fSinRoll * fCosPitchCosYaw - fCosRoll * fSinPitchSinYaw; 1.128 + y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw; 1.129 + z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw; 1.130 + w = fCosRoll * fCosPitchCosYaw + fSinRoll * fSinPitchSinYaw; 1.131 +} 1.132 + 1.133 +// --------------------------------------------------------------------------- 1.134 +// Returns a matrix representation of the quaternion 1.135 +template<typename TReal> 1.136 +inline aiMatrix3x3t<TReal> aiQuaterniont<TReal>::GetMatrix() const 1.137 +{ 1.138 + aiMatrix3x3t<TReal> resMatrix; 1.139 + resMatrix.a1 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (y * y + z * z); 1.140 + resMatrix.a2 = static_cast<TReal>(2.0) * (x * y - z * w); 1.141 + resMatrix.a3 = static_cast<TReal>(2.0) * (x * z + y * w); 1.142 + resMatrix.b1 = static_cast<TReal>(2.0) * (x * y + z * w); 1.143 + resMatrix.b2 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + z * z); 1.144 + resMatrix.b3 = static_cast<TReal>(2.0) * (y * z - x * w); 1.145 + resMatrix.c1 = static_cast<TReal>(2.0) * (x * z - y * w); 1.146 + resMatrix.c2 = static_cast<TReal>(2.0) * (y * z + x * w); 1.147 + resMatrix.c3 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + y * y); 1.148 + 1.149 + return resMatrix; 1.150 +} 1.151 + 1.152 +// --------------------------------------------------------------------------- 1.153 +// Construction from an axis-angle pair 1.154 +template<typename TReal> 1.155 +inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> axis, TReal angle) 1.156 +{ 1.157 + axis.Normalize(); 1.158 + 1.159 + const TReal sin_a = sin( angle / 2 ); 1.160 + const TReal cos_a = cos( angle / 2 ); 1.161 + x = axis.x * sin_a; 1.162 + y = axis.y * sin_a; 1.163 + z = axis.z * sin_a; 1.164 + w = cos_a; 1.165 +} 1.166 +// --------------------------------------------------------------------------- 1.167 +// Construction from am existing, normalized quaternion 1.168 +template<typename TReal> 1.169 +inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> normalized) 1.170 +{ 1.171 + x = normalized.x; 1.172 + y = normalized.y; 1.173 + z = normalized.z; 1.174 + 1.175 + const TReal t = static_cast<TReal>(1.0) - (x*x) - (y*y) - (z*z); 1.176 + 1.177 + if (t < static_cast<TReal>(0.0)) { 1.178 + w = static_cast<TReal>(0.0); 1.179 + } 1.180 + else w = sqrt (t); 1.181 +} 1.182 + 1.183 +// --------------------------------------------------------------------------- 1.184 +// Performs a spherical interpolation between two quaternions 1.185 +// Implementation adopted from the gmtl project. All others I found on the net fail in some cases. 1.186 +// Congrats, gmtl! 1.187 +template<typename TReal> 1.188 +inline void aiQuaterniont<TReal>::Interpolate( aiQuaterniont& pOut, const aiQuaterniont& pStart, const aiQuaterniont& pEnd, TReal pFactor) 1.189 +{ 1.190 + // calc cosine theta 1.191 + TReal cosom = pStart.x * pEnd.x + pStart.y * pEnd.y + pStart.z * pEnd.z + pStart.w * pEnd.w; 1.192 + 1.193 + // adjust signs (if necessary) 1.194 + aiQuaterniont end = pEnd; 1.195 + if( cosom < static_cast<TReal>(0.0)) 1.196 + { 1.197 + cosom = -cosom; 1.198 + end.x = -end.x; // Reverse all signs 1.199 + end.y = -end.y; 1.200 + end.z = -end.z; 1.201 + end.w = -end.w; 1.202 + } 1.203 + 1.204 + // Calculate coefficients 1.205 + TReal sclp, sclq; 1.206 + if( (static_cast<TReal>(1.0) - cosom) > static_cast<TReal>(0.0001)) // 0.0001 -> some epsillon 1.207 + { 1.208 + // Standard case (slerp) 1.209 + TReal omega, sinom; 1.210 + omega = acos( cosom); // extract theta from dot product's cos theta 1.211 + sinom = sin( omega); 1.212 + sclp = sin( (static_cast<TReal>(1.0) - pFactor) * omega) / sinom; 1.213 + sclq = sin( pFactor * omega) / sinom; 1.214 + } else 1.215 + { 1.216 + // Very close, do linear interp (because it's faster) 1.217 + sclp = static_cast<TReal>(1.0) - pFactor; 1.218 + sclq = pFactor; 1.219 + } 1.220 + 1.221 + pOut.x = sclp * pStart.x + sclq * end.x; 1.222 + pOut.y = sclp * pStart.y + sclq * end.y; 1.223 + pOut.z = sclp * pStart.z + sclq * end.z; 1.224 + pOut.w = sclp * pStart.w + sclq * end.w; 1.225 +} 1.226 + 1.227 +// --------------------------------------------------------------------------- 1.228 +template<typename TReal> 1.229 +inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Normalize() 1.230 +{ 1.231 + // compute the magnitude and divide through it 1.232 + const TReal mag = sqrt(x*x + y*y + z*z + w*w); 1.233 + if (mag) 1.234 + { 1.235 + const TReal invMag = static_cast<TReal>(1.0)/mag; 1.236 + x *= invMag; 1.237 + y *= invMag; 1.238 + z *= invMag; 1.239 + w *= invMag; 1.240 + } 1.241 + return *this; 1.242 +} 1.243 + 1.244 +// --------------------------------------------------------------------------- 1.245 +template<typename TReal> 1.246 +inline aiQuaterniont<TReal> aiQuaterniont<TReal>::operator* (const aiQuaterniont& t) const 1.247 +{ 1.248 + return aiQuaterniont(w*t.w - x*t.x - y*t.y - z*t.z, 1.249 + w*t.x + x*t.w + y*t.z - z*t.y, 1.250 + w*t.y + y*t.w + z*t.x - x*t.z, 1.251 + w*t.z + z*t.w + x*t.y - y*t.x); 1.252 +} 1.253 + 1.254 +// --------------------------------------------------------------------------- 1.255 +template<typename TReal> 1.256 +inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Conjugate () 1.257 +{ 1.258 + x = -x; 1.259 + y = -y; 1.260 + z = -z; 1.261 + return *this; 1.262 +} 1.263 + 1.264 +// --------------------------------------------------------------------------- 1.265 +template<typename TReal> 1.266 +inline aiVector3t<TReal> aiQuaterniont<TReal>::Rotate (const aiVector3t<TReal>& v) 1.267 +{ 1.268 + aiQuaterniont q2(0.f,v.x,v.y,v.z), q = *this, qinv = q; 1.269 + q.Conjugate(); 1.270 + 1.271 + q = q*q2*qinv; 1.272 + return aiVector3t<TReal>(q.x,q.y,q.z); 1.273 + 1.274 +} 1.275 + 1.276 +#endif 1.277 +#endif