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;
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
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
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 }