00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "ExQuaternion.h"
00025
00026 ExQuaternion::ExQuaternion()
00027 {
00028 SetQuaternion(1,0,0,0);
00029 }
00030
00031 ExQuaternion::ExQuaternion(float w,float x,float y,float z)
00032 {
00033 SetQuaternion(w,x,y,z);
00034 }
00035
00036 ExQuaternion::~ExQuaternion()
00037 {
00038
00039 }
00040
00042
00044 void ExQuaternion::SetQuaternion(float w,float x,float y,float z)
00045 {
00046 qw=w;
00047 qx=x;
00048 qy=y;
00049 qz=z;
00050 }
00051
00052 void ExQuaternion::Conjugate(void)
00053 {
00054
00055 }
00056
00057 void ExQuaternion::LoadIdentity(void)
00058 {
00059 qx=-qx;
00060 qy=-qy;
00061 qz=-qz;
00062 }
00063 float ExQuaternion::GetMagnitude(void)
00064 {
00065 return(sqrt(qw*qw+qx*qx+qy*qy+qz*qz));
00066
00067 }
00068 void ExQuaternion::Normalize(void)
00069 {
00070 float factor=GetMagnitude();
00071 float scaleby(1.0/sqrt(factor));
00072 qw=qw*scaleby;
00073 qx=qx*scaleby;
00074 qy=qy*scaleby;
00075 qz=qz*scaleby;
00076 }
00077
00078 void ExQuaternion::SetEuler(float yaw,float pitch,float roll)
00079 {
00080 float cosY = cosf(yaw/2.0f);
00081 float sinY = sinf(yaw/2.0f);
00082 float cosP = cosf(pitch/2.0f);
00083 float sinP = sinf(pitch/2.0f);
00084 float cosR = cosf(roll/2.0f);
00085 float sinR = sinf(roll/2.0f);
00086
00087 qw=cosR*sinP*cosY+sinR*cosP*sinY;
00088 qx=cosR*cosP*sinY-sinR*sinP*cosY;
00089 qy=sinR*cosP*cosY-cosR*sinP*sinY;
00090 qz=cosR*cosP*cosY+sinR*sinP*sinY;
00091
00092 }
00093 void ExQuaternion::CreateFromAxisAngle(float X, float Y, float Z, float degree)
00094 {
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 float angle = float((degree / 180.0f) * PI);
00111
00112
00113 float result = (float)sin( angle / 2.0f );
00114
00115
00116 qw = (float)cos( angle / 2.0f );
00117
00118
00119 qx = float(X * result);
00120 qy = float(Y * result);
00121 qz = float(Z * result);
00122 }
00123
00124 void ExQuaternion::CreateMatrix(float *pMatrix)
00125 {
00126
00127 if(!pMatrix) return;
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 pMatrix[ 0] = 1.0f - 2.0f * ( qy * qy + qz * qz );
00160 pMatrix[ 1] = 2.0f * ( qx * qy - qw * qz );
00161 pMatrix[ 2] = 2.0f * ( qx * qz + qw * qy );
00162 pMatrix[ 3] = 0.0f;
00163
00164
00165 pMatrix[ 4] = 2.0f * ( qx * qy + qw * qz );
00166 pMatrix[ 5] = 1.0f - 2.0f * ( qx * qx + qz * qz );
00167 pMatrix[ 6] = 2.0f * ( qy * qz - qw * qx );
00168 pMatrix[ 7] = 0.0f;
00169
00170
00171 pMatrix[ 8] = 2.0f * ( qx * qz - qw * qy );
00172 pMatrix[ 9] = 2.0f * ( qy * qz + qw * qx );
00173 pMatrix[10] = 1.0f - 2.0f * ( qx * qx + qy * qy );
00174 pMatrix[11] = 0.0f;
00175
00176
00177 pMatrix[12] = 0;
00178 pMatrix[13] = 0;
00179 pMatrix[14] = 0;
00180 pMatrix[15] = 1.0f;
00181
00182
00183 }
00184
00186
00188 ExQuaternion ExQuaternion::operator* (const ExQuaternion &Q)
00189 {
00190 ExQuaternion result;
00191 result.qw=(Q.qw*qw)-(Q.qx*qx)-(Q.qy*qy)-(Q.qz*qz);
00192 result.qx=(Q.qw*qx)-(Q.qx*qw)-(Q.qy*qz)-(Q.qz*qy);
00193 result.qy=(Q.qw*qy)-(Q.qy*qw)-(Q.qz*qx)-(Q.qx*qz);
00194 result.qz=(Q.qw*qz)-(Q.qz*qw)-(Q.qx*qy)-(Q.qz*qx);
00195 return result;
00196 }
00197 ExQuaternion ExQuaternion::operator+ (const ExQuaternion &Q)
00198 {
00199 return ExQuaternion(qw+Q.qw,qx+Q.qx,qy+Q.qy,qz+Q.qz);
00200 }
00202
00204
00205 std::ostream& operator<<(std::ostream& s,const ExQuaternion &q)
00206 {
00207 Guard(friend std::ostream& operator<<(std::ostream& s,const ExQuaternion &q))
00208 s<<"W:"<<q.qw<<"X:"<<q.qx<<"Y:"<<q.qy<<"Z:"<<q.qz<<std::endl;
00209 return s;
00210 UnGuard
00211 }