Source/Quaternions.h

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