Source/Quaternions.h

   1 /*
   2 Copyright (C) 2003, 2010 - Wolfire Games
   3
   4 This file is part of Lugaru.
   5
   6 Lugaru is free software; you can redistribute it and/or
   7 modify it under the terms of the GNU General Public License
   8 as published by the Free Software Foundation; either version 2
   9 of the License, or (at your option) any later version.
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  14
  15 See the GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with this program; if not, write to the Free Software
  19 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  20 */
  21
  22
  23 #ifndef _QUATERNIONS_H_
  24 #define _QUATERNIONS_H_
  25
  26 #ifndef WIN32
  27 #pragma mark -
  28 #endif
  29
  30 //#include "Carbon.h"
  31 #include "math.h"
  32 #include "PhysicsMath.h"
  33 #include "gamegl.h"
  34
  35 /**> Quaternion Structures <**/
  36 #define PI      3.14159265355555897932384626
  37 #define RADIANS 0
  38 #define DEGREES 1
  39 #define deg2rad .0174532925
  40
  41 //using namespace std;
  42 typedef float Matrix_t [4][4];
  43 struct euler
  44 {
  45         float x, y, z;
  46 };
  47 struct angle_axis
  48 {
  49         float x, y, z, angle;
  50 };
  51 struct quaternion
  52 {
  53         float x, y, z, w;
  54 };
  55
  56 class XYZ{
  57 public:
  58         float x;
  59         float y;
  60         float z;
  61     XYZ() : x(0.0f), y(0.0f), z(0.0f) {}
  62         inline XYZ operator+(XYZ add);
  63         inline XYZ operator-(XYZ add);
  64         inline XYZ operator*(float add);
  65         inline XYZ operator*(XYZ add);
  66         inline XYZ operator/(float add);
  67         inline void operator+=(XYZ add);
  68         inline void operator-=(XYZ add);
  69         inline void operator*=(float add);
  70         inline void operator*=(XYZ add);
  71         inline void operator/=(float add);
  72         inline void operator=(float add);
  73         inline void vec(Vector add);
  74         inline bool operator==(XYZ add);
  75 };
  76
  77 /*********************> Quaternion Function definition <********/
  78 quaternion To_Quat(int Degree_Flag, euler Euler);
  79 quaternion To_Quat(angle_axis Ang_Ax);
  80 quaternion To_Quat(Matrix_t m);
  81 angle_axis Quat_2_AA(quaternion Quat);
  82 void Quat_2_Matrix(quaternion Quat, Matrix_t m);
  83 quaternion Normalize(quaternion Quat);
  84 quaternion Quat_Mult(quaternion q1, quaternion q2);
  85 quaternion QNormalize(quaternion Quat);
  86 XYZ Quat2Vector(quaternion Quat);
  87
  88 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V);
  89 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V);
  90 inline void Normalise(XYZ *vectory);
  91 inline float normaldotproduct(XYZ point1, XYZ point2);
  92 inline float fast_sqrt (register float arg);
  93 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3);
  94 bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
  95 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
  96 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ n, XYZ *p);
  97 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc,XYZ *n, XYZ *p);
  98 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *p);
  99 bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33);
 100 bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p);
 101 inline void ReflectVector(XYZ *vel, XYZ *n);
 102 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
 103 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
 104 inline float findDistance(XYZ *point1, XYZ *point2);
 105 inline float findLength(XYZ *point1);
 106 inline float findLengthfast(XYZ *point1);
 107 inline float findDistancefast(XYZ *point1, XYZ *point2);
 108 inline float findDistancefast(XYZ point1, XYZ point2);
 109 inline float findDistancefastflat(XYZ *point1, XYZ *point2);
 110 inline float dotproduct(XYZ *point1, XYZ *point2);
 111 bool sphere_line_intersection (
 112                                                            float x1, float y1 , float z1,
 113                                                            float x2, float y2 , float z2,
 114                                                            float x3, float y3 , float z3, float r );
 115 bool sphere_line_intersection (
 116                                                            XYZ *p1, XYZ *p2, XYZ *p3, float *r );
 117 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection );
 118
 119
 120 inline void Normalise(XYZ *vectory) {
 121         static float d;
 122         d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
 123         if(d==0){return;}
 124         vectory->x /= d;
 125         vectory->y /= d;
 126         vectory->z /= d;
 127 }
 128
 129 inline XYZ XYZ::operator+(XYZ add){
 130         static XYZ ne;
 131         ne=add;
 132         ne.x+=x;
 133         ne.y+=y;
 134         ne.z+=z;
 135         return ne;
 136 }
 137
 138 inline XYZ XYZ::operator-(XYZ add){
 139         static XYZ ne;
 140         ne=add;
 141         ne.x=x-ne.x;
 142         ne.y=y-ne.y;
 143         ne.z=z-ne.z;
 144         return ne;
 145 }
 146
 147 inline XYZ XYZ::operator*(float add){
 148         static XYZ ne;
 149         ne.x=x*add;
 150         ne.y=y*add;
 151         ne.z=z*add;
 152         return ne;
 153 }
 154
 155 inline XYZ XYZ::operator*(XYZ add){
 156         static XYZ ne;
 157         ne.x=x*add.x;
 158         ne.y=y*add.y;
 159         ne.z=z*add.z;
 160         return ne;
 161 }
 162
 163 inline XYZ XYZ::operator/(float add){
 164         static XYZ ne;
 165         ne.x=x/add;
 166         ne.y=y/add;
 167         ne.z=z/add;
 168         return ne;
 169 }
 170
 171 inline void XYZ::operator+=(XYZ add){
 172         x+=add.x;
 173         y+=add.y;
 174         z+=add.z;
 175 }
 176
 177 inline void XYZ::operator-=(XYZ add){
 178         x=x-add.x;
 179         y=y-add.y;
 180         z=z-add.z;
 181 }
 182
 183 inline void XYZ::operator*=(float add){
 184         x=x*add;
 185         y=y*add;
 186         z=z*add;
 187 }
 188
 189 inline void XYZ::operator*=(XYZ add){
 190         x=x*add.x;
 191         y=y*add.y;
 192         z=z*add.z;
 193 }
 194
 195 inline void XYZ::operator/=(float add){
 196         x=x/add;
 197         y=y/add;
 198         z=z/add;
 199 }
 200
 201 inline void XYZ::operator=(float add){
 202         x=add;
 203         y=add;
 204         z=add;
 205 }
 206
 207 inline void XYZ::vec(Vector add){
 208         x=add.x;
 209         y=add.y;
 210         z=add.z;
 211 }
 212
 213 inline bool XYZ::operator==(XYZ add){
 214         if(x==add.x&&y==add.y&&z==add.z)return 1;
 215         return 0;
 216 }
 217
 218 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V){
 219         V->x = P->y * Q->z - P->z * Q->y;
 220         V->y = P->z * Q->x - P->x * Q->z;
 221         V->z = P->x * Q->y - P->y * Q->x;
 222 }
 223
 224 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V){
 225         V->x = P.y * Q.z - P.z * Q.y;
 226         V->y = P.z * Q.x - P.x * Q.z;
 227         V->z = P.x * Q.y - P.y * Q.x;
 228 }
 229
 230 inline float fast_sqrt (register float arg)
 231 {
 232 #if PLATFORM_MACOSX
 233         // Can replace with slower return std::sqrt(arg);
 234         register float result;
 235
 236         if (arg == 0.0) return 0.0;
 237
 238         asm {
 239                 frsqrte         result,arg                      // Calculate Square root
 240         }
 241
 242         // Newton Rhapson iterations.
 243         result = result + 0.5 * result * (1.0 - arg * result * result);
 244         result = result + 0.5 * result * (1.0 - arg * result * result);
 245
 246         return result * arg;
 247 #else
 248         return sqrtf( arg);
 249 #endif
 250 }
 251
 252 inline float normaldotproduct(XYZ point1, XYZ point2){
 253         static GLfloat returnvalue;
 254         Normalise(&point1);
 255         Normalise(&point2);
 256         returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z);
 257         return returnvalue;
 258 }
 259
 260 inline void ReflectVector(XYZ *vel, XYZ *n)
 261 {
 262         static XYZ vn;
 263         static XYZ vt;
 264         static float dotprod;
 265
 266         dotprod=dotproduct(n,vel);
 267         vn.x=n->x*dotprod;
 268         vn.y=n->y*dotprod;
 269         vn.z=n->z*dotprod;
 270
 271         vt.x=vel->x-vn.x;
 272         vt.y=vel->y-vn.y;
 273         vt.z=vel->z-vn.z;
 274
 275         vel->x = vt.x - vn.x;
 276         vel->y = vt.y - vn.y;
 277         vel->z = vt.z - vn.z;
 278 }
 279
 280 inline float dotproduct(XYZ *point1, XYZ *point2){
 281         static GLfloat returnvalue;
 282         returnvalue=(point1->x*point2->x+point1->y*point2->y+point1->z*point2->z);
 283         return returnvalue;
 284 }
 285
 286 inline float findDistance(XYZ *point1, XYZ *point2){
 287         return(fast_sqrt((point1->x-point2->x)*(point1->x-point2->x)+(point1->y-point2->y)*(point1->y-point2->y)+(point1->z-point2->z)*(point1->z-point2->z)));
 288 }
 289
 290 inline float findLength(XYZ *point1){
 291         return(fast_sqrt((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z)));
 292 }
 293
 294
 295 inline float findLengthfast(XYZ *point1){
 296         return((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z));
 297 }
 298
 299 inline float findDistancefast(XYZ *point1, XYZ *point2){
 300         return((point1->x-point2->x)*(point1->x-point2->x)+(point1->y-point2->y)*(point1->y-point2->y)+(point1->z-point2->z)*(point1->z-point2->z));
 301 }
 302
 303 inline float findDistancefast(XYZ point1, XYZ point2){
 304         return((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z));
 305 }
 306
 307 inline float findDistancefastflat(XYZ *point1, XYZ *point2){
 308         return((point1->x-point2->x)*(point1->x-point2->x)+(point1->z-point2->z)*(point1->z-point2->z));
 309 }
 310
 311 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
 312         static XYZ newpoint;
 313         if(xang){
 314                 xang*=6.283185f;
 315                 xang/=360;
 316         }
 317         if(yang){
 318                 yang*=6.283185f;
 319                 yang/=360;
 320         }
 321         if(zang){
 322                 zang*=6.283185f;
 323                 zang/=360;
 324         }
 325
 326
 327         if(yang){
 328                 newpoint.z=thePoint.z*cosf(yang)-thePoint.x*sinf(yang);
 329                 newpoint.x=thePoint.z*sinf(yang)+thePoint.x*cosf(yang);
 330                 thePoint.z=newpoint.z;
 331                 thePoint.x=newpoint.x;
 332         }
 333
 334         if(zang){
 335                 newpoint.x=thePoint.x*cosf(zang)-thePoint.y*sinf(zang);
 336                 newpoint.y=thePoint.y*cosf(zang)+thePoint.x*sinf(zang);
 337                 thePoint.x=newpoint.x;
 338                 thePoint.y=newpoint.y;
 339         }
 340
 341         if(xang){
 342                 newpoint.y=thePoint.y*cosf(xang)-thePoint.z*sinf(xang);
 343                 newpoint.z=thePoint.y*sinf(xang)+thePoint.z*cosf(xang);
 344                 thePoint.z=newpoint.z;
 345                 thePoint.y=newpoint.y;
 346         }
 347
 348         return thePoint;
 349 }
 350
 351 inline float square( float f ) { return (f*f) ;}
 352
 353 inline bool sphere_line_intersection (
 354                                                                           float x1, float y1 , float z1,
 355                                                                           float x2, float y2 , float z2,
 356                                                                           float x3, float y3 , float z3, float r )
 357 {
 358
 359         // x1,y1,z1  P1 coordinates (point of line)
 360         // x2,y2,z2  P2 coordinates (point of line)
 361         // x3,y3,z3, r  P3 coordinates and radius (sphere)
 362         // x,y,z   intersection coordinates
 363         //
 364         // This function returns a pointer array which first index indicates
 365         // the number of intersection point, followed by coordinate pairs.
 366
 367         static float x , y , z;
 368         static float a, b, c, mu, i ;
 369
 370         if(x1>x3+r&&x2>x3+r)return(0);
 371         if(x1<x3-r&&x2<x3-r)return(0);
 372         if(y1>y3+r&&y2>y3+r)return(0);
 373         if(y1<y3-r&&y2<y3-r)return(0);
 374         if(z1>z3+r&&z2>z3+r)return(0);
 375         if(z1<z3-r&&z2<z3-r)return(0);
 376         a =  square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
 377         b =  2* ( (x2 - x1)*(x1 - x3)
 378                 + (y2 - y1)*(y1 - y3)
 379                 + (z2 - z1)*(z1 - z3) ) ;
 380         c =  square(x3) + square(y3) +
 381                 square(z3) + square(x1) +
 382                 square(y1) + square(z1) -
 383                 2* ( x3*x1 + y3*y1 + z3*z1 ) - square(r) ;
 384         i =   b * b - 4 * a * c ;
 385
 386         if ( i < 0.0 )
 387         {
 388                 // no intersection
 389                 return(0);
 390         }
 391         return(1);
 392 }
 393
 394 inline bool sphere_line_intersection (
 395                                                                           XYZ *p1, XYZ *p2, XYZ *p3, float *r )
 396 {
 397
 398         // x1,p1->y,p1->z  P1 coordinates (point of line)
 399         // p2->x,p2->y,p2->z  P2 coordinates (point of line)
 400         // p3->x,p3->y,p3->z, r  P3 coordinates and radius (sphere)
 401         // x,y,z   intersection coordinates
 402         //
 403         // This function returns a pointer array which first index indicates
 404         // the number of intersection point, followed by coordinate pairs.
 405
 406         static float x , y , z;
 407         static float a, b, c, mu, i ;
 408
 409         if(p1->x>p3->x+*r&&p2->x>p3->x+*r)return(0);
 410         if(p1->x<p3->x-*r&&p2->x<p3->x-*r)return(0);
 411         if(p1->y>p3->y+*r&&p2->y>p3->y+*r)return(0);
 412         if(p1->y<p3->y-*r&&p2->y<p3->y-*r)return(0);
 413         if(p1->z>p3->z+*r&&p2->z>p3->z+*r)return(0);
 414         if(p1->z<p3->z-*r&&p2->z<p3->z-*r)return(0);
 415         a =  square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
 416         b =  2* ( (p2->x - p1->x)*(p1->x - p3->x)
 417                 + (p2->y - p1->y)*(p1->y - p3->y)
 418                 + (p2->z - p1->z)*(p1->z - p3->z) ) ;
 419         c =  square(p3->x) + square(p3->y) +
 420                 square(p3->z) + square(p1->x) +
 421                 square(p1->y) + square(p1->z) -
 422                 2* ( p3->x*p1->x + p3->y*p1->y + p3->z*p1->z ) - square(*r) ;
 423         i =   b * b - 4 * a * c ;
 424
 425         if ( i < 0.0 )
 426         {
 427                 // no intersection
 428                 return(0);
 429         }
 430         return(1);
 431 }
 432
 433 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){
 434         static XYZ newpoint;
 435         static XYZ oldpoint;
 436
 437         oldpoint=thePoint;
 438
 439         if(yang!=0){
 440                 newpoint.z=oldpoint.z*cosf(yang)-oldpoint.x*sinf(yang);
 441                 newpoint.x=oldpoint.z*sinf(yang)+oldpoint.x*cosf(yang);
 442                 oldpoint.z=newpoint.z;
 443                 oldpoint.x=newpoint.x;
 444         }
 445
 446         if(zang!=0){
 447                 newpoint.x=oldpoint.x*cosf(zang)-oldpoint.y*sinf(zang);
 448                 newpoint.y=oldpoint.y*cosf(zang)+oldpoint.x*sinf(zang);
 449                 oldpoint.x=newpoint.x;
 450                 oldpoint.y=newpoint.y;
 451         }
 452
 453         if(xang!=0){
 454                 newpoint.y=oldpoint.y*cosf(xang)-oldpoint.z*sinf(xang);
 455                 newpoint.z=oldpoint.y*sinf(xang)+oldpoint.z*cosf(xang);
 456                 oldpoint.z=newpoint.z;
 457                 oldpoint.y=newpoint.y;
 458         }
 459
 460         return oldpoint;
 461
 462 }
 463
 464 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
 465 {
 466         float LineMag;
 467         float U;
 468
 469         LineMag = findDistance( LineEnd, LineStart );
 470
 471         U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) +
 472                 ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) +
 473                 ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) /
 474                 ( LineMag * LineMag );
 475
 476         if( U < 0.0f || U > 1.0f )
 477                 return 0;   // closest point does not fall within the line segment
 478
 479         Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x );
 480         Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y );
 481         Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z );
 482
 483         *Distance = findDistance( Point, Intersection );
 484
 485         return 1;
 486 }
 487
 488 #endif