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