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 "ExMath.h"
00025
00026 ExCMatrix4x4 MatriceByVec3D(const ExCMatrix4x4 &m,const ExCVec3D &v)
00027 {
00028 ExCMatrix4x4 RetMat;
00029 return RetMat;
00030 }
00031
00032 float GetDotProduct(const ExCVec3D& Vec1,const ExCVec3D& Vec2)
00033 {
00034 return((Vec1.m_Vector[0]*Vec2.m_Vector[0])+(Vec1.m_Vector[1]*Vec2.m_Vector[1])+(Vec1.m_Vector[2]*Vec2.m_Vector[2]));
00035 }
00036
00037 ExCVec3D GetCrossProduct(const ExCVec3D& Vec1,const ExCVec3D& Vec2)
00038 {
00039 ExCVec3D CrossProduct;
00040 CrossProduct.m_Vector[0]=((Vec1.m_Vector[1]*Vec2.m_Vector[2])-(Vec1.m_Vector[2]*Vec2.m_Vector[1]));
00041 CrossProduct.m_Vector[1]=((Vec1.m_Vector[2]*Vec2.m_Vector[0])-(Vec1.m_Vector[0]*Vec2.m_Vector[2]));
00042 CrossProduct.m_Vector[2]=((Vec1.m_Vector[0]*Vec2.m_Vector[1])-(Vec1.m_Vector[1]*Vec2.m_Vector[0]));
00043 return CrossProduct;
00044 }
00045
00046 ExCVec3D GetVecNormale(const ExCVec3D& Vec1)
00047 {
00048 ExCVec3D VecNorm;
00049 VecNorm=Vec1;
00050 VecNorm=VecNorm/VecNorm.GetVectorLenght();
00051 return VecNorm;
00052 }
00053
00054 ExCMatrix4x4 GetMatrixFromQuaternion(const ExQuaternion& Q)
00055 {
00056 ExCMatrix4x4 RetMatrix;
00057
00058 float w=Q.qw;
00059 float x=Q.qx;
00060 float y=Q.qy;
00061 float z=Q.qz;
00062
00063
00064 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
00065
00066
00067
00068 x2 = Q.qx + Q.qx; y2 = Q.qy + Q.qy;
00069 z2 = Q.qz + Q.qz;
00070 xx = Q.qx * x2; xy = Q.qx * y2; xz = Q.qx * z2;
00071 yy = Q.qy * y2; yz = Q.qy * z2; zz = Q.qz * z2;
00072 wx = Q.qw * x2; wy = Q.qw * y2; wz = Q.qw * z2;
00073
00074
00075 RetMatrix.m_Matrix[0] = 1.0 - (yy + zz);
00076 RetMatrix.m_Matrix[1] = xy - wz;
00077 RetMatrix.m_Matrix[2] = xz + wy;
00078 RetMatrix.m_Matrix[3] = 0.0;
00079
00080 RetMatrix.m_Matrix[4] = xy + wz;
00081 RetMatrix.m_Matrix[5] = 1.0 - (xx + zz);
00082 RetMatrix.m_Matrix[6] = yz - wx;
00083 RetMatrix.m_Matrix[7] = 0.0;
00084
00085
00086 RetMatrix.m_Matrix[8] = xz - wy;
00087 RetMatrix.m_Matrix[9] = yz + wx;
00088 RetMatrix.m_Matrix[10] = 1.0 - (xx + yy);
00089 RetMatrix.m_Matrix[11] = 0.0;
00090
00091
00092 RetMatrix.m_Matrix[12] = 0;
00093 RetMatrix.m_Matrix[13] = 0;
00094 RetMatrix.m_Matrix[14] = 0;
00095 RetMatrix.m_Matrix[15] = 1;
00096
00097
00098 return RetMatrix;
00099 }
00100
00101 ExQuaternion GetQuaternionFromEuler(float x,float y,float z)
00102 {
00103 ExQuaternion QResult;
00104 double roll= DegreesToRadians(x);
00105 double pitch = DegreesToRadians(y);
00106 double yaw = DegreesToRadians(z);
00107
00108 double cyaw,cpitch,croll,syaw,spitch,sroll;
00109 double cyawcpitch,syawspitch,cyawspitch,syawcpitch;
00110
00111 cyaw = cos(0.5f * yaw);
00112 cpitch = cos(0.5f * pitch);
00113 croll = cos(0.5f * roll);
00114 syaw = sin(0.5f * yaw);
00115 spitch = sin(0.5f * pitch);
00116 sroll = sin(0.5f * roll);
00117
00118 cyawcpitch = cyaw*cpitch;
00119 syawspitch = syaw*pitch;
00120 cyawspitch = cyaw*spitch;
00121 syawcpitch = syaw*cpitch;
00122
00123
00124 QResult.qw=(float)(cyawcpitch * croll + syawspitch * sroll);
00125 QResult.qx=(float)(cyawcpitch * sroll - syawspitch * croll);
00126 QResult.qy=(float)(cyawspitch * croll + syawcpitch * sroll);
00127 QResult.qz=(float)(cyawcpitch * croll - cyawspitch * sroll);
00128 return QResult;
00129 }
00130
00131 ExCMatrix4x4 GetMatrixFromEuler(float roll,float pitch,float yaw)
00132 {
00133 Guard(ExCMatrix4x4 GetMatrixFromEuler(float roll,float pitch,float yaw))
00134 ExCMatrix4x4 RetMatrix;
00135
00136 float A,B,C,D,E,F,AD,BD;
00137
00138 A=Cos[(int)roll];
00139
00140 B=Sin[(int)roll];
00141
00142 C=Cos[(int)pitch];
00143
00144 D=Sin[(int)pitch];
00145
00146 E=Cos[(int)yaw];
00147
00148 F=Sin[(int)yaw];
00149
00150
00151 AD=A*D;
00152 BD=B*D;
00153
00154
00155 RetMatrix.m_Matrix[0] = C*E;
00156 RetMatrix.m_Matrix[1] =-C*F;
00157 RetMatrix.m_Matrix[2] =-D;
00158 RetMatrix.m_Matrix[3] =0.0;
00159
00160 RetMatrix.m_Matrix[4] =-BD * E + A * F;
00161 RetMatrix.m_Matrix[5] = BD * F + A * E;
00162 RetMatrix.m_Matrix[6] =-B * C;
00163 RetMatrix.m_Matrix[7] =0.0;
00164
00165 RetMatrix.m_Matrix[8] = AD * E + B * F;
00166 RetMatrix.m_Matrix[9] =-AD * F + B * E;
00167 RetMatrix.m_Matrix[10]= A * C;
00168 RetMatrix.m_Matrix[11]=0.0;
00169
00170 RetMatrix.m_Matrix[12]=0.0;
00171 RetMatrix.m_Matrix[13]=0.0;
00172 RetMatrix.m_Matrix[14]=0.0;
00173 RetMatrix.m_Matrix[15]=1.0;
00174
00175 return RetMatrix;
00176 UnGuard
00177 }
00178
00179 void NormalizePlane(float Plane[6][4], int side)
00180 {
00181 float magnitude = (float)sqrt( Plane[side][0] * Plane[side][0] +
00182 Plane[side][1] * Plane[side][1] +
00183 Plane[side][2] * Plane[side][2] );
00184 Plane[side][0] /= magnitude;
00185 Plane[side][1] /= magnitude;
00186 Plane[side][2] /= magnitude;
00187 Plane[side][3] /= magnitude;
00188 }
00189
00190 ExCVec3D GetAxisFromQuaternion(const ExQuaternion& Q)
00191 {
00192 ExCVec3D vec;
00193 float m;
00194 vec.SetValue(Q.qx,Q.qy,Q.qz);
00195 m=vec.GetVectorLenght();
00196
00197 return vec/m;
00198
00199
00200 }
00201
00202 ExCVec3D GetNewVecFromEuler(ExCVec3D force,float roll,float pitch,float yaw)
00203 {
00204 ExCVec3D Result,VecX,VecY,VecZ;
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 VecY.m_Vector[0]=0.0f;
00216 VecY.m_Vector[1]=force.m_Vector[1]*sin(roll);
00217 VecY.m_Vector[2]=force.m_Vector[1]*cos(roll);
00218
00219 VecZ.m_Vector[0]=force.m_Vector[2]*cos(yaw);
00220 VecZ.m_Vector[1]=force.m_Vector[2]*sin(yaw);
00221 VecZ.m_Vector[2]=0.0f;
00222
00223 Result=VecX+VecY+VecZ;
00224
00225
00226
00227
00228
00229
00230 return Result;
00231 }
00232