GTestAndEngine: VEC3D.C Source File

00001 /****************************************************************************************/
00002 /*  VEC3D.C                                                                             */
00003 /*                                                                                      */
00004 /*  Author:                                                                             */
00005 /*  Description: 3D Vector implementation                                               */
00006 /*                                                                                      */
00007 /*  The contents of this file are subject to the Genesis3D Public License               */
00008 /*  Version 1.01 (the "License"); you may not use this file except in                   */
00009 /*  compliance with the License. You may obtain a copy of the License at                */
00010 /*  http://www.genesis3d.com                                                            */
00011 /*                                                                                      */
00012 /*  Software distributed under the License is distributed on an "AS IS"                 */
00013 /*  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied.  See                */
00014 /*  the License for the specific language governing rights and limitations              */
00015 /*  under the License.                                                                  */
00016 /*                                                                                      */
00017 /*  The Original Code is Genesis3D, released March 25, 1999.                            */
00018 /*Genesis3D Version 1.1 released November 15, 1999                            */
00019 /*  Copyright (C) 1999 WildTangent, Inc. All Rights Reserved           */
00020 /*                                                                                      */
00021 /****************************************************************************************/
00022 #include <Math.h>
00023 #include <assert.h>
00024 #include "Vec3d.h"
00025 
00026 #define VCOMPARE_EPSILON        (geFloat)0.0005
00027 
00028 #ifdef NDEBUG
00029 #ifdef _MSC_VER
00030 #define ASM_NORMALIZE
00031 #endif
00032 #endif
00033 
00034 #ifndef NDEBUG
00035 GENESISAPI geFloat GENESISCC   geVec3d_GetElement(geVec3d *V, int Index)
00036 {
00037         assert( V != NULL );
00038         assert( Index >= 0 );
00039         assert( Index <  3 );
00040         return (* ((&((V)->X)) +  (Index) ));
00041 }
00042 #endif
00043 
00044 GENESISAPI geBoolean GENESISCC geVec3d_IsValid(const geVec3d *V)
00045 {
00046         if (V == NULL)
00047                 return GE_FALSE;
00048         if ((V->X * V->X) < 0.0f) 
00049                 return GE_FALSE;
00050         if ((V->Y * V->Y) < 0.0f) 
00051                 return GE_FALSE;
00052         if ((V->Z * V->Z) < 0.0f) 
00053                 return GE_FALSE;
00054         return GE_TRUE;
00055 }
00056 
00057 
00058 GENESISAPI void GENESISCC               geVec3d_Set(geVec3d *V, geFloat X, geFloat Y, geFloat Z)
00059 {
00060         assert ( V != NULL );
00061         V->X = X;
00062         V->Y = Y;
00063         V->Z = Z;
00064         assert( geVec3d_IsValid(V) != GE_FALSE );
00065 }
00066 
00067 GENESISAPI void GENESISCC               geVec3d_Get(const geVec3d *V, geFloat *X, geFloat *Y, geFloat *Z)
00068 {
00069         assert ( V != NULL );
00070         assert ( X != NULL );
00071         assert ( Y != NULL );
00072         assert ( Z != NULL );
00073         assert( geVec3d_IsValid(V) != GE_FALSE );
00074         
00075         *X = V->X;
00076         *Y = V->Y;
00077         *Z = V->Z;
00078 }
00079 
00080 
00081 GENESISAPI geFloat GENESISCC    geVec3d_DotProduct(const geVec3d *V1, const geVec3d *V2)
00082 {
00083         assert ( V1 != NULL );
00084         assert ( V2 != NULL );
00085         assert( geVec3d_IsValid(V1) != GE_FALSE );
00086         assert( geVec3d_IsValid(V2) != GE_FALSE );
00087         
00088         return(V1->X*V2->X + V1->Y*V2->Y + V1->Z*V2->Z);
00089 }
00090 
00091 GENESISAPI void GENESISCC geVec3d_CrossProduct(const geVec3d *V1, const geVec3d *V2, geVec3d *VResult)
00092 {
00093         geVec3d Result;
00094         assert ( V1 != NULL );
00095         assert ( V2 != NULL );
00096         assert ( VResult != NULL );
00097         assert( geVec3d_IsValid(V1) != GE_FALSE );
00098         assert( geVec3d_IsValid(V2) != GE_FALSE );
00099 
00100         Result.X = V1->Y*V2->Z - V1->Z*V2->Y;
00101     Result.Y = V1->Z*V2->X - V1->X*V2->Z;
00102     Result.Z = V1->X*V2->Y - V1->Y*V2->X;
00103 
00104         *VResult = Result;
00105 }
00106 
00107 GENESISAPI geBoolean GENESISCC geVec3d_Compare(const geVec3d *V1, const geVec3d *V2, geFloat Tolerance)
00108 {
00109         assert ( V1 != NULL );
00110         assert ( V2 != NULL );
00111         assert ( Tolerance >= 0.0 );
00112         assert( geVec3d_IsValid(V1) != GE_FALSE );
00113         assert( geVec3d_IsValid(V2) != GE_FALSE );
00114 
00115         if (fabs(V2->X - V1->X) > Tolerance) //VCOMPARE_EPSILON)
00116                 return GE_FALSE;
00117         if (fabs(V2->Y - V1->Y) > Tolerance) //VCOMPARE_EPSILON)
00118                 return GE_FALSE;
00119         if (fabs(V2->Z - V1->Z) > Tolerance) //VCOMPARE_EPSILON)
00120                 return GE_FALSE;
00121 
00122         return GE_TRUE;
00123 }
00124 
00125 #ifdef ASM_NORMALIZE
00126 
00127 GENESISAPI geFloat GENESISCC geVec3d_Normalize(geVec3d *V1)
00128 {
00129 geFloat *fPtr;
00130 geFloat Dist,OneOverDist;
00131         
00132         fPtr = (geFloat *)V1;
00133         Dist =  (*fPtr) * (*fPtr); fPtr++;
00134         Dist += (*fPtr) * (*fPtr); fPtr++;
00135         Dist += (*fPtr) * (*fPtr);
00136 
00137         // Vtune shows the geFloat <-> double conversions
00138         // required for the clib sqrt() are taking a lot of time.
00139         // hence we use asm to access the geFloat fsqrt() directly
00140 
00141         __asm 
00142         {
00143                 FLD Dist
00144                 FSQRT
00145                 FSTP Dist
00146         }
00147 
00148         if ( Dist == 0.0f )
00149                 return 0.0f;
00150 
00151         OneOverDist = 1.0f/Dist;
00152                 
00153         fPtr = (geFloat *)V1;
00154         *fPtr *= OneOverDist; fPtr++;
00155         *fPtr *= OneOverDist; fPtr++;
00156         *fPtr *= OneOverDist;
00157 
00158 return (geFloat)Dist;
00159 }
00160 #else
00161 GENESISAPI geFloat GENESISCC geVec3d_Normalize(geVec3d *V1)
00162 {
00163         geFloat OneOverDist;
00164         geFloat Dist;
00165 
00166         assert( geVec3d_IsValid(V1) != GE_FALSE );
00167 
00168         Dist = (geFloat)sqrt(geVec3d_DotProduct(V1, V1));
00169 
00170         if (Dist == 0.0)
00171                 return 0.0f;
00172         OneOverDist = 1.0f/Dist;
00173         
00174         V1->X *= OneOverDist;
00175         V1->Y *= OneOverDist;
00176         V1->Z *= OneOverDist;
00177 
00178         return Dist;
00179 }
00180 #endif // ASM_NORMALIZE
00181 
00182 GENESISAPI geBoolean GENESISCC  geVec3d_IsNormalized(const geVec3d *V)
00183 {
00184         geFloat length;
00185 
00186         assert( geVec3d_IsValid(V) != GE_FALSE );
00187 
00188         length = geVec3d_Length(V);
00189         if      ((length >= 1.0f - VCOMPARE_EPSILON) && (length <= 1.0f + VCOMPARE_EPSILON))
00190                 return GE_TRUE;
00191 
00192         return GE_FALSE;
00193 }
00194 
00195 GENESISAPI void GENESISCC geVec3d_Scale(const geVec3d *VSrc, geFloat Scale, geVec3d *VDst)
00196 {
00197         assert ( VDst != NULL );
00198         assert( geVec3d_IsValid(VSrc) != GE_FALSE );
00199 
00200         VDst->X = VSrc->X * Scale;
00201         VDst->Y = VSrc->Y * Scale;
00202         VDst->Z = VSrc->Z * Scale;
00203         assert( geVec3d_IsValid(VDst) != GE_FALSE );
00204 }
00205 
00206 GENESISAPI geFloat GENESISCC geVec3d_LengthSquared(const geVec3d *V1)
00207 {       
00208         return geVec3d_DotProduct(V1, V1);
00209 }
00210 
00211 GENESISAPI geFloat GENESISCC geVec3d_Length(const geVec3d *V1)
00212 {       
00213         assert( geVec3d_IsValid(V1) != GE_FALSE );
00214 
00215         return (geFloat)sqrt(geVec3d_DotProduct(V1, V1));
00216 }
00217 
00218 GENESISAPI void GENESISCC geVec3d_Subtract(const geVec3d *V1, const geVec3d *V2, geVec3d *V1MinusV2)
00219 {
00220         assert( geVec3d_IsValid(V1) != GE_FALSE );
00221         assert( geVec3d_IsValid(V2) != GE_FALSE );
00222         assert ( V1MinusV2 != NULL );
00223 
00224         V1MinusV2->X = V1->X - V2->X;
00225         V1MinusV2->Y = V1->Y - V2->Y;
00226         V1MinusV2->Z = V1->Z - V2->Z;
00227 }
00228 
00229 GENESISAPI void GENESISCC geVec3d_Add(const geVec3d *V1, const geVec3d *V2, geVec3d *V1PlusV2)
00230 {
00231         assert( geVec3d_IsValid(V1) != GE_FALSE );
00232         assert( geVec3d_IsValid(V2) != GE_FALSE );
00233         assert ( V1PlusV2 != NULL );
00234         
00235         V1PlusV2->X = V1->X + V2->X;
00236         V1PlusV2->Y = V1->Y + V2->Y;
00237         V1PlusV2->Z = V1->Z + V2->Z;
00238 }
00239 
00240 GENESISAPI void GENESISCC geVec3d_MA(geVec3d *V1, geFloat Scale, const geVec3d *V2, geVec3d *V1PlusV2Scaled)
00241 {
00242         assert( geVec3d_IsValid(V1) != GE_FALSE );
00243         assert( geVec3d_IsValid(V2) != GE_FALSE );
00244         assert ( V1PlusV2Scaled != NULL );
00245         
00246         V1PlusV2Scaled->X = V1->X + V2->X*Scale;
00247         V1PlusV2Scaled->Y = V1->Y + V2->Y*Scale;
00248         V1PlusV2Scaled->Z = V1->Z + V2->Z*Scale;
00249 }
00250 
00251 GENESISAPI void GENESISCC geVec3d_AddScaled(const geVec3d *V1, const geVec3d *V2, geFloat Scale, geVec3d *V1PlusV2Scaled)
00252 {
00253         assert( geVec3d_IsValid(V1) != GE_FALSE );
00254         assert( geVec3d_IsValid(V2) != GE_FALSE );
00255         assert ( V1PlusV2Scaled != NULL );
00256         
00257         V1PlusV2Scaled->X = V1->X + V2->X*Scale;
00258         V1PlusV2Scaled->Y = V1->Y + V2->Y*Scale;
00259         V1PlusV2Scaled->Z = V1->Z + V2->Z*Scale;
00260 }
00261 
00262 GENESISAPI void GENESISCC geVec3d_Copy(const geVec3d *VSrc, geVec3d *VDst)
00263 {
00264         assert ( VDst != NULL );
00265         assert( geVec3d_IsValid(VSrc) != GE_FALSE );
00266         
00267         *VDst = *VSrc;
00268 }
00269 
00270 GENESISAPI void GENESISCC geVec3d_Clear(geVec3d *V)
00271 {
00272         assert ( V != NULL );
00273         
00274         V->X = 0.0f;
00275         V->Y = 0.0f;
00276         V->Z = 0.0f;
00277 }
00278 
00279 GENESISAPI void GENESISCC geVec3d_Inverse(geVec3d *V)
00280 {
00281         assert( geVec3d_IsValid(V) != GE_FALSE );
00282         
00283         V->X = -V->X;
00284         V->Y = -V->Y;
00285         V->Z = -V->Z;
00286 }
00287 
00288 GENESISAPI geFloat GENESISCC            geVec3d_DistanceBetween(const geVec3d *V1, const geVec3d *V2)   // returns length of V1-V2      
00289 {
00290         geVec3d B;
00291         
00292         assert( geVec3d_IsValid(V1) != GE_FALSE );
00293         assert( geVec3d_IsValid(V2) != GE_FALSE );
00294 
00295         geVec3d_Subtract(V1,V2,&B);
00296         return geVec3d_Length(&B);
00297 }
00298