Main Page   Namespace List   Class Hierarchy   Compound List   File List   Compound Members   File Members  

quat.cpp

Go to the documentation of this file.
00001 #include <windows.h>
00002 #include "quat.h"
00003 #include "mmgr.h"
00004 
00005 QUAT::QUAT(float sx, float sy, float sz, float sw)
00006 :
00007     x(sx),
00008     y(sy),
00009     z(sz),
00010         w(sw)
00011 {
00012 }
00013 
00014 QUAT::~QUAT()
00015 {
00016 }
00017 
00018 void QUAT::Reset()
00019 {
00020     x = 0.0;
00021     y = 0.0;
00022     z = 0.0;
00023     w = 1.0;
00024 }
00025 
00026 void QUAT::CopyQuat(QUAT q)
00027 {
00028     x = q.x;
00029     y = q.y;
00030     z = q.z;
00031     w = q.w;
00032 }
00033 
00034 void QUAT::AxisAngleToQuat(VECTOR axis, float theta)
00035 {
00036         float halfTheta = theta * 0.5;
00037         float cosHalfTheta = cos(halfTheta);
00038         float sinHalfTheta = sin(halfTheta);
00039         x = axis.x * sinHalfTheta;
00040         y = axis.y * sinHalfTheta;
00041         z = axis.z * sinHalfTheta;
00042         w = cosHalfTheta;
00043 }
00044 
00045 void QUAT::EulerToQuat(float roll, float pitch, float yaw)
00046 {
00047         float cr, cp, cy, sr, sp, sy, cpcy, spsy;  // calculate trig identities
00048         cr = cos(roll/2);
00049         cp = cos(pitch/2);
00050         cy = cos(yaw/2);
00051         sr = sin(roll/2);
00052         sp = sin(pitch/2);
00053         sy = sin(yaw/2);
00054         cpcy = cp * cy;
00055         spsy = sp * sy;
00056         w = cr * cpcy + sr * spsy;
00057         x = sr * cpcy - cr * spsy;
00058         y = cr * sp * cy + sr * cp * sy;
00059         z = cr * cp * sy - sr * sp * cy;
00060 }
00061 
00062 float QUAT::MagnitudeQuat()
00063 {
00064       return( sqrt(w*w+x*x+y*y+z*z));
00065 }
00066 
00067 void QUAT::NormaliseQuat()
00068 {
00069       float Mag;
00070       Mag = MagnitudeQuat();
00071       w = w/Mag;
00072       x = x/Mag;
00073       y = y/Mag;
00074       z = z/Mag;
00075 }
00076 
00077 void QUAT::MultQuat(QUAT q)
00078 {
00079       QUAT q3;
00080       VECTOR vectorq1;
00081       VECTOR vectorq2;
00082       vectorq1.x = x;
00083       vectorq1.y = y;
00084       vectorq1.z = z;
00085       vectorq2.x = q.x;
00086       vectorq2.y = q.y;
00087       vectorq2.z = q.z;
00088 
00089       VECTOR tempvec1;
00090       VECTOR tempvec2;
00091       VECTOR tempvec3;
00092       tempvec1 = vectorq1;
00093       q3.w = (w*q.w) - tempvec1.DotProduct(vectorq2);
00094       tempvec1.CrossVector(vectorq2);
00095       tempvec2.x = w * q.x;
00096       tempvec2.y = w * q.y;
00097       tempvec2.z = w * q.z;
00098       tempvec3.x = q.w * x;
00099       tempvec3.y = q.w * y;
00100       tempvec3.z = q.w * z;
00101       q3.x = tempvec1.x + tempvec2.x + tempvec3.x;
00102       q3.y = tempvec1.y + tempvec2.y + tempvec3.y;
00103       q3.z = tempvec1.z + tempvec2.z + tempvec3.z;
00104       CopyQuat(q3);
00105 }
00106 
00107 void QUAT::MatrixToQuat(float m[4][4])
00108 {
00109   float  tr, s, q[4];
00110   int    i, j, k;
00111 
00112   int nxt[3] = {1, 2, 0};
00113 
00114   tr = m[0][0] + m[1][1] + m[2][2];
00115 
00116   // check the diagonal
00117   if (tr > 0.0) {
00118     s = sqrt (tr + 1.0);
00119     w = s / 2.0;
00120     s = 0.5 / s;
00121     x = (m[1][2] - m[2][1]) * s;
00122     y = (m[2][0] - m[0][2]) * s;
00123     z = (m[0][1] - m[1][0]) * s;
00124 } 
00125 else 
00126 {        
00127      // diagonal is negative
00128           i = 0;
00129           if (m[1][1] > m[0][0]) i = 1;
00130          if (m[2][2] > m[i][i]) i = 2;
00131             j = nxt[i];
00132             k = nxt[j];
00133 
00134             s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
00135       
00136          q[i] = s * 0.5;
00137             
00138             if (s != 0.0) s = 0.5 / s;
00139 
00140         q[3] = (m[j][k] - m[k][j]) * s;
00141             q[j] = (m[i][j] + m[j][i]) * s;
00142             q[k] = (m[i][k] + m[k][i]) * s;
00143 
00144       x = q[0];
00145       y = q[1];
00146       z = q[2];
00147       w = q[3];
00148   }
00149 }

Generated on Fri Dec 23 05:20:18 2005 for Portals by doxygen1.2.15