Page principale   Liste des namespaces   Hiérarchie des classes   Liste par ordre alphabétique   Liste des composants   Liste des fichiers   Membres des namespaces   Composants   Déclarations  

ExMath.cpp

Aller à la documentation de ce fichier.
00001 /*
00002  * ExNihilo 3D Engine
00003  * 
00004  *  This program is free software; you can redistribute it and/or modify
00005  *  it under the terms of the GNU General Public License as published by
00006  *  the Free Software Foundation; either version 2 of the License, or
00007  *  (at your option) any later version.
00008  *
00009  *  This program is distributed in the hope that it will be useful,
00010  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *  GNU Library General Public License for more details.
00013  *
00014  *  You should have received a copy of the GNU General Public License
00015  *  along with this program; if not, write to the Free Software
00016  *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017  *
00018  * Please read AUTHORS file !!!
00019  * 
00020  * $Id: ExMath.cpp,v 1.8 2002/07/21 18:49:33 binny Exp $
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         // calculate coefficients
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         //cout<<"cos roll "<<A<<endl;
00140         B=Sin[(int)roll];
00141         //cout<<"sin roll "<<B<<endl;
00142         C=Cos[(int)pitch];
00143         //cout<<"cos pitch "<<C<<endl;
00144         D=Sin[(int)pitch];
00145         //cout<<"sin pitch "<<D<<endl;
00146         E=Cos[(int)yaw];
00147         //cout<<"cos yaw "<<E<<endl;
00148         F=Sin[(int)yaw];
00149         //cout<<"sin yaw "<<F<<endl;
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         //roll angle x
00207         //pitch angle y
00208         //yaw angle z
00209 
00210         // calcul new vec pour x
00211         /*VecX.m_Vector[0]=force.m_Vector[0]*cos(pitch);
00212         VecX.m_Vector[1]=0.0f;
00213         VecX.m_Vector[2]=force.m_Vector[0]*sin(pitch);
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         //std::cout<<"leng of result:"<<Result.GetVectorLenght()<<std::endl;
00226         //std::cout<<"leng of force:"<<force.GetVectorLenght()<<std::endl;
00227 
00228         //std::cout<<"Result:"<<Result<<std::endl;
00229         //std::cout<<"Force:"<<force<<std::endl;
00230         return Result;
00231 }
00232 

Généré le Tue Dec 10 18:18:12 2002 pour ExNihilo par doxygen1.3-rc1