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