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

locmath.cpp

Go to the documentation of this file.
00001 #include <windows.h>
00002 #include "locmath.h"
00003 #include "vector.h"
00004 #include "quat.h"
00005 #include "vertex.h"
00006 
00007 void LoadIdentity(float m[])
00008 {
00009         m[0]=1.0f;
00010         m[1]=0.0f;
00011         m[2]=0.0f;
00012         m[3]=0.0f;
00013 
00014         m[4]=0.0f;
00015         m[5]=1.0f;
00016         m[6]=0.0f;
00017         m[7]=0.0f;
00018 
00019         m[8]=0.0f;
00020         m[9]=0.0f;
00021         m[10]=1.0f;
00022         m[11]=0.0f;
00023 
00024         m[12]=0.0f;
00025         m[13]=0.0f;
00026         m[14]=0.0f;
00027         m[15]=1.0f;
00028 }
00029 
00030 void CopyMatrix(float m[], float n[])
00031 {
00032         m[0 ] = n[0 ];
00033         m[1 ] = n[1 ];
00034         m[2 ] = n[2 ];
00035         m[3 ] = n[3 ];
00036         m[4 ] = n[4 ];
00037         m[5 ] = n[5 ];
00038         m[6 ] = n[6 ];
00039         m[7 ] = n[7 ];
00040         m[8 ] = n[8 ];
00041         m[9 ] = n[9 ];
00042         m[10] = n[10];
00043         m[11] = n[11];
00044         m[12] = n[12];
00045         m[13] = n[13];
00046         m[14] = n[14];
00047         m[15] = n[15];
00048 }
00049 
00050 void MultMatrix(float m[], float n[])
00051 {
00052         float temp[16];
00053 
00054         CopyMatrix(temp, m);
00055       m[0] = temp[0 ]*n[0 ]
00056                + temp[4 ]*n[1 ]
00057                + temp[8 ]*n[2 ]
00058                + temp[12]*n[3 ];
00059 
00060       m[1] = temp[1 ]*n[0 ]
00061                + temp[5 ]*n[1 ]
00062                + temp[9 ]*n[2 ]
00063                + temp[13]*n[3 ];
00064 
00065       m[2] = temp[2 ]*n[0 ]
00066                + temp[6 ]*n[1 ]
00067                + temp[10]*n[2 ]
00068                + temp[14]*n[3 ];
00069 
00070       m[3] = temp[3 ]*n[0 ]
00071                + temp[7 ]*n[1 ]
00072                + temp[11]*n[2 ]
00073                + temp[15]*n[3 ];
00074 
00075       m[4] = temp[0 ]*n[4 ]
00076                + temp[4 ]*n[5 ]
00077                + temp[8 ]*n[6 ]
00078                + temp[12]*n[7 ];
00079 
00080       m[5] = temp[1 ]*n[4 ]
00081                + temp[5 ]*n[5 ]
00082                + temp[9 ]*n[6 ]
00083                + temp[13]*n[7 ];
00084 
00085       m[6] = temp[2 ]*n[4 ]
00086                + temp[6 ]*n[5 ]
00087                + temp[10]*n[6 ]
00088                + temp[14]*n[7 ];
00089 
00090       m[7] = temp[3 ]*n[4 ]
00091                + temp[7 ]*n[5 ]
00092                + temp[11]*n[6 ]
00093                + temp[15]*n[7 ];
00094 
00095       m[8] = temp[0 ]*n[8 ]
00096                + temp[4 ]*n[9 ]
00097                + temp[8 ]*n[10]
00098                + temp[12]*n[11];
00099 
00100       m[9] = temp[1 ]*n[8 ]
00101                + temp[5 ]*n[9 ]
00102                + temp[9 ]*n[10]
00103                + temp[13]*n[11];
00104 
00105       m[10]= temp[2 ]*n[8 ]
00106                + temp[6 ]*n[9 ]
00107                + temp[10]*n[10]
00108                + temp[14]*n[11];
00109 
00110       m[11]= temp[3 ]*n[8 ]
00111                + temp[7 ]*n[9 ]
00112                + temp[11]*n[10]
00113                + temp[15]*n[11];
00114 
00115       m[12]= temp[0 ]*n[12]
00116                + temp[4 ]*n[13]
00117                + temp[8 ]*n[14]
00118                + temp[12]*n[15];
00119 
00120       m[13]= temp[1 ]*n[12]
00121                + temp[5 ]*n[13]
00122                + temp[9 ]*n[14]
00123                + temp[13]*n[15];
00124 
00125       m[14]= temp[2 ]*n[12]
00126                + temp[6 ]*n[13]
00127                + temp[10]*n[14]
00128                + temp[14]*n[15];
00129 
00130       m[15]= temp[3 ]*n[12]
00131                + temp[7 ]*n[13]
00132                + temp[11]*n[14]
00133                + temp[15]*n[15];
00134 }
00135 
00136 void MatrixInverse(float m[])
00137 {
00138     float n[16];
00139 
00140       CopyMatrix(n, m);
00141       m[0 ] = n[0 ];
00142       m[1 ] = n[4 ];
00143       m[2 ] = n[8 ];
00144 
00145       m[4 ] = n[1 ];
00146       m[5 ] = n[5 ];
00147       m[6 ] = n[9 ];
00148 
00149       m[8 ] = n[2 ];
00150       m[9 ] = n[6 ];
00151       m[10] = n[10];
00152 
00153       m[12] *= -1.0f;
00154       m[13] *= -1.0f;
00155       m[14] *= -1.0f;
00156 }
00157 
00158     /* The following routine converts an angle and a unit axis vector
00159         * to a matrix, returning the corresponding unit quaternion at no
00160         * extra cost. It is written in such a way as to allow both fixed
00161         * point and floating point versions to be created by appropriate
00162         * definitions of FPOINT, ANGLE, VECTOR, QUAT, MATRIX, MUL, HALF,
00163         * TWICE, COS, SIN, ONE, and ZERO.
00164         * The following is an example of floating point definitions.*/
00165 QUAT AxisAngleToMatrix(VECTOR axis, float theta, float m[16])
00166 {
00167         QUAT q;
00168         float halfTheta = theta * 0.5;
00169         float cosHalfTheta = cos(halfTheta);
00170         float sinHalfTheta = sin(halfTheta);
00171         float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00172         q.x = axis.x * sinHalfTheta;
00173         q.y = axis.y * sinHalfTheta;
00174         q.z = axis.z * sinHalfTheta;
00175         q.w = cosHalfTheta;
00176         xs = q.x * 2;  ys = q.y * 2;  zs = q.z * 2;
00177         wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
00178         xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
00179         yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
00180         m[0] = 1 - (yy + zz);
00181         m[1] = xy - wz;
00182         m[2] = xz + wy;
00183         m[4] = xy + wz;
00184         m[5] = 1 - (xx + zz);
00185         m[6] = yz - wx;
00186         m[8] = xz - wy;
00187         m[9] = yz + wx;
00188         m[10] = 1 - (xx + yy);
00189         /* Fill in remainder of 4x4 homogeneous transform matrix. */
00190         m[12] = m[13] = m[14] = m[3] = m[7] = m[11] = 0;
00191         m[15] = 1;
00192         return (q);
00193 }
00194 
00195 float DotProduct(VECTOR vec1, VECTOR vec2)
00196 {
00197     /*
00198     Dot Product of two Vectors.
00199 
00200     U = (Ux, Uy, Uz)
00201     V = (Vx, Vy, Vz)
00202     U*V = UxVx + UyVy + UzVz
00203     U*V = |U||V|cos(t) (where t is the angle theta between the two vectors)
00204     */
00205       float dot;
00206       dot = vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
00207       return dot;
00208 }
00209 
00210 VECTOR CrossVector(VECTOR vec1, VECTOR vec2)
00211 {
00212     /*
00213     Cross Product of Two Vectors.
00214 
00215     a × b = ( a.y * b.z - a.z * b.y,
00216 
00217     a.z * b.x - a.x * b.z,
00218 
00219     a.x * b.y - a.y * b.x )
00220 
00221     | a × b | = |a| * |b| * sin(ø)
00222     */
00223       VECTOR vec3;
00224       vec3.x = vec1.y * vec2.z - vec1.z * vec2.y;
00225       vec3.y = vec1.z * vec2.x - vec1.x * vec2.z;
00226       vec3.z = vec1.x * vec2.y - vec1.y * vec2.x;
00227       return vec3;
00228 }
00229 
00230 void EulerToQuat(float roll, float pitch, float yaw, QUAT * quat)
00231 {
00232     /*
00233     Euler Angle to Quarternion.
00234 
00235     q = qyaw qpitch qroll where:
00236 
00237     qyaw = [cos(f /2), (0, 0, sin(f /2)]
00238     qpitch = [cos (q/2), (0, sin(q/2), 0)]
00239     qroll = [cos (y/2), (sin(y/2), 0, 0)]
00240     */
00241     float cr, cp, cy, sr, sp, sy, cpcy, spsy;  // calculate trig identities
00242         cr = cos(roll/2);
00243     cp = cos(pitch/2);
00244         cy = cos(yaw/2);
00245         sr = sin(roll/2);
00246     sp = sin(pitch/2);
00247         sy = sin(yaw/2);
00248         cpcy = cp * cy;
00249         spsy = sp * sy;
00250     quat->w = cr * cpcy + sr * spsy;
00251         quat->x = sr * cpcy - cr * spsy;
00252     quat->y = cr * sp * cy + sr * cp * sy;
00253         quat->z = cr * cp * sy - sr * sp * cy;
00254 }
00255 
00256 float MagnitudeQuat(QUAT q1)
00257 {
00258       return( sqrt(q1.w*q1.w+q1.x*q1.x+q1.y*q1.y+q1.z*q1.z));
00259 }
00260 
00261 QUAT NormaliseQuat(QUAT q1)
00262 {
00263       QUAT q2;
00264       float Mag;
00265       Mag = MagnitudeQuat(q1);
00266       q2.w = q1.w/Mag;
00267       q2.x = q1.x/Mag;
00268       q2.y = q1.y/Mag;
00269       q2.z = q1.z/Mag;
00270       return q2;
00271 }
00272 
00273 void QuatToMatrix(QUAT quat, float m[16])
00274 {
00275       float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
00276       // calculate coefficients
00277       x2 = quat.x + quat.x;
00278       y2 = quat.y + quat.y;
00279       z2 = quat.z + quat.z;
00280       xx = quat.x * x2;
00281       xy = quat.x * y2;
00282       xz = quat.x * z2;
00283       yy = quat.y * y2;
00284       yz = quat.y * z2;
00285       zz = quat.z * z2;
00286       wx = quat.w * x2;
00287       wy = quat.w * y2;
00288       wz = quat.w * z2;
00289       m[0] = 1.0 - (yy + zz);
00290       m[1] = xy - wz;
00291       m[2] = xz + wy;
00292       m[3] = 0.0;
00293       m[4] = xy + wz;
00294       m[5] = 1.0 - (xx + zz);
00295       m[6] = yz - wx;
00296       m[7] = 0.0;
00297       m[8] = xz - wy;
00298       m[9] = yz + wx;
00299       m[10] = 1.0 - (xx + yy);
00300       m[11] = 0.0;
00301       m[12] = 0;
00302       m[13] = 0;
00303       m[14] = 0;
00304       m[15] = 1;
00305 }
00306 
00307 QUAT MultQuat(QUAT q1, QUAT q2)
00308 {
00309     /*
00310     Multiplication of two Quarternions.
00311 
00312     qq´ = [ww´ - v · v´, v x v´ + wv´ +w´v]
00313     ( · is vector dot product and x is vector cross product )
00314     */
00315       QUAT q3;
00316       VECTOR vectorq1;
00317       VECTOR vectorq2;
00318       vectorq1.x = q1.x;
00319       vectorq1.y = q1.y;
00320       vectorq1.z = q1.z;
00321       vectorq2.x = q2.x;
00322       vectorq2.y = q2.y;
00323       vectorq2.z = q2.z;
00324 
00325       VECTOR tempvec1;
00326       VECTOR tempvec2;
00327       VECTOR tempvec3;
00328       q3.w = (q1.w*q2.w) - DotProduct(vectorq1, vectorq2);
00329       tempvec1 = CrossVector(vectorq1, vectorq2);
00330       tempvec2.x = q1.w * q2.x;
00331       tempvec2.y = q1.w * q2.y;
00332       tempvec2.z = q1.w * q2.z;
00333       tempvec3.x = q2.w * q1.x;
00334       tempvec3.y = q2.w * q1.y;
00335       tempvec3.z = q2.w * q1.z;
00336       q3.x = tempvec1.x + tempvec2.x + tempvec3.x;
00337       q3.y = tempvec1.y + tempvec2.y + tempvec3.y;
00338       q3.z = tempvec1.z + tempvec2.z + tempvec3.z;
00339       return NormaliseQuat(q3);
00340 }
00341 
00342 // returns the normal vector to a plane defined by three vertices
00343 VECTOR GetNormal(VECTOR vertex1, VECTOR vertex2, VECTOR vertex3)
00344 {
00345         float ux, uy, uz, vx, vy, vz;
00346           VECTOR temp_vertex;
00347 
00348           ux = vertex1.x - vertex2.x;
00349           uy = vertex1.y - vertex2.y;
00350           uz = vertex1.z - vertex2.z;
00351           vx = vertex3.x - vertex2.x;
00352           vy = vertex3.y - vertex2.y;
00353           vz = vertex3.z - vertex2.z;
00354           temp_vertex.x = (uy*vz)-(vy*uz);
00355           temp_vertex.y = (uz*vx)-(vz*ux);
00356           temp_vertex.z = (ux*vy)-(vx*uy);
00357           return temp_vertex;
00358 }
00359 
00360 VERTEX GetNorm(float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3)
00361 {
00362         float ux;
00363         float uy;
00364         float uz;
00365         float vx;
00366         float vy;
00367         float vz;
00368           VERTEX temp_vertex;
00369           ux = x1 - x2;
00370           uy = y1 - y2;
00371           uz = z1 - z2;
00372           vx = x3 - x2;
00373           vy = y3 - y2;
00374           vz = z3 - z2;
00375           temp_vertex.nx = (uy*vz)-(vy*uz);
00376           temp_vertex.ny = (uz*vx)-(vz*ux);
00377           temp_vertex.nz = (ux*vy)-(vx*uy);
00378           return temp_vertex;
00379 }
00380 
00381 GLfloat MagnitudeVector(VECTOR vec1)
00382 {
00383   return(sqrt(vec1.x*vec1.x+vec1.y*vec1.y+vec1.z*vec1.z));
00384 }
00385 
00386 VECTOR GetUnitVector(VECTOR vector)
00387 {
00388         // Reduces a normal vector specified as a set of three coordinates,
00389         // to a unit normal vector of length one.
00390 
00391         // Calculate the length of the vector           
00392         float length = (float) sqrt(( vector.x * vector.x) + 
00393                                                         ( vector.y * vector.y) +
00394                                                         ( vector.z * vector.z) );
00395 
00396         // Keep the program from blowing up by providing an exceptable
00397         // value for vectors that may calculated too close to zero.
00398         if(length == 0.0f)
00399                 length = 1.0f;
00400 
00401         // Dividing each element by the length will result in a
00402         // unit normal vector.
00403         vector.x /= length;
00404         vector.y /= length;
00405         vector.z /= length;
00406         return vector;
00407 }
00408 
00409 VECTOR GetEdgeVector(VECTOR point1, VECTOR point2)
00410 {
00411   VECTOR temp_vector;
00412   temp_vector.x = point1.x - point2.x;
00413   temp_vector.y = point1.y - point2.y;
00414   temp_vector.z = point1.z - point2.z;
00415   return temp_vector;
00416 }

Generated on Fri Dec 23 05:21:20 2005 for Skybox by doxygen1.2.15