vrshoot

annotate libs/assimp/assimp/quaternion.inl @ 0:b2f14e535253

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