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 #if PLATFORM_MACOSX
 244     // Can replace with slower return std::sqrt(arg);
 245     register float result;
 246
 247     if (arg == 0.0)
 248         return 0.0;
 249
 250     asm {
 251         frsqrte     result, arg         // Calculate Square root
 252     }
 253
 254     // Newton Rhapson iterations.
 255     result = result + 0.5 * result * (1.0 - arg * result * result);
 256     result = result + 0.5 * result * (1.0 - arg * result * result);
 257
 258     return result * arg;
 259 #else
 260     return sqrtf( arg);
 261 #endif
 262 }
 263
 264 inline float normaldotproduct(XYZ point1, XYZ point2)
 265 {
 266     static GLfloat returnvalue;
 267     Normalise(&point1);
 268     Normalise(&point2);
 269     returnvalue = (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
 270     return returnvalue;
 271 }
 272
 273 inline void ReflectVector(XYZ *vel, const XYZ *n)
 274 {
 275     ReflectVector(vel, *n);
 276 }
 277
 278 inline void ReflectVector(XYZ *vel, const XYZ &n)
 279 {
 280     static XYZ vn;
 281     static XYZ vt;
 282     static float dotprod;
 283
 284     dotprod = dotproduct(&n, vel);
 285     vn.x = n.x * dotprod;
 286     vn.y = n.y * dotprod;
 287     vn.z = n.z * dotprod;
 288
 289     vt.x = vel->x - vn.x;
 290     vt.y = vel->y - vn.y;
 291     vt.z = vel->z - vn.z;
 292
 293     vel->x = vt.x - vn.x;
 294     vel->y = vt.y - vn.y;
 295     vel->z = vt.z - vn.z;
 296 }
 297
 298 inline float dotproduct(const XYZ *point1, const XYZ *point2)
 299 {
 300     static GLfloat returnvalue;
 301     returnvalue = (point1->x * point2->x + point1->y * point2->y + point1->z * point2->z);
 302     return returnvalue;
 303 }
 304
 305 inline float findDistance(XYZ *point1, XYZ *point2)
 306 {
 307     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)));
 308 }
 309
 310 inline float findLength(XYZ *point1)
 311 {
 312     return(fast_sqrt((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z)));
 313 }
 314
 315
 316 inline float findLengthfast(XYZ *point1)
 317 {
 318     return((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z));
 319 }
 320
 321 inline float distsq(XYZ *point1, XYZ *point2)
 322 {
 323     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));
 324 }
 325
 326 inline float distsq(XYZ point1, XYZ point2)
 327 {
 328     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));
 329 }
 330
 331 inline float distsqflat(XYZ *point1, XYZ *point2)
 332 {
 333     return((point1->x - point2->x) * (point1->x - point2->x) + (point1->z - point2->z) * (point1->z - point2->z));
 334 }
 335
 336 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang)
 337 {
 338     static XYZ newpoint;
 339     if (xang) {
 340         xang *= 6.283185f;
 341         xang /= 360;
 342     }
 343     if (yang) {
 344         yang *= 6.283185f;
 345         yang /= 360;
 346     }
 347     if (zang) {
 348         zang *= 6.283185f;
 349         zang /= 360;
 350     }
 351
 352
 353     if (yang) {
 354         newpoint.z = thePoint.z * cosf(yang) - thePoint.x * sinf(yang);
 355         newpoint.x = thePoint.z * sinf(yang) + thePoint.x * cosf(yang);
 356         thePoint.z = newpoint.z;
 357         thePoint.x = newpoint.x;
 358     }
 359
 360     if (zang) {
 361         newpoint.x = thePoint.x * cosf(zang) - thePoint.y * sinf(zang);
 362         newpoint.y = thePoint.y * cosf(zang) + thePoint.x * sinf(zang);
 363         thePoint.x = newpoint.x;
 364         thePoint.y = newpoint.y;
 365     }
 366
 367     if (xang) {
 368         newpoint.y = thePoint.y * cosf(xang) - thePoint.z * sinf(xang);
 369         newpoint.z = thePoint.y * sinf(xang) + thePoint.z * cosf(xang);
 370         thePoint.z = newpoint.z;
 371         thePoint.y = newpoint.y;
 372     }
 373
 374     return thePoint;
 375 }
 376
 377 inline float square( float f )
 378 {
 379     return (f * f) ;
 380 }
 381
 382 inline bool sphere_line_intersection (
 383     float x1, float y1 , float z1,
 384     float x2, float y2 , float z2,
 385     float x3, float y3 , float z3, float r )
 386 {
 387
 388     // x1,y1,z1  P1 coordinates (point of line)
 389     // x2,y2,z2  P2 coordinates (point of line)
 390     // x3,y3,z3, r  P3 coordinates and radius (sphere)
 391     // x,y,z   intersection coordinates
 392     //
 393     // This function returns a pointer array which first index indicates
 394     // the number of intersection point, followed by coordinate pairs.
 395
 396     //~ static float x , y , z;
 397     static float a, b, c, /*mu,*/ i ;
 398
 399     if (x1 > x3 + r && x2 > x3 + r) return(0);
 400     if (x1 < x3 - r && x2 < x3 - r) return(0);
 401     if (y1 > y3 + r && y2 > y3 + r) return(0);
 402     if (y1 < y3 - r && y2 < y3 - r) return(0);
 403     if (z1 > z3 + r && z2 > z3 + r) return(0);
 404     if (z1 < z3 - r && z2 < z3 - r) return(0);
 405     a =  square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
 406     b =  2 * ( (x2 - x1) * (x1 - x3)
 407                + (y2 - y1) * (y1 - y3)
 408                + (z2 - z1) * (z1 - z3) ) ;
 409     c =  square(x3) + square(y3) +
 410          square(z3) + square(x1) +
 411          square(y1) + square(z1) -
 412          2 * ( x3 * x1 + y3 * y1 + z3 * z1 ) - square(r) ;
 413     i =   b * b - 4 * a * c ;
 414
 415     if ( i < 0.0 ) {
 416         // no intersection
 417         return(0);
 418     }
 419     return(1);
 420 }
 421
 422 inline bool sphere_line_intersection (
 423     XYZ *p1, XYZ *p2, XYZ *p3, float *r )
 424 {
 425
 426     // x1,p1->y,p1->z  P1 coordinates (point of line)
 427     // p2->x,p2->y,p2->z  P2 coordinates (point of line)
 428     // p3->x,p3->y,p3->z, r  P3 coordinates and radius (sphere)
 429     // x,y,z   intersection coordinates
 430     //
 431     // This function returns a pointer array which first index indicates
 432     // the number of intersection point, followed by coordinate pairs.
 433
 434     //~ static float x , y , z;
 435     static float a, b, c, /*mu,*/ i ;
 436
 437     if (p1->x > p3->x + *r && p2->x > p3->x + *r) return(0);
 438     if (p1->x < p3->x - *r && p2->x < p3->x - *r) return(0);
 439     if (p1->y > p3->y + *r && p2->y > p3->y + *r) return(0);
 440     if (p1->y < p3->y - *r && p2->y < p3->y - *r) return(0);
 441     if (p1->z > p3->z + *r && p2->z > p3->z + *r) return(0);
 442     if (p1->z < p3->z - *r && p2->z < p3->z - *r) return(0);
 443     a =  square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
 444     b =  2 * ( (p2->x - p1->x) * (p1->x - p3->x)
 445                + (p2->y - p1->y) * (p1->y - p3->y)
 446                + (p2->z - p1->z) * (p1->z - p3->z) ) ;
 447     c =  square(p3->x) + square(p3->y) +
 448          square(p3->z) + square(p1->x) +
 449          square(p1->y) + square(p1->z) -
 450          2 * ( p3->x * p1->x + p3->y * p1->y + p3->z * p1->z ) - square(*r) ;
 451     i =   b * b - 4 * a * c ;
 452
 453     if ( i < 0.0 ) {
 454         // no intersection
 455         return(0);
 456     }
 457     return(1);
 458 }
 459
 460 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang)
 461 {
 462     static XYZ newpoint;
 463     static XYZ oldpoint;
 464
 465     oldpoint = thePoint;
 466
 467     if (yang != 0) {
 468         newpoint.z = oldpoint.z * cosf(yang) - oldpoint.x * sinf(yang);
 469         newpoint.x = oldpoint.z * sinf(yang) + oldpoint.x * cosf(yang);
 470         oldpoint.z = newpoint.z;
 471         oldpoint.x = newpoint.x;
 472     }
 473
 474     if (zang != 0) {
 475         newpoint.x = oldpoint.x * cosf(zang) - oldpoint.y * sinf(zang);
 476         newpoint.y = oldpoint.y * cosf(zang) + oldpoint.x * sinf(zang);
 477         oldpoint.x = newpoint.x;
 478         oldpoint.y = newpoint.y;
 479     }
 480
 481     if (xang != 0) {
 482         newpoint.y = oldpoint.y * cosf(xang) - oldpoint.z * sinf(xang);
 483         newpoint.z = oldpoint.y * sinf(xang) + oldpoint.z * cosf(xang);
 484         oldpoint.z = newpoint.z;
 485         oldpoint.y = newpoint.y;
 486     }
 487
 488     return oldpoint;
 489
 490 }
 491
 492 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
 493 {
 494     float LineMag;
 495     float U;
 496
 497     LineMag = findDistance( LineEnd, LineStart );
 498
 499     U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) +
 500           ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) +
 501           ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) /
 502         ( LineMag * LineMag );
 503
 504     if ( U < 0.0f || U > 1.0f )
 505         return 0;   // closest point does not fall within the line segment
 506
 507     Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x );
 508     Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y );
 509     Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z );
 510
 511     *Distance = findDistance( Point, Intersection );
 512
 513     return 1;
 514 }
 515
 516 #endif