G3D/Math/VEC3D.C

   1 /****************************************************************************************/\r
   2 /*  VEC3D.C                                                                             */\r
   3 /*                                                                                      */\r
   4 /*  Author:                                                                             */\r
   5 /*  Description: 3D Vector implementation                                               */\r
   6 /*                                                                                      */\r
   7 /*  The contents of this file are subject to the Genesis3D Public License               */\r
   8 /*  Version 1.01 (the "License"); you may not use this file except in                   */\r
   9 /*  compliance with the License. You may obtain a copy of the License at                */\r
  10 /*  http://www.genesis3d.com                                                            */\r
  11 /*                                                                                      */\r
  12 /*  Software distributed under the License is distributed on an "AS IS"                 */\r
  13 /*  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied.  See                */\r
  14 /*  the License for the specific language governing rights and limitations              */\r
  15 /*  under the License.                                                                  */\r
  16 /*                                                                                      */\r
  17 /*  The Original Code is Genesis3D, released March 25, 1999.                            */\r
  18 /*Genesis3D Version 1.1 released November 15, 1999                            */\r
  19 /*  Copyright (C) 1999 WildTangent, Inc. All Rights Reserved           */\r
  20 /*                                                                                      */\r
  21 /****************************************************************************************/\r
  22 #include <Math.h>\r
  23 #include <assert.h>\r
  24 #include "Vec3d.h"\r
  25 \r
  26 #define VCOMPARE_EPSILON        (geFloat)0.0005\r
  27 \r
  28 #ifdef NDEBUG\r
  29 #ifdef _MSC_VER\r
  30 #define ASM_NORMALIZE\r
  31 #endif\r
  32 #endif\r
  33 \r
  34 #ifndef NDEBUG\r
  35 GENESISAPI geFloat GENESISCC   geVec3d_GetElement(geVec3d *V, int Index)\r
  36 {\r
  37         assert( V != NULL );\r
  38         assert( Index >= 0 );\r
  39         assert( Index <  3 );\r
  40         return (* ((&((V)->X)) +  (Index) ));\r
  41 }\r
  42 #endif\r
  43 \r
  44 GENESISAPI geBoolean GENESISCC geVec3d_IsValid(const geVec3d *V)\r
  45 {\r
  46         if (V == NULL)\r
  47                 return GE_FALSE;\r
  48         if ((V->X * V->X) < 0.0f) \r
  49                 return GE_FALSE;\r
  50         if ((V->Y * V->Y) < 0.0f) \r
  51                 return GE_FALSE;\r
  52         if ((V->Z * V->Z) < 0.0f) \r
  53                 return GE_FALSE;\r
  54         return GE_TRUE;\r
  55 }\r
  56 \r
  57 \r
  58 GENESISAPI void GENESISCC               geVec3d_Set(geVec3d *V, geFloat X, geFloat Y, geFloat Z)\r
  59 {\r
  60         assert ( V != NULL );\r
  61         V->X = X;\r
  62         V->Y = Y;\r
  63         V->Z = Z;\r
  64         assert( geVec3d_IsValid(V) != GE_FALSE );\r
  65 }\r
  66 \r
  67 GENESISAPI void GENESISCC               geVec3d_Get(const geVec3d *V, geFloat *X, geFloat *Y, geFloat *Z)\r
  68 {\r
  69         assert ( V != NULL );\r
  70         assert ( X != NULL );\r
  71         assert ( Y != NULL );\r
  72         assert ( Z != NULL );\r
  73         assert( geVec3d_IsValid(V) != GE_FALSE );\r
  74         \r
  75         *X = V->X;\r
  76         *Y = V->Y;\r
  77         *Z = V->Z;\r
  78 }\r
  79 \r
  80 \r
  81 GENESISAPI geFloat GENESISCC    geVec3d_DotProduct(const geVec3d *V1, const geVec3d *V2)\r
  82 {\r
  83         assert ( V1 != NULL );\r
  84         assert ( V2 != NULL );\r
  85         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
  86         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
  87         \r
  88         return(V1->X*V2->X + V1->Y*V2->Y + V1->Z*V2->Z);\r
  89 }\r
  90 \r
  91 GENESISAPI void GENESISCC geVec3d_CrossProduct(const geVec3d *V1, const geVec3d *V2, geVec3d *VResult)\r
  92 {\r
  93         geVec3d Result;\r
  94         assert ( V1 != NULL );\r
  95         assert ( V2 != NULL );\r
  96         assert ( VResult != NULL );\r
  97         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
  98         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
  99 \r
 100         Result.X = V1->Y*V2->Z - V1->Z*V2->Y;\r
 101     Result.Y = V1->Z*V2->X - V1->X*V2->Z;\r
 102     Result.Z = V1->X*V2->Y - V1->Y*V2->X;\r
 103 \r
 104         *VResult = Result;\r
 105 }\r
 106 \r
 107 GENESISAPI geBoolean GENESISCC geVec3d_Compare(const geVec3d *V1, const geVec3d *V2, geFloat Tolerance)\r
 108 {\r
 109         assert ( V1 != NULL );\r
 110         assert ( V2 != NULL );\r
 111         assert ( Tolerance >= 0.0 );\r
 112         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 113         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 114 \r
 115         if (fabs(V2->X - V1->X) > Tolerance) //VCOMPARE_EPSILON)\r
 116                 return GE_FALSE;\r
 117         if (fabs(V2->Y - V1->Y) > Tolerance) //VCOMPARE_EPSILON)\r
 118                 return GE_FALSE;\r
 119         if (fabs(V2->Z - V1->Z) > Tolerance) //VCOMPARE_EPSILON)\r
 120                 return GE_FALSE;\r
 121 \r
 122         return GE_TRUE;\r
 123 }\r
 124 \r
 125 #ifdef ASM_NORMALIZE\r
 126 \r
 127 GENESISAPI geFloat GENESISCC geVec3d_Normalize(geVec3d *V1)\r
 128 {\r
 129 geFloat *fPtr;\r
 130 geFloat Dist,OneOverDist;\r
 131         \r
 132         fPtr = (geFloat *)V1;\r
 133         Dist =  (*fPtr) * (*fPtr); fPtr++;\r
 134         Dist += (*fPtr) * (*fPtr); fPtr++;\r
 135         Dist += (*fPtr) * (*fPtr);\r
 136 \r
 137         // Vtune shows the geFloat <-> double conversions\r
 138         // required for the clib sqrt() are taking a lot of time.\r
 139         // hence we use asm to access the geFloat fsqrt() directly\r
 140 \r
 141         __asm \r
 142         {\r
 143                 FLD Dist\r
 144                 FSQRT\r
 145                 FSTP Dist\r
 146         }\r
 147 \r
 148         if ( Dist == 0.0f )\r
 149                 return 0.0f;\r
 150 \r
 151         OneOverDist = 1.0f/Dist;\r
 152                 \r
 153         fPtr = (geFloat *)V1;\r
 154         *fPtr *= OneOverDist; fPtr++;\r
 155         *fPtr *= OneOverDist; fPtr++;\r
 156         *fPtr *= OneOverDist;\r
 157 \r
 158 return (geFloat)Dist;\r
 159 }\r
 160 #else\r
 161 GENESISAPI geFloat GENESISCC geVec3d_Normalize(geVec3d *V1)\r
 162 {\r
 163         geFloat OneOverDist;\r
 164         geFloat Dist;\r
 165 \r
 166         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 167 \r
 168         Dist = (geFloat)sqrt(geVec3d_DotProduct(V1, V1));\r
 169 \r
 170         if (Dist == 0.0)\r
 171                 return 0.0f;\r
 172         OneOverDist = 1.0f/Dist;\r
 173         \r
 174         V1->X *= OneOverDist;\r
 175         V1->Y *= OneOverDist;\r
 176         V1->Z *= OneOverDist;\r
 177 \r
 178         return Dist;\r
 179 }\r
 180 #endif // ASM_NORMALIZE\r
 181 \r
 182 GENESISAPI geBoolean GENESISCC  geVec3d_IsNormalized(const geVec3d *V)\r
 183 {\r
 184         geFloat length;\r
 185 \r
 186         assert( geVec3d_IsValid(V) != GE_FALSE );\r
 187 \r
 188         length = geVec3d_Length(V);\r
 189         if      ((length >= 1.0f - VCOMPARE_EPSILON) && (length <= 1.0f + VCOMPARE_EPSILON))\r
 190                 return GE_TRUE;\r
 191 \r
 192         return GE_FALSE;\r
 193 }\r
 194 \r
 195 GENESISAPI void GENESISCC geVec3d_Scale(const geVec3d *VSrc, geFloat Scale, geVec3d *VDst)\r
 196 {\r
 197         assert ( VDst != NULL );\r
 198         assert( geVec3d_IsValid(VSrc) != GE_FALSE );\r
 199 \r
 200         VDst->X = VSrc->X * Scale;\r
 201         VDst->Y = VSrc->Y * Scale;\r
 202         VDst->Z = VSrc->Z * Scale;\r
 203         assert( geVec3d_IsValid(VDst) != GE_FALSE );\r
 204 }\r
 205 \r
 206 GENESISAPI geFloat GENESISCC geVec3d_LengthSquared(const geVec3d *V1)\r
 207 {       \r
 208         return geVec3d_DotProduct(V1, V1);\r
 209 }\r
 210 \r
 211 GENESISAPI geFloat GENESISCC geVec3d_Length(const geVec3d *V1)\r
 212 {       \r
 213         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 214 \r
 215         return (geFloat)sqrt(geVec3d_DotProduct(V1, V1));\r
 216 }\r
 217 \r
 218 GENESISAPI void GENESISCC geVec3d_Subtract(const geVec3d *V1, const geVec3d *V2, geVec3d *V1MinusV2)\r
 219 {\r
 220         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 221         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 222         assert ( V1MinusV2 != NULL );\r
 223 \r
 224         V1MinusV2->X = V1->X - V2->X;\r
 225         V1MinusV2->Y = V1->Y - V2->Y;\r
 226         V1MinusV2->Z = V1->Z - V2->Z;\r
 227 }\r
 228 \r
 229 GENESISAPI void GENESISCC geVec3d_Add(const geVec3d *V1, const geVec3d *V2, geVec3d *V1PlusV2)\r
 230 {\r
 231         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 232         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 233         assert ( V1PlusV2 != NULL );\r
 234         \r
 235         V1PlusV2->X = V1->X + V2->X;\r
 236         V1PlusV2->Y = V1->Y + V2->Y;\r
 237         V1PlusV2->Z = V1->Z + V2->Z;\r
 238 }\r
 239 \r
 240 GENESISAPI void GENESISCC geVec3d_MA(geVec3d *V1, geFloat Scale, const geVec3d *V2, geVec3d *V1PlusV2Scaled)\r
 241 {\r
 242         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 243         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 244         assert ( V1PlusV2Scaled != NULL );\r
 245         \r
 246         V1PlusV2Scaled->X = V1->X + V2->X*Scale;\r
 247         V1PlusV2Scaled->Y = V1->Y + V2->Y*Scale;\r
 248         V1PlusV2Scaled->Z = V1->Z + V2->Z*Scale;\r
 249 }\r
 250 \r
 251 GENESISAPI void GENESISCC geVec3d_AddScaled(const geVec3d *V1, const geVec3d *V2, geFloat Scale, geVec3d *V1PlusV2Scaled)\r
 252 {\r
 253         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 254         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 255         assert ( V1PlusV2Scaled != NULL );\r
 256         \r
 257         V1PlusV2Scaled->X = V1->X + V2->X*Scale;\r
 258         V1PlusV2Scaled->Y = V1->Y + V2->Y*Scale;\r
 259         V1PlusV2Scaled->Z = V1->Z + V2->Z*Scale;\r
 260 }\r
 261 \r
 262 GENESISAPI void GENESISCC geVec3d_Copy(const geVec3d *VSrc, geVec3d *VDst)\r
 263 {\r
 264         assert ( VDst != NULL );\r
 265         assert( geVec3d_IsValid(VSrc) != GE_FALSE );\r
 266         \r
 267         *VDst = *VSrc;\r
 268 }\r
 269 \r
 270 GENESISAPI void GENESISCC geVec3d_Clear(geVec3d *V)\r
 271 {\r
 272         assert ( V != NULL );\r
 273         \r
 274         V->X = 0.0f;\r
 275         V->Y = 0.0f;\r
 276         V->Z = 0.0f;\r
 277 }\r
 278 \r
 279 GENESISAPI void GENESISCC geVec3d_Inverse(geVec3d *V)\r
 280 {\r
 281         assert( geVec3d_IsValid(V) != GE_FALSE );\r
 282         \r
 283         V->X = -V->X;\r
 284         V->Y = -V->Y;\r
 285         V->Z = -V->Z;\r
 286 }\r
 287 \r
 288 GENESISAPI geFloat GENESISCC            geVec3d_DistanceBetween(const geVec3d *V1, const geVec3d *V2)   // returns length of V1-V2      \r
 289 {\r
 290         geVec3d B;\r
 291         \r
 292         assert( geVec3d_IsValid(V1) != GE_FALSE );\r
 293         assert( geVec3d_IsValid(V2) != GE_FALSE );\r
 294 \r
 295         geVec3d_Subtract(V1,V2,&B);\r
 296         return geVec3d_Length(&B);\r
 297 }\r
 298 \r