Source/Quaternions.h

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