MercuryTypes.cpp

Go to the documentation of this file.
00001 #include <cmath>
00002 #include <string.h>
00003 #include "MercuryTypes.h"
00004 #include "MercuryMath.h"
00005 #include "MercuryUtil.h"
00006 
00007 const float MercuryPoint::operator[] ( const int rhs ) const
00008 {
00009     switch (rhs)
00010     {
00011     case 0: return x;
00012     case 1: return y;
00013     case 2: return z;
00014     }
00015     return x;   //haha we won't even get here.
00016 }
00017 
00018 float & MercuryPoint::operator [] ( const int rhs )
00019 {
00020     switch (rhs)
00021     {
00022     case 0: return x;
00023     case 1: return y;
00024     case 2: return z;
00025     }
00026     return x;   //haha we won't even get here.
00027 }
00028 
00029 MercuryPoint MercuryPoint::operator*(const MercuryPoint& p) const
00030 {
00031     MercuryPoint tmp;
00032     tmp.x = x * p.x;
00033     tmp.y = y * p.y; 
00034     tmp.z = z * p.z;
00035     return tmp;
00036 }
00037 
00038 MercuryPoint MercuryPoint::operator/(const MercuryPoint& p) const
00039 {
00040     MercuryPoint tmp;
00041     tmp.x = x / p.x;
00042     tmp.y = y / p.y; 
00043     tmp.z = z / p.z;
00044     return tmp;
00045 }
00046 
00047 bool MercuryPoint::operator==(const MercuryPoint& p) const
00048 {
00049     if ((x == p.x) && (y == p.y) && (z == p.z))
00050         return true;
00051 
00052     return false;
00053 }
00054 
00055 bool MercuryPoint::operator==(const float f) const
00056 {
00057     if ((x == f) && (y == f) && (z == f))
00058         return true;
00059 
00060     return false;
00061 }
00062 
00063 MercuryPoint MercuryPoint::CrossProduct(const MercuryPoint& p) const
00064 {
00065     MercuryPoint ret;
00066 
00067     ret[0] = y*p.z - z*p.y;
00068     ret[1] = z*p.x - x*p.z;
00069     ret[2] = x*p.y - y*p.x;
00070 
00071     return ret;
00072 }
00073 
00074 void MercuryPoint::NormalizeSelf()
00075 {
00076     float imag = 1.0f/Magnitude();
00077     x *= imag; y *= imag; z *= imag;
00078 }
00079 
00080 const MercuryPoint MercuryPoint::Normalize() const
00081 {
00082     MercuryPoint t(*this);
00083     t.NormalizeSelf();
00084     return t;
00085 }
00086 
00087 float MercuryPoint::Magnitude() const
00088 {
00089     float length = 0;
00090     length += x*x;
00091     length += y*y;
00092     length += z*z;
00093     return SQRT(length);
00094 }
00095 
00096 MercuryPoint Rotate2DPoint( float fAngle, MercuryPoint pIn )
00097 {
00098     MercuryPoint ret;
00099     ret.x += SIN(fAngle)*pIn.y + COS(-fAngle)*pIn.x;
00100     ret.y += COS(fAngle)*pIn.y + SIN(-fAngle)*pIn.x;
00101     return ret;
00102 }
00103 
00104 void AngleMatrix (const MercuryPoint & angles, MercuryMatrix & matrix )
00105 {
00106     float X = angles[0]*2*Q_PI/360; //Reduced calulation for speed
00107     float Y = angles[1]*2*Q_PI/360;
00108     float Z = angles[2]*2*Q_PI/360;
00109     float cx = COS(X);
00110     float sx = SIN(X);
00111     float cy = COS(Y);
00112     float sy = SIN(Y);
00113     float cz = COS(Z);
00114     float sz = SIN(Z);
00115       
00116     //Row major
00117     matrix[0][0] = cy*cz;
00118     matrix[0][1] = (sx*sy*cz)-(cx*sz);
00119     matrix[0][2] = (cx*sy*cz)+(sx*sz);
00120     matrix[0][3] = 0;
00121 
00122     matrix[1][0] = cy*sz;
00123     matrix[1][1] = (sx*sy*sz)+(cx*cz);
00124     matrix[1][2] = (cx*sy*sz)-(sx*cz);
00125     matrix[1][3] = 0;
00126 
00127     matrix[2][0] = -sy;
00128     matrix[2][1] = sx*cy;
00129     matrix[2][2] = cx*cy;
00130     matrix[2][3] = 0;
00131 
00132     matrix[3][0] = 0;
00133     matrix[3][1] = 0;
00134     matrix[3][2] = 0;
00135     matrix[3][3] = 1;
00136 }
00137 
00138 void R_ConcatTransforms3 ( MercuryMatrix in1, MercuryMatrix in2, MercuryMatrix & out)
00139 {
00140     out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
00141                 in1[0][2] * in2[2][0];
00142     out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
00143                 in1[0][2] * in2[2][1];
00144     out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
00145                 in1[0][2] * in2[2][2];
00146     out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
00147                 in1[0][2] * in2[2][3] + in1[0][3];
00148     out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
00149                 in1[1][2] * in2[2][0];
00150     out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
00151                 in1[1][2] * in2[2][1];
00152     out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
00153                 in1[1][2] * in2[2][2];
00154     out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
00155                 in1[1][2] * in2[2][3] + in1[1][3];
00156     out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
00157                 in1[2][2] * in2[2][0];
00158     out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
00159                 in1[2][2] * in2[2][1];
00160     out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
00161                 in1[2][2] * in2[2][2];
00162     out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
00163                 in1[2][2] * in2[2][3] + in1[2][3];
00164 }
00165 
00166 //rewrite
00167 //Get rid of vectors and use Quaternion.rotateAbout(Quaternion)
00168 void VectorRotate (const MercuryPoint &in1,  const MercuryMatrix &in2, MercuryPoint &out) {
00169     out[0] = DotProduct(in1, in2[0]);
00170     out[1] = DotProduct(in1, in2[1]);
00171     out[2] = DotProduct(in1, in2[2]);
00172 }
00173 
00174 //Must rewrite
00175 // rotate by the inverse of the matrix
00176 void VectorIRotate (const MercuryPoint &in1,  MercuryMatrix &in2, MercuryPoint & out)
00177 {
00178     out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0];
00179     out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1];
00180     out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2];
00181 }
00182 
00183 void TranslationMatrix( const MercuryPoint & position, MercuryMatrix & mat )
00184 {
00185     mat[0][0] = 1;
00186     mat[1][0] = 0;
00187     mat[2][0] = 0;
00188     mat[3][0] = 0;
00189     mat[0][1] = 0;
00190     mat[1][1] = 1;
00191     mat[2][1] = 0;
00192     mat[3][1] = 0;
00193     mat[0][2] = 0;
00194     mat[1][2] = 0;
00195     mat[2][2] = 1;
00196     mat[3][2] = 0;
00197     mat[0][3] = position.x;
00198     mat[1][3] = position.y;
00199     mat[2][3] = position.z;
00200     mat[3][3] = 1;
00201 }
00202 
00203 void VectorMultiply( MercuryMatrix &m, const MercuryPoint &p, MercuryPoint &out )
00204 {
00205     out[0] = p[0] * m[0][0] + p[1] * m[0][1] + p[2] * m[0][2] + m[0][3];
00206     out[1] = p[0] * m[1][0] + p[1] * m[1][1] + p[2] * m[1][2] + m[1][3];
00207     out[2] = p[0] * m[2][0] + p[1] * m[2][1] + p[2] * m[2][2] + m[2][3];
00208 }
00209 
00210 /* CODE FOR COMPLETING MATRIX-MATRIX MULTIPLICATION  (Thank god for MATH 221 w/ Dr. Yoon Song @ UMBC ) */
00211 void ScaleSector ( MercuryMatrix &in, MercuryMatrix &out, int iLine, float fAmount )
00212 {
00213     out[iLine][0] = in[iLine][0] * fAmount;
00214     out[iLine][1] = in[iLine][1] * fAmount;
00215     out[iLine][2] = in[iLine][2] * fAmount;
00216     out[iLine][3] = in[iLine][3] * fAmount;
00217 }
00218 
00219 void TranslateSector ( MercuryMatrix &in, MercuryMatrix &out, int iLine, int iLine2, float fAmount )
00220 {
00221     out[iLine][0] += in[iLine2][0] * fAmount;
00222     out[iLine][1] += in[iLine2][1] * fAmount;
00223     out[iLine][2] += in[iLine2][2] * fAmount;
00224     out[iLine][3] += in[iLine2][3] * fAmount;
00225 }
00226 
00227 void InvertMatrix( MercuryMatrix &in, MercuryMatrix &out )
00228 {
00229     MercuryMatrix tmp;
00230     memcpy( &tmp[0][0], &in[0][0], sizeof(float) * 16 );
00231 
00232     out[0][1] = out[0][2] = out[0][3] = 0;
00233     out[1][0] = out[1][2] = out[1][3] = 0;
00234     out[2][0] = out[2][1] = out[2][3] = 0;
00235     out[3][0] = out[3][1] = out[3][2] = 0;
00236     out[0][0] = out[1][1] = out[2][2] = out[3][3] = 1;
00237     
00238     for ( int i = 0; i < 4; i++ )
00239         for ( int j = 0; j < 4; j++ )
00240         {
00241             //Ok, this is a pretty damn discusting trick.
00242             //On matricies with one of the pivots having tiny values
00243             // there's a big issue when it tries to get inverted.
00244             // in many cases it can produce a matrix off by as much as 1 or 2 hundred
00245             // the only way around this is to pick another row and add it to the 
00246             // row with an issue ahead of time, bringing all of the numbers to 
00247             // usable numbers.
00248             if( ABS( 1.0f/tmp[i][i] ) > 1000.0f )
00249             {
00250                 TranslateSector( out, out, i, (i+1)%4, 1 );
00251                 TranslateSector( tmp, tmp, i, (i+1)%4, 1 );
00252             }
00253 
00254             float fMt;
00255             if ( i == j )
00256             {
00257                 fMt = 1/tmp[i][i];
00258                 ScaleSector( tmp, tmp, j, fMt );
00259                 ScaleSector( out, out, j, fMt );
00260             }
00261             else
00262             {
00263                 fMt = -tmp[j][i]/tmp[i][i];
00264                 TranslateSector( tmp, tmp, j, i, fMt );
00265                 TranslateSector( out, out, j, i, fMt );
00266             }
00267         }
00268 
00269     tmp.Ptr();
00270 }
00271 
00272 
00273 
00274 
00275 
00276 
00277 
00278 
00279 
00280 
00281 
00282 
00283 
00284 
00285 
00286 
00287 
00288 
00289 
00290 float & MQuaternion::operator [] (int i)
00291 {
00292     switch (i)
00293     {
00294     case 0: return x;
00295     case 1: return y;
00296     case 2: return z;
00297     case 3: return w;
00298     }
00299     return x;   //haha we won't even get here.
00300 }
00301 
00302 void MQuaternion::SetEuler(const MercuryPoint& angles)
00303 {
00304     float X = angles[0]/2.0f; //roll
00305     float Y = angles[1]/2.0f; //pitch
00306     float Z = angles[2]/2.0f; //yaw
00307 
00308     float cx = COS(X);
00309     float sx = SIN(X);
00310     float cy = COS(Y);
00311     float sy = SIN(Y);
00312     float cz = COS(Z);
00313     float sz = SIN(Z);
00314 
00315     //Correct according to
00316     //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
00317     w = cx*cy*cz+sx*sy*sz;//q1
00318     x = sx*cy*cz-cx*sy*sz;//q2
00319     y = cx*sy*cz+sx*cy*sz;//q3
00320     z = cx*cy*sz-sx*sy*cz;//q4
00321 }
00322 
00323 void MQuaternion::CreateFromAxisAngle(const MercuryPoint& p, const float radians)
00324 {
00325     float sn = SIN(radians/2.0f);
00326     w = COS(radians/2.0f);
00327     x = sn * p.x;
00328     y = sn * p.y;
00329     z = sn * p.z;
00330 }
00331 
00332 //Returns the magnitude
00333 float MQuaternion::magnitude() const {
00334     return SQRT((w*w)+(x*x)+(y*y)+(z*z));
00335 }
00336 
00337 //Returns the normalized MQuaternion
00338 MQuaternion MQuaternion::normalize() const {
00339     return (*this)/magnitude();
00340 }
00341 
00342 //Returns the conjugate MQuaternion
00343 MQuaternion MQuaternion::conjugate() const {
00344     MQuaternion c(w,-x,-y,-z);
00345     return c;
00346 }
00347 
00348 //Returns the reciprocal MQuaternion
00349 MQuaternion MQuaternion::reciprocal() const {
00350     float m = magnitude();
00351     return conjugate()/(m*m);
00352 }
00353 
00354 //Rotate MQuaternion about another MQuaternion
00355 MQuaternion MQuaternion::rotateAbout(const MQuaternion &spinAxis) const {
00356     return (*this)*spinAxis;
00357 }
00358 
00359 //Converts MQuaternion to 4x4 Matrix(3x3 Spatial)
00360 void MQuaternion::toMatrix( MercuryMatrix &matrix ) const {
00361     float X = 2*x*x; //Reduced calulation for speed
00362     float Y = 2*y*y;
00363     float Z = 2*z*z;
00364     float a = 2*w*x;
00365     float b = 2*w*y;
00366     float c = 2*w*z;
00367     float d = 2*x*y;
00368     float e = 2*x*z;
00369     float f = 2*y*z;
00370 
00371     //row major
00372     matrix[0][0] = 1-Y-Z;
00373     matrix[0][1] = d-c;
00374     matrix[0][2] = e+b;  
00375 
00376     matrix[1][0] = d+c;
00377     matrix[1][1] = 1-X-Z;
00378     matrix[1][2] = f-a;
00379 
00380     matrix[2][0] = e-b;
00381     matrix[2][1] = f+a;
00382     matrix[2][2] = 1-X-Y;
00383 }
00384 
00385 void MQuaternion::toMatrix4( MercuryMatrix &matrix ) const {
00386     toMatrix( matrix );
00387 
00388     //row major (even though it looks like column)
00389     matrix[0][3] = 0;
00390     matrix[1][3] = 0;
00391     matrix[2][3] = 0;
00392     matrix[3][3] = 1;
00393 
00394     matrix[3][0] = 0;
00395     matrix[3][1] = 0;
00396     matrix[3][2] = 0;
00397 }
00398 
00399 MQuaternion MQuaternion::operator + (const MQuaternion &rhs) const
00400 {
00401     MQuaternion result;
00402     result.w = w + rhs.w;
00403     result.x = x + rhs.x;
00404     result.y = y + rhs.y;
00405     result.z = z + rhs.z;
00406     return result;
00407 }
00408 
00409 MQuaternion MQuaternion::operator - (const MQuaternion &rhs) const
00410 {
00411     MQuaternion result;
00412     result.w = w - rhs.w;
00413     result.x = x - rhs.x;
00414     result.y = y - rhs.y;
00415     result.z = z - rhs.z;
00416     return result;
00417 }
00418 
00419 MQuaternion MQuaternion::operator * (const MQuaternion &rhs) const 
00420 {
00421     MQuaternion result;
00422     result.w = (w*rhs.w)-(x*rhs.x)-(y*rhs.y)-(z*rhs.z);
00423     result.x = (w*rhs.x)+(x*rhs.w)+(y*rhs.z)-(z*rhs.y);
00424     result.y = (w*rhs.y)-(x*rhs.z)+(y*rhs.w)+(z*rhs.x);
00425     result.z = (w*rhs.z)+(x*rhs.y)-(y*rhs.x)+(z*rhs.w);
00426     return result;
00427 }
00428 
00429 MQuaternion& MQuaternion::operator = (const MQuaternion &rhs) 
00430 {
00431     w = rhs.w;
00432     x = rhs.x;
00433     y = rhs.y;
00434     z = rhs.z;
00435     return (*this);
00436 }
00437 
00438 MQuaternion& MQuaternion::operator += (const MQuaternion &rhs) {
00439     w += rhs.w;
00440     x += rhs.x;
00441     y += rhs.y;
00442     z += rhs.z;
00443     return (*this);
00444 }
00445 
00446 MQuaternion& MQuaternion::operator -= (const MQuaternion &rhs) {
00447     w -= rhs.w;
00448     x -= rhs.x;
00449     y -= rhs.y;
00450     z -= rhs.z;
00451     return (*this);
00452 }
00453 
00454 MQuaternion& MQuaternion::operator *= (const MQuaternion &rhs) {
00455     w = (w*rhs.w)-(x*rhs.x)-(y*rhs.y)-(z*rhs.z);
00456     x = (w*rhs.x)+(x*rhs.w)+(y*rhs.z)-(z*rhs.y);
00457     y = (w*rhs.y)-(x*rhs.z)+(y*rhs.w)+(z*rhs.x);
00458     z = (w*rhs.z)+(x*rhs.y)-(y*rhs.x)+(z*rhs.w);
00459     return (*this);
00460 }
00461 
00462 MQuaternion MQuaternion::operator * (const float &rhs) const {
00463     MQuaternion result;
00464     result.w = w*rhs;
00465     result.x = x*rhs;
00466     result.y = y*rhs;
00467     result.z = z*rhs;
00468     return result;
00469 }
00470 
00471 MQuaternion MQuaternion::operator / (const float &rhs) const {
00472     MQuaternion result;
00473     result.w = w/rhs;
00474     result.x = x/rhs;
00475     result.y = y/rhs;
00476     result.z = z/rhs;
00477     return result;
00478 }
00479 
00480 MQuaternion& MQuaternion::operator *= (const float &rhs) {
00481     w *= rhs;
00482     x *= rhs;
00483     y *= rhs;
00484     z *= rhs;
00485     return (*this);
00486 }
00487 
00488 MQuaternion& MQuaternion::operator /= (const float &rhs) {
00489     w /= rhs;
00490     x /= rhs;
00491     y /= rhs;
00492     z /= rhs;
00493     return (*this);
00494 }
00495 
00496 //Returns the Euclidian Inner Product of two MQuaternions (Similar to Vector Dot-Product)
00497 float innerProduct(const MQuaternion & a, const MQuaternion &b) 
00498 {
00499     return (a.w*b.w)+(a.x*b.x)+(a.y*b.y)+(a.z*b.z);
00500 }
00501 
00502 //Returns the Euclidian Outer Product of two MQuaternions
00503 MercuryPoint outerProduct(MQuaternion a,MQuaternion b) 
00504 {
00505     MercuryPoint result;
00506     result.x = (a.w*b.x)-(a.x*b.w)-(a.y*b.z)+(a.z*b.y);
00507     result.y = (a.w*b.y)+(a.x*b.z)-(a.y*b.w)-(a.z*b.x);
00508     result.z = (a.w*b.z)-(a.x*b.y)+(a.y*b.x)-(a.z*b.w);
00509     return result;
00510 }
00511 
00512 //Returns the Even Product of two MQuaternions
00513 MQuaternion evenProduct(MQuaternion a,MQuaternion b) {
00514     MQuaternion result;
00515     result.w = (a.w*b.w)-(a.x*b.x)-(a.y*b.y)-(a.z*b.z);
00516     result.x = (a.w*b.x)+(a.x*b.w);
00517     result.y = (a.w*b.y)+(a.y*b.w);
00518     result.z = (a.w*b.z)+(a.z*b.w);
00519     return result;
00520 }
00521 
00522 //Returns the Odd Product of two MQuaternions (Similar to Vector Cross-Product)
00523 MercuryPoint oddProduct(MQuaternion a,MQuaternion b) {
00524     MercuryPoint result;
00525     result.x = (a.y*b.z)-(a.z*b.y);
00526     result.y = (a.z*b.x)-(a.x*b.z);
00527     result.z = (a.x*b.y)-(a.y*b.x);
00528     return result;
00529 }
00530 
00531 //Spherical Linear Interpolation between two MQuaternions at t percent completion(0-1)
00532 MQuaternion SLERP( const MQuaternion &a, const MQuaternion &b,float t) {
00533     MQuaternion an = a.normalize(), bn = b.normalize();
00534     float cosTheta = innerProduct(MQuaternion(an),bn);
00535     float sinTheta;
00536 
00537     //Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll
00538     // cause SQRT to produce not a number, and screw everything up.
00539     if ( 1 - (cosTheta*cosTheta) <= 0 )
00540         sinTheta = 0;
00541     else
00542         sinTheta = SQRT(1 - (cosTheta*cosTheta));
00543 
00544     float Theta = ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions
00545 
00546     if(fabs(Theta) < 0.01)
00547         return a;
00548     if(fabs(sinTheta) < 0.01)
00549         return (a+b)/2;
00550     return ( (a*SIN((1-t)*Theta)) + (b*SIN(t*Theta)) ) / sinTheta;
00551 }
00552 
00553 const MercuryPoint gpZero = MercuryPoint( 0,0,0 );
00554 const MercuryPoint gpOne = MercuryPoint( 1,1,1 );
00555 
00556 
00557 
00558 /* 
00559  * Copyright (c) 2005-2006, Joshua Allen, Charles Lohr, Adam Lowman
00560  * All rights reserved.
00561  *
00562  * Redistribution and use in source and binary forms, with or
00563  * without modification, are permitted provided that the following
00564  * conditions are met:
00565  *  -   Redistributions of source code must retain the above
00566  *      copyright notice, this list of conditions and the following disclaimer.
00567  *  -   Redistributions in binary form must reproduce the above copyright
00568  *      notice, this list of conditions and the following disclaimer in
00569  *      the documentation and/or other materials provided with the distribution.
00570  *  -   Neither the name of the Mercury Engine nor the names of its
00571  *      contributors may be used to endorse or promote products derived from
00572  *      this software without specific prior written permission.
00573  *
00574  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00575  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00576  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00577  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
00578  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00579  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00580  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00581  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00582  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
00583  * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00584  */

Hosted by SourceForge.net Logo