mQuat.cpp
Public Defines
define
idx(r, c) (r*4 + c)
Detailed Description
Public Defines
idx(r, c) (r*4 + c)
1 2//----------------------------------------------------------------------------- 3// Copyright (c) 2012 GarageGames, LLC 4// 5// Permission is hereby granted, free of charge, to any person obtaining a copy 6// of this software and associated documentation files (the "Software"), to 7// deal in the Software without restriction, including without limitation the 8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9// sell copies of the Software, and to permit persons to whom the Software is 10// furnished to do so, subject to the following conditions: 11// 12// The above copyright notice and this permission notice shall be included in 13// all copies or substantial portions of the Software. 14// 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21// IN THE SOFTWARE. 22//----------------------------------------------------------------------------- 23 24#include "platform/platform.h" 25#include "math/mQuat.h" 26 27#include "math/mAngAxis.h" 28#include "math/mMatrix.h" 29#include "platform/profiler.h" 30 31 32const QuatF QuatF::Identity(0.0f,0.0f,0.0f,1.0f); 33 34QuatF& QuatF::set( const EulerF & e ) 35{ 36 /* 37 F32 cx, sx; 38 F32 cy, sy; 39 F32 cz, sz; 40 mSinCos( -e.x * 0.5f, sx, cx ); 41 mSinCos( -e.y * 0.5f, sy, cy ); 42 mSinCos( -e.z * 0.5f, sz, cz ); 43 44 // Qyaw(z) = [ (0, 0, sin z/2), cos z/2 ] 45 // Qpitch(x) = [ (sin x/2, 0, 0), cos x/2 ] 46 // Qroll(y) = [ (0, sin y/2, 0), cos y/2 ] 47 // this = Qresult = Qyaw*Qpitch*Qroll ZXY 48 // 49 // The code that folows is a simplification of: 50 // roll*=pitch; 51 // roll*=yaw; 52 // *this = roll; 53 F32 cycz, sysz, sycz, cysz; 54 cycz = cy*cz; 55 sysz = sy*sz; 56 sycz = sy*cz; 57 cysz = cy*sz; 58 w = cycz*cx + sysz*sx; 59 x = cycz*sx + sysz*cx; 60 y = sycz*cx - cysz*sx; 61 z = cysz*cx - sycz*sx; 62 */ 63 // Assuming the angles are in radians. 64 F32 c1 = mCos(e.y * 0.5f); 65 F32 s1 = mSin(e.y * 0.5f); 66 F32 c2 = mCos(e.z * 0.5f); 67 F32 s2 = mSin(e.z * 0.5f); 68 F32 c3 = mCos(e.x * 0.5f); 69 F32 s3 = mSin(e.x * 0.5f); 70 F32 c1c2 = c1*c2; 71 F32 s1s2 = s1*s2; 72 w =c1c2*c3 - s1s2*s3; 73 x =c1c2*s3 + s1s2*c3; 74 y =s1*c2*c3 + c1*s2*s3; 75 z =c1*s2*c3 - s1*c2*s3; 76 77 return *this; 78} 79 80QuatF& QuatF::operator*=( const QuatF & b ) 81{ 82 QuatF prod; 83 prod.w = w * b.w - x * b.x - y * b.y - z * b.z; 84 prod.x = w * b.x + x * b.w + y * b.z - z * b.y; 85 prod.y = w * b.y + y * b.w + z * b.x - x * b.z; 86 prod.z = w * b.z + z * b.w + x * b.y - y * b.x; 87 *this = prod; 88 return (*this); 89} 90 91QuatF& QuatF::operator/=( const QuatF & c ) 92{ 93 QuatF temp = c; 94 return ( (*this) *= temp.inverse() ); 95} 96 97QuatF& QuatF::square() 98{ 99 F32 t = w*2.0f; 100 w = (w*w) - (x*x + y*y + z*z); 101 x *= t; 102 y *= t; 103 z *= t; 104 return *this; 105} 106 107QuatF& QuatF::inverse() 108{ 109 F32 magnitude = w*w + x*x + y*y + z*z; 110 F32 invMagnitude; 111 if( magnitude == 1.0f ) // special case unit quaternion 112 { 113 x = -x; 114 y = -y; 115 z = -z; 116 } 117 else // else scale 118 { 119 if( magnitude == 0.0f ) 120 invMagnitude = 1.0f; 121 else 122 invMagnitude = 1.0f / magnitude; 123 w *= invMagnitude; 124 x *= -invMagnitude; 125 y *= -invMagnitude; 126 z *= -invMagnitude; 127 } 128 return *this; 129} 130 131QuatF & QuatF::set( const AngAxisF & a ) 132{ 133 return set( a.axis, a.angle ); 134} 135 136QuatF & QuatF::set( const Point3F &axis, F32 angle ) 137{ 138 PROFILE_SCOPE( QuatF_set_AngAxisF ); 139 140 F32 sinHalfAngle, cosHalfAngle; 141 mSinCos( angle * 0.5f, sinHalfAngle, cosHalfAngle ); 142 x = axis.x * sinHalfAngle; 143 y = axis.y * sinHalfAngle; 144 z = axis.z * sinHalfAngle; 145 w = cosHalfAngle; 146 return *this; 147} 148 149QuatF & QuatF::normalize() 150{ 151 PROFILE_SCOPE( QuatF_normalize ); 152 153 F32 l = mSqrt( x*x + y*y + z*z + w*w ); 154 if( l == 0.0f ) 155 identity(); 156 else 157 { 158 x /= l; 159 y /= l; 160 z /= l; 161 w /= l; 162 } 163 return *this; 164} 165 166#define idx(r,c) (r*4 + c) 167 168QuatF & QuatF::set( const MatrixF & mat ) 169{ 170 PROFILE_SCOPE( QuatF_set_MatrixF ); 171 172 F32 const *m = mat; 173 174 F32 trace = m[idx(0, 0)] + m[idx(1, 1)] + m[idx(2, 2)]; 175 if (trace > 0.0f) 176 { 177 F32 s = mSqrt(trace + F32(1)); 178 w = s * 0.5f; 179 s = 0.5f / s; 180 x = (m[idx(1,2)] - m[idx(2,1)]) * s; 181 y = (m[idx(2,0)] - m[idx(0,2)]) * s; 182 z = (m[idx(0,1)] - m[idx(1,0)]) * s; 183 } 184 else 185 { 186 F32* q = &x; 187 U32 i = 0; 188 if (m[idx(1, 1)] > m[idx(0, 0)]) i = 1; 189 if (m[idx(2, 2)] > m[idx(i, i)]) i = 2; 190 U32 j = (i + 1) % 3; 191 U32 k = (j + 1) % 3; 192 193 F32 s = mSqrt((m[idx(i, i)] - (m[idx(j, j)] + m[idx(k, k)])) + 1.0f); 194 q[i] = s * 0.5f; 195 s = 0.5f / s; 196 q[j] = (m[idx(i,j)] + m[idx(j,i)]) * s; 197 q[k] = (m[idx(i,k)] + m[idx(k, i)]) * s; 198 w = (m[idx(j,k)] - m[idx(k, j)]) * s; 199 } 200 201 // Added to resolve issue #2230 202 normalize(); 203 204 return *this; 205} 206 207MatrixF * QuatF::setMatrix( MatrixF * mat ) const 208{ 209 if( x*x + y*y + z*z < 10E-20f) // isIdentity() -- substituted code a little more stringent but a lot faster 210 mat->identity(); 211 else 212 m_quatF_set_matF( x, y, z, w, *mat ); 213 return mat; 214} 215 216QuatF & QuatF::slerp( const QuatF & q, F32 t ) 217{ 218 return interpolate( *this, q, t ); 219} 220 221QuatF & QuatF::extrapolate( const QuatF & q1, const QuatF & q2, F32 t ) 222{ 223 // assert t >= 0 && t <= 1 224 // q1 is value at time = 0 225 // q2 is value at time = t 226 // Computes quaternion at time = 1 227 F64 flip,cos = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; 228 if (cos < 0.0) 229 { 230 cos = -cos; 231 flip = -1.0; 232 } 233 else 234 flip = 1.0; 235 236 F64 s1,s2; 237 if ((1.0 - cos) > 0.00001) 238 { 239 F64 om = mAcos(cos) / t; 240 F64 sd = 1.0 / mSin(t * om); 241 s1 = flip * mSin(om) * sd; 242 s2 = mSin((1.0 - t) * om) * sd; 243 } 244 else 245 { 246 // If quats are very close, do linear interpolation 247 s1 = flip / t; 248 s2 = (1.0 - t) / t; 249 } 250 251 x = F32(s1 * q2.x - s2 * q1.x); 252 y = F32(s1 * q2.y - s2 * q1.y); 253 z = F32(s1 * q2.z - s2 * q1.z); 254 w = F32(s1 * q2.w - s2 * q1.w); 255 256 return *this; 257} 258 259QuatF & QuatF::interpolate( const QuatF & q1, const QuatF & q2, F32 t ) 260{ 261 //----------------------------------- 262 // Calculate the cosine of the angle: 263 264 double cosOmega = q1.dot( q2 ); 265 266 //----------------------------------- 267 // adjust signs if necessary: 268 269 F32 sign2; 270 if ( cosOmega < 0.0 ) 271 { 272 cosOmega = -cosOmega; 273 sign2 = -1.0f; 274 } 275 else 276 sign2 = 1.0f; 277 278 //----------------------------------- 279 // calculate interpolating coeffs: 280 281 F64 scale1, scale2; 282 if ( (1.0 - cosOmega) > 0.00001 ) 283 { 284 // standard case 285 F64 omega = mAcos(cosOmega); 286 F64 sinOmega = mSin(omega); 287 scale1 = mSin((1.0 - t) * omega) / sinOmega; 288 scale2 = sign2 * mSin(t * omega) / sinOmega; 289 } 290 else 291 { 292 // if quats are very close, just do linear interpolation 293 scale1 = 1.0 - t; 294 scale2 = sign2 * t; 295 } 296 297 298 //----------------------------------- 299 // actually do the interpolation: 300 301 x = F32(scale1 * q1.x + scale2 * q2.x); 302 y = F32(scale1 * q1.y + scale2 * q2.y); 303 z = F32(scale1 * q1.z + scale2 * q2.z); 304 w = F32(scale1 * q1.w + scale2 * q2.w); 305 return *this; 306} 307 308Point3F & QuatF::mulP(const Point3F& p, Point3F* r) const 309{ 310 QuatF qq; 311 QuatF qi = *this; 312 QuatF qv( p.x, p.y, p.z, 0.0f); 313 314 qi.inverse(); 315 qq.mul(qi, qv); 316 qv.mul(qq, *this); 317 r->set(qv.x, qv.y, qv.z); 318 return *r; 319} 320 321QuatF & QuatF::mul( const QuatF &a, const QuatF &b) 322{ 323 AssertFatal( &a != this && &b != this, "QuatF::mul: dest should not be same as source" ); 324 w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; 325 x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; 326 y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z; 327 z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x; 328 return *this; 329} 330 331QuatF & QuatF::shortestArc( const VectorF &a, const VectorF &b ) 332{ 333 // From Game Programming Gems pg. 217 334 VectorF c = mCross( a, b ); 335 F32 d = mDot( a, b ); 336 F32 s = mSqrt( ( 1 + d ) * 2 ); 337 338 x = c.x / s; 339 y = c.y / s; 340 z = c.z / s; 341 w = s / 2.f; 342 343 return *this; 344} 345 346