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