Al's Programming Resource Homepage  Main Page   Namespace List   Class Hierarchy   Compound List   File List   Compound Members   File Members  

locmath.h File Reference

#include "shared.h"
#include "vector.h"
#include "vertex.h"
#include "quat.h"

Go to the source code of this file.

Functions

void LoadIdentity (float m[])
void CopyMatrix (float m[], float n[])
void MultMatrix (float m[], float n[])
void MatrixInverse (float m[])
QUAT AxisAngleToMatrix (VECTOR axis, float theta, float m[16])
float DotProduct (VECTOR vec1, VECTOR vec2)
VECTOR CrossVector (VECTOR vec1, VECTOR vec2)
void EulerToQuat (float roll, float pitch, float yaw, QUAT *quat)
float MagnitudeQuat (QUAT q1)
QUAT NormaliseQuat (QUAT q1)
void QuatToMatrix (QUAT quat, float m[16])
QUAT MultQuat (QUAT q1, QUAT q2)
VERTEX GetNorm (float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3)
float MagnitudeVector (VECTOR vec1)
VECTOR GetUnitVector (VECTOR vector)
VECTOR GetEdgeVector (VECTOR point1, VECTOR point2)

Function Documentation

QUAT AxisAngleToMatrix (  VECTOR    axis,
float    theta,
float    m[16]
) 
 

Definition at line 165 of file locmath.cpp.

References QUAT::w, VECTOR::x, QUAT::x, VECTOR::y, QUAT::y, VECTOR::z, and QUAT::z. 

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 }

void CopyMatrix (  float    m[],
float    n[]
) 
 

Definition at line 30 of file locmath.cpp.

Referenced by MatrixInverse(), and MultMatrix(). 

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 }

VECTOR CrossVector (  VECTOR    vec1,
VECTOR    vec2
) 
 

Definition at line 210 of file locmath.cpp.

References VECTOR::x, VECTOR::y, and VECTOR::z.

Referenced by MultQuat(). 

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 }

float DotProduct (  VECTOR    vec1,
VECTOR    vec2
) 
 

Definition at line 195 of file locmath.cpp.

References VECTOR::x, VECTOR::y, and VECTOR::z.

Referenced by MultQuat(). 

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 }

void EulerToQuat (  float    roll,
float    pitch,
float    yaw,
QUAT *    quat
) 
 

Definition at line 230 of file locmath.cpp.

References QUAT::w, QUAT::x, QUAT::y, and QUAT::z. 

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 }

VECTOR GetEdgeVector (  VECTOR    point1,
VECTOR    point2
) 
 

Definition at line 391 of file locmath.cpp.

References VECTOR::x, VECTOR::y, and VECTOR::z. 

00392 {
00393   VECTOR temp_vector;
00394   temp_vector.x = point1.x - point2.x;
00395   temp_vector.y = point1.y - point2.y;
00396   temp_vector.z = point1.z - point2.z;
00397   return temp_vector;
00398 }

VERTEX GetNorm (  float    x1,
float    y1,
float    z1,
float    x2,
float    y2,
float    z2,
float    x3,
float    y3,
float    z3
) 
 

Definition at line 342 of file locmath.cpp.

References VERTEX::nx, VERTEX::ny, and VERTEX::nz. 

00343 {
00344         float ux;
00345         float uy;
00346         float uz;
00347         float vx;
00348         float vy;
00349         float vz;
00350           VERTEX temp_vertex;
00351           ux = x1 - x2;
00352           uy = y1 - y2;
00353           uz = z1 - z2;
00354           vx = x3 - x2;
00355           vy = y3 - y2;
00356           vz = z3 - z2;
00357           temp_vertex.nx = (uy*vz)-(vy*uz);
00358           temp_vertex.ny = (uz*vx)-(vz*ux);
00359           temp_vertex.nz = (ux*vy)-(vx*uy);
00360           return temp_vertex;
00361 }

VECTOR GetUnitVector (  VECTOR    vector ) 
 

Definition at line 368 of file locmath.cpp.

References VECTOR::x, VECTOR::y, and VECTOR::z. 

00369 {
00370         // Reduces a normal vector specified as a set of three coordinates,
00371         // to a unit normal vector of length one.
00372 
00373         // Calculate the length of the vector           
00374         float length = (float) sqrt(( vector.x * vector.x) + 
00375                                                         ( vector.y * vector.y) +
00376                                                         ( vector.z * vector.z) );
00377 
00378         // Keep the program from blowing up by providing an exceptable
00379         // value for vectors that may calculated too close to zero.
00380         if(length == 0.0f)
00381                 length = 1.0f;
00382 
00383         // Dividing each element by the length will result in a
00384         // unit normal vector.
00385         vector.x /= length;
00386         vector.y /= length;
00387         vector.z /= length;
00388         return vector;
00389 }

void LoadIdentity (  float    m[] ) 
 

Definition at line 7 of file locmath.cpp. 

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 }

float MagnitudeQuat (  QUAT    q1 ) 
 

Definition at line 256 of file locmath.cpp.

References QUAT::w, QUAT::x, QUAT::y, and QUAT::z.

Referenced by NormaliseQuat(). 

00257 {
00258       return( sqrt(q1.w*q1.w+q1.x*q1.x+q1.y*q1.y+q1.z*q1.z));
00259 }

float MagnitudeVector (  VECTOR    vec1 ) 
 

Definition at line 363 of file locmath.cpp.

References VECTOR::x, VECTOR::y, and VECTOR::z. 

00364 {
00365   return(sqrt(vec1.x*vec1.x+vec1.y*vec1.y+vec1.z*vec1.z));
00366 }

void MatrixInverse (  float    m[] ) 
 

Definition at line 136 of file locmath.cpp.

References CopyMatrix(). 

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 }

void MultMatrix (  float    m[],
float    n[]
) 
 

Definition at line 50 of file locmath.cpp.

References CopyMatrix(). 

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 }

QUAT MultQuat (  QUAT    q1,
QUAT    q2
) 
 

Definition at line 307 of file locmath.cpp.

References CrossVector(), DotProduct(), NormaliseQuat(), QUAT::w, QUAT::x, VECTOR::x, QUAT::y, VECTOR::y, QUAT::z, and VECTOR::z. 

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 }

QUAT NormaliseQuat (  QUAT    q1 ) 
 

Definition at line 261 of file locmath.cpp.

References MagnitudeQuat(), QUAT::w, QUAT::x, QUAT::y, and QUAT::z.

Referenced by MultQuat(). 

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 }

void QuatToMatrix (  QUAT    quat,
float    m[16]
) 
 

Definition at line 273 of file locmath.cpp.

References QUAT::w, QUAT::x, QUAT::y, and QUAT::z. 

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 }

Generated on Fri Dec 23 05:22:37 2005 for Timing by doxygen1.2.15