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 }