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, const XYZ *n);
 102 inline void ReflectVector(XYZ *vel, const XYZ &n);
 103 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
 104 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
 105 inline float findDistance(XYZ *point1, XYZ *point2);
 106 inline float findLength(XYZ *point1);
 107 inline float findLengthfast(XYZ *point1);
 108 inline float findDistancefast(XYZ *point1, XYZ *point2);
 109 inline float findDistancefast(XYZ point1, XYZ point2);
 110 inline float findDistancefastflat(XYZ *point1, XYZ *point2);
 111 inline float dotproduct(const XYZ *point1, const XYZ *point2);
 112 bool sphere_line_intersection (
 113                                                            float x1, float y1 , float z1,
 114                                                            float x2, float y2 , float z2,
 115                                                            float x3, float y3 , float z3, float r );
 116 bool sphere_line_intersection (
 117                                                            XYZ *p1, XYZ *p2, XYZ *p3, float *r );
 118 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection );
 119
 120
 121 inline void Normalise(XYZ *vectory) {
 122         static float d;
 123         d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
 124         if(d==0){return;}
 125         vectory->x /= d;
 126         vectory->y /= d;
 127         vectory->z /= d;
 128 }
 129
 130 inline XYZ XYZ::operator+(XYZ add){
 131         static XYZ ne;
 132         ne=add;
 133         ne.x+=x;
 134         ne.y+=y;
 135         ne.z+=z;
 136         return ne;
 137 }
 138
 139 inline XYZ XYZ::operator-(XYZ add){
 140         static XYZ ne;
 141         ne=add;
 142         ne.x=x-ne.x;
 143         ne.y=y-ne.y;
 144         ne.z=z-ne.z;
 145         return ne;
 146 }
 147
 148 inline XYZ XYZ::operator*(float add){
 149         static XYZ ne;
 150         ne.x=x*add;
 151         ne.y=y*add;
 152         ne.z=z*add;
 153         return ne;
 154 }
 155
 156 inline XYZ XYZ::operator*(XYZ add){
 157         static XYZ ne;
 158         ne.x=x*add.x;
 159         ne.y=y*add.y;
 160         ne.z=z*add.z;
 161         return ne;
 162 }
 163
 164 inline XYZ XYZ::operator/(float add){
 165         static XYZ ne;
 166         ne.x=x/add;
 167         ne.y=y/add;
 168         ne.z=z/add;
 169         return ne;
 170 }
 171
 172 inline void XYZ::operator+=(XYZ add){
 173         x+=add.x;
 174         y+=add.y;
 175         z+=add.z;
 176 }
 177
 178 inline void XYZ::operator-=(XYZ add){
 179         x=x-add.x;
 180         y=y-add.y;
 181         z=z-add.z;
 182 }
 183
 184 inline void XYZ::operator*=(float add){
 185         x=x*add;
 186         y=y*add;
 187         z=z*add;
 188 }
 189
 190 inline void XYZ::operator*=(XYZ add){
 191         x=x*add.x;
 192         y=y*add.y;
 193         z=z*add.z;
 194 }
 195
 196 inline void XYZ::operator/=(float add){
 197         x=x/add;
 198         y=y/add;
 199         z=z/add;
 200 }
 201
 202 inline void XYZ::operator=(float add){
 203         x=add;
 204         y=add;
 205         z=add;
 206 }
 207
 208 inline void XYZ::vec(Vector add){
 209         x=add.x;
 210         y=add.y;
 211         z=add.z;
 212 }
 213
 214 inline bool XYZ::operator==(XYZ add){
 215         if(x==add.x&&y==add.y&&z==add.z)return 1;
 216         return 0;
 217 }
 218
 219 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V){
 220         V->x = P->y * Q->z - P->z * Q->y;
 221         V->y = P->z * Q->x - P->x * Q->z;
 222         V->z = P->x * Q->y - P->y * Q->x;
 223 }
 224
 225 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V){
 226         V->x = P.y * Q.z - P.z * Q.y;
 227         V->y = P.z * Q.x - P.x * Q.z;
 228         V->z = P.x * Q.y - P.y * Q.x;
 229 }
 230
 231 inline float fast_sqrt (register float arg)
 232 {
 233 #if PLATFORM_MACOSX
 234         // Can replace with slower return std::sqrt(arg);
 235         register float result;
 236
 237         if (arg == 0.0) return 0.0;
 238
 239         asm {
 240                 frsqrte         result,arg                      // Calculate Square root
 241         }
 242
 243         // Newton Rhapson iterations.
 244         result = result + 0.5 * result * (1.0 - arg * result * result);
 245         result = result + 0.5 * result * (1.0 - arg * result * result);
 246
 247         return result * arg;
 248 #else
 249         return sqrtf( arg);
 250 #endif
 251 }
 252
 253 inline float normaldotproduct(XYZ point1, XYZ point2){
 254         static GLfloat returnvalue;
 255         Normalise(&point1);
 256         Normalise(&point2);
 257         returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z);
 258         return returnvalue;
 259 }
 260
 261 inline void ReflectVector(XYZ *vel, const XYZ *n)
 262 {
 263     ReflectVector(vel, *n);
 264 }
 265
 266 inline void ReflectVector(XYZ *vel, const XYZ &n)
 267 {
 268         static XYZ vn;
 269         static XYZ vt;
 270         static float dotprod;
 271
 272         dotprod=dotproduct(&n,vel);
 273         vn.x=n.x*dotprod;
 274         vn.y=n.y*dotprod;
 275         vn.z=n.z*dotprod;
 276
 277         vt.x=vel->x-vn.x;
 278         vt.y=vel->y-vn.y;
 279         vt.z=vel->z-vn.z;
 280
 281         vel->x = vt.x - vn.x;
 282         vel->y = vt.y - vn.y;
 283         vel->z = vt.z - vn.z;
 284 }
 285
 286 inline float dotproduct(const XYZ *point1, const XYZ *point2){
 287         static GLfloat returnvalue;
 288         returnvalue=(point1->x*point2->x+point1->y*point2->y+point1->z*point2->z);
 289         return returnvalue;
 290 }
 291
 292 inline float findDistance(XYZ *point1, XYZ *point2){
 293         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)));
 294 }
 295
 296 inline float findLength(XYZ *point1){
 297         return(fast_sqrt((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z)));
 298 }
 299
 300
 301 inline float findLengthfast(XYZ *point1){
 302         return((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z));
 303 }
 304
 305 inline float findDistancefast(XYZ *point1, XYZ *point2){
 306         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));
 307 }
 308
 309 inline float findDistancefast(XYZ point1, XYZ point2){
 310         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));
 311 }
 312
 313 inline float findDistancefastflat(XYZ *point1, XYZ *point2){
 314         return((point1->x-point2->x)*(point1->x-point2->x)+(point1->z-point2->z)*(point1->z-point2->z));
 315 }
 316
 317 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
 318         static XYZ newpoint;
 319         if(xang){
 320                 xang*=6.283185f;
 321                 xang/=360;
 322         }
 323         if(yang){
 324                 yang*=6.283185f;
 325                 yang/=360;
 326         }
 327         if(zang){
 328                 zang*=6.283185f;
 329                 zang/=360;
 330         }
 331
 332
 333         if(yang){
 334                 newpoint.z=thePoint.z*cosf(yang)-thePoint.x*sinf(yang);
 335                 newpoint.x=thePoint.z*sinf(yang)+thePoint.x*cosf(yang);
 336                 thePoint.z=newpoint.z;
 337                 thePoint.x=newpoint.x;
 338         }
 339
 340         if(zang){
 341                 newpoint.x=thePoint.x*cosf(zang)-thePoint.y*sinf(zang);
 342                 newpoint.y=thePoint.y*cosf(zang)+thePoint.x*sinf(zang);
 343                 thePoint.x=newpoint.x;
 344                 thePoint.y=newpoint.y;
 345         }
 346
 347         if(xang){
 348                 newpoint.y=thePoint.y*cosf(xang)-thePoint.z*sinf(xang);
 349                 newpoint.z=thePoint.y*sinf(xang)+thePoint.z*cosf(xang);
 350                 thePoint.z=newpoint.z;
 351                 thePoint.y=newpoint.y;
 352         }
 353
 354         return thePoint;
 355 }
 356
 357 inline float square( float f ) { return (f*f) ;}
 358
 359 inline bool sphere_line_intersection (
 360                                                                           float x1, float y1 , float z1,
 361                                                                           float x2, float y2 , float z2,
 362                                                                           float x3, float y3 , float z3, float r )
 363 {
 364
 365         // x1,y1,z1  P1 coordinates (point of line)
 366         // x2,y2,z2  P2 coordinates (point of line)
 367         // x3,y3,z3, r  P3 coordinates and radius (sphere)
 368         // x,y,z   intersection coordinates
 369         //
 370         // This function returns a pointer array which first index indicates
 371         // the number of intersection point, followed by coordinate pairs.
 372
 373         static float x , y , z;
 374         static float a, b, c, mu, i ;
 375
 376         if(x1>x3+r&&x2>x3+r)return(0);
 377         if(x1<x3-r&&x2<x3-r)return(0);
 378         if(y1>y3+r&&y2>y3+r)return(0);
 379         if(y1<y3-r&&y2<y3-r)return(0);
 380         if(z1>z3+r&&z2>z3+r)return(0);
 381         if(z1<z3-r&&z2<z3-r)return(0);
 382         a =  square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
 383         b =  2* ( (x2 - x1)*(x1 - x3)
 384                 + (y2 - y1)*(y1 - y3)
 385                 + (z2 - z1)*(z1 - z3) ) ;
 386         c =  square(x3) + square(y3) +
 387                 square(z3) + square(x1) +
 388                 square(y1) + square(z1) -
 389                 2* ( x3*x1 + y3*y1 + z3*z1 ) - square(r) ;
 390         i =   b * b - 4 * a * c ;
 391
 392         if ( i < 0.0 )
 393         {
 394                 // no intersection
 395                 return(0);
 396         }
 397         return(1);
 398 }
 399
 400 inline bool sphere_line_intersection (
 401                                                                           XYZ *p1, XYZ *p2, XYZ *p3, float *r )
 402 {
 403
 404         // x1,p1->y,p1->z  P1 coordinates (point of line)
 405         // p2->x,p2->y,p2->z  P2 coordinates (point of line)
 406         // p3->x,p3->y,p3->z, r  P3 coordinates and radius (sphere)
 407         // x,y,z   intersection coordinates
 408         //
 409         // This function returns a pointer array which first index indicates
 410         // the number of intersection point, followed by coordinate pairs.
 411
 412         static float x , y , z;
 413         static float a, b, c, mu, i ;
 414
 415         if(p1->x>p3->x+*r&&p2->x>p3->x+*r)return(0);
 416         if(p1->x<p3->x-*r&&p2->x<p3->x-*r)return(0);
 417         if(p1->y>p3->y+*r&&p2->y>p3->y+*r)return(0);
 418         if(p1->y<p3->y-*r&&p2->y<p3->y-*r)return(0);
 419         if(p1->z>p3->z+*r&&p2->z>p3->z+*r)return(0);
 420         if(p1->z<p3->z-*r&&p2->z<p3->z-*r)return(0);
 421         a =  square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
 422         b =  2* ( (p2->x - p1->x)*(p1->x - p3->x)
 423                 + (p2->y - p1->y)*(p1->y - p3->y)
 424                 + (p2->z - p1->z)*(p1->z - p3->z) ) ;
 425         c =  square(p3->x) + square(p3->y) +
 426                 square(p3->z) + square(p1->x) +
 427                 square(p1->y) + square(p1->z) -
 428                 2* ( p3->x*p1->x + p3->y*p1->y + p3->z*p1->z ) - square(*r) ;
 429         i =   b * b - 4 * a * c ;
 430
 431         if ( i < 0.0 )
 432         {
 433                 // no intersection
 434                 return(0);
 435         }
 436         return(1);
 437 }
 438
 439 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){
 440         static XYZ newpoint;
 441         static XYZ oldpoint;
 442
 443         oldpoint=thePoint;
 444
 445         if(yang!=0){
 446                 newpoint.z=oldpoint.z*cosf(yang)-oldpoint.x*sinf(yang);
 447                 newpoint.x=oldpoint.z*sinf(yang)+oldpoint.x*cosf(yang);
 448                 oldpoint.z=newpoint.z;
 449                 oldpoint.x=newpoint.x;
 450         }
 451
 452         if(zang!=0){
 453                 newpoint.x=oldpoint.x*cosf(zang)-oldpoint.y*sinf(zang);
 454                 newpoint.y=oldpoint.y*cosf(zang)+oldpoint.x*sinf(zang);
 455                 oldpoint.x=newpoint.x;
 456                 oldpoint.y=newpoint.y;
 457         }
 458
 459         if(xang!=0){
 460                 newpoint.y=oldpoint.y*cosf(xang)-oldpoint.z*sinf(xang);
 461                 newpoint.z=oldpoint.y*sinf(xang)+oldpoint.z*cosf(xang);
 462                 oldpoint.z=newpoint.z;
 463                 oldpoint.y=newpoint.y;
 464         }
 465
 466         return oldpoint;
 467
 468 }
 469
 470 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
 471 {
 472         float LineMag;
 473         float U;
 474
 475         LineMag = findDistance( LineEnd, LineStart );
 476
 477         U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) +
 478                 ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) +
 479                 ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) /
 480                 ( LineMag * LineMag );
 481
 482         if( U < 0.0f || U > 1.0f )
 483                 return 0;   // closest point does not fall within the line segment
 484
 485         Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x );
 486         Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y );
 487         Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z );
 488
 489         *Distance = findDistance( Point, Intersection );
 490
 491         return 1;
 492 }
 493
 494 #endif