2 Copyright (C) 2003, 2010 - Wolfire Games
4 This file is part of Lugaru.
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.
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.
15 See the GNU General Public License for more details.
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.
23 #ifndef _QUATERNIONS_H_
24 #define _QUATERNIONS_H_
32 #include "PhysicsMath.h"
35 /**> Quaternion Structures <**/
36 #define PI 3.14159265355555897932384626
39 #define deg2rad .0174532925
41 //using namespace std;
42 typedef float Matrix_t [4][4];
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);
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);
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 );
119 inline void Normalise(XYZ *vectory)
122 d = fast_sqrt(vectory->x * vectory->x + vectory->y * vectory->y + vectory->z * vectory->z);
131 inline XYZ XYZ::operator+(XYZ add)
141 inline XYZ XYZ::operator-(XYZ add)
151 inline XYZ XYZ::operator*(float add)
160 inline XYZ XYZ::operator*(XYZ add)
169 inline XYZ XYZ::operator/(float add)
178 inline void XYZ::operator+=(XYZ add)
185 inline void XYZ::operator-=(XYZ add)
192 inline void XYZ::operator*=(float add)
199 inline void XYZ::operator*=(XYZ add)
206 inline void XYZ::operator/=(float add)
213 inline void XYZ::operator=(float add)
220 inline void XYZ::vec(Vector add)
227 inline bool XYZ::operator==(XYZ add)
229 if (x == add.x && y == add.y && z == add.z)return 1;
233 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V)
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;
240 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V)
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;
247 inline float fast_sqrt (register float arg)
250 // Can replace with slower return std::sqrt(arg);
251 register float result;
253 if (arg == 0.0) return 0.0;
256 frsqrte result, arg // Calculate Square root
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);
269 inline float normaldotproduct(XYZ point1, XYZ point2)
271 static GLfloat returnvalue;
274 returnvalue = (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
278 inline void ReflectVector(XYZ *vel, const XYZ *n)
280 ReflectVector(vel, *n);
283 inline void ReflectVector(XYZ *vel, const XYZ &n)
287 static float dotprod;
289 dotprod = dotproduct(&n, vel);
290 vn.x = n.x * dotprod;
291 vn.y = n.y * dotprod;
292 vn.z = n.z * dotprod;
294 vt.x = vel->x - vn.x;
295 vt.y = vel->y - vn.y;
296 vt.z = vel->z - vn.z;
298 vel->x = vt.x - vn.x;
299 vel->y = vt.y - vn.y;
300 vel->z = vt.z - vn.z;
303 inline float dotproduct(const XYZ *point1, const XYZ *point2)
305 static GLfloat returnvalue;
306 returnvalue = (point1->x * point2->x + point1->y * point2->y + point1->z * point2->z);
310 inline float findDistance(XYZ *point1, XYZ *point2)
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)));
315 inline float findLength(XYZ *point1)
317 return(fast_sqrt((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z)));
321 inline float findLengthfast(XYZ *point1)
323 return((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z));
326 inline float distsq(XYZ *point1, XYZ *point2)
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));
331 inline float distsq(XYZ point1, XYZ point2)
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));
336 inline float distsqflat(XYZ *point1, XYZ *point2)
338 return((point1->x - point2->x) * (point1->x - point2->x) + (point1->z - point2->z) * (point1->z - point2->z));
341 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang)
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;
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;
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;
382 inline float square( float f )
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 )
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
398 // This function returns a pointer array which first index indicates
399 // the number of intersection point, followed by coordinate pairs.
401 //~ static float x , y , z;
402 static float a, b, c, /*mu,*/ i ;
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 ;
427 inline bool sphere_line_intersection (
428 XYZ *p1, XYZ *p2, XYZ *p3, float *r )
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
436 // This function returns a pointer array which first index indicates
437 // the number of intersection point, followed by coordinate pairs.
439 //~ static float x , y , z;
440 static float a, b, c, /*mu,*/ i ;
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 ;
465 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang)
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;
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;
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;
497 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
502 LineMag = findDistance( LineEnd, LineStart );
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 );
509 if ( U < 0.0f || U > 1.0f )
510 return 0; // closest point does not fall within the line segment
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 );
516 *Distance = findDistance( Point, Intersection );