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];
61 XYZ() : x(0.0f), y(0.0f), z(0.0f) {}
62 inline XYZ operator+(XYZ add);
63 inline XYZ operator-(XYZ add);
64 inline XYZ operator*(float add);
65 inline XYZ operator*(XYZ add);
66 inline XYZ operator/(float add);
67 inline void operator+=(XYZ add);
68 inline void operator-=(XYZ add);
69 inline void operator*=(float add);
70 inline void operator*=(XYZ add);
71 inline void operator/=(float add);
72 inline void operator=(float add);
73 inline void vec(Vector add);
74 inline bool operator==(XYZ add);
77 /*********************> Quaternion Function definition <********/
78 quaternion To_Quat(int Degree_Flag, euler Euler);
79 quaternion To_Quat(angle_axis Ang_Ax);
80 quaternion To_Quat(Matrix_t m);
81 angle_axis Quat_2_AA(quaternion Quat);
82 void Quat_2_Matrix(quaternion Quat, Matrix_t m);
83 quaternion Normalize(quaternion Quat);
84 quaternion Quat_Mult(quaternion q1, quaternion q2);
85 quaternion QNormalize(quaternion Quat);
86 XYZ Quat2Vector(quaternion Quat);
88 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V);
89 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V);
90 inline void Normalise(XYZ *vectory);
91 inline float normaldotproduct(XYZ point1, XYZ point2);
92 inline float fast_sqrt (register float arg);
93 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3);
94 bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
95 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
96 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ n, XYZ *p);
97 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc,XYZ *n, XYZ *p);
98 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *p);
99 bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33);
100 bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p);
101 inline void ReflectVector(XYZ *vel, const XYZ *n);
102 inline void ReflectVector(XYZ *vel, const XYZ &n);
103 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
104 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
105 inline float findDistance(XYZ *point1, XYZ *point2);
106 inline float findLength(XYZ *point1);
107 inline float findLengthfast(XYZ *point1);
108 inline float findDistancefast(XYZ *point1, XYZ *point2);
109 inline float findDistancefast(XYZ point1, XYZ point2);
110 inline float findDistancefastflat(XYZ *point1, XYZ *point2);
111 inline float dotproduct(const XYZ *point1, const XYZ *point2);
112 bool sphere_line_intersection (
113 float x1, float y1 , float z1,
114 float x2, float y2 , float z2,
115 float x3, float y3 , float z3, float r );
116 bool sphere_line_intersection (
117 XYZ *p1, XYZ *p2, XYZ *p3, float *r );
118 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection );
121 inline void Normalise(XYZ *vectory) {
123 d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
130 inline XYZ XYZ::operator+(XYZ add){
139 inline XYZ XYZ::operator-(XYZ add){
148 inline XYZ XYZ::operator*(float add){
156 inline XYZ XYZ::operator*(XYZ add){
164 inline XYZ XYZ::operator/(float add){
172 inline void XYZ::operator+=(XYZ add){
178 inline void XYZ::operator-=(XYZ add){
184 inline void XYZ::operator*=(float add){
190 inline void XYZ::operator*=(XYZ add){
196 inline void XYZ::operator/=(float add){
202 inline void XYZ::operator=(float add){
208 inline void XYZ::vec(Vector add){
214 inline bool XYZ::operator==(XYZ add){
215 if(x==add.x&&y==add.y&&z==add.z)return 1;
219 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V){
220 V->x = P->y * Q->z - P->z * Q->y;
221 V->y = P->z * Q->x - P->x * Q->z;
222 V->z = P->x * Q->y - P->y * Q->x;
225 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V){
226 V->x = P.y * Q.z - P.z * Q.y;
227 V->y = P.z * Q.x - P.x * Q.z;
228 V->z = P.x * Q.y - P.y * Q.x;
231 inline float fast_sqrt (register float arg)
234 // Can replace with slower return std::sqrt(arg);
235 register float result;
237 if (arg == 0.0) return 0.0;
240 frsqrte result,arg // Calculate Square root
243 // Newton Rhapson iterations.
244 result = result + 0.5 * result * (1.0 - arg * result * result);
245 result = result + 0.5 * result * (1.0 - arg * result * result);
253 inline float normaldotproduct(XYZ point1, XYZ point2){
254 static GLfloat returnvalue;
257 returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z);
261 inline void ReflectVector(XYZ *vel, const XYZ *n)
263 ReflectVector(vel, *n);
266 inline void ReflectVector(XYZ *vel, const XYZ &n)
270 static float dotprod;
272 dotprod=dotproduct(&n,vel);
281 vel->x = vt.x - vn.x;
282 vel->y = vt.y - vn.y;
283 vel->z = vt.z - vn.z;
286 inline float dotproduct(const XYZ *point1, const XYZ *point2){
287 static GLfloat returnvalue;
288 returnvalue=(point1->x*point2->x+point1->y*point2->y+point1->z*point2->z);
292 inline float findDistance(XYZ *point1, XYZ *point2){
293 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)));
296 inline float findLength(XYZ *point1){
297 return(fast_sqrt((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z)));
301 inline float findLengthfast(XYZ *point1){
302 return((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z));
305 inline float findDistancefast(XYZ *point1, XYZ *point2){
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));
309 inline float findDistancefast(XYZ point1, XYZ point2){
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));
313 inline float findDistancefastflat(XYZ *point1, XYZ *point2){
314 return((point1->x-point2->x)*(point1->x-point2->x)+(point1->z-point2->z)*(point1->z-point2->z));
317 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
334 newpoint.z=thePoint.z*cosf(yang)-thePoint.x*sinf(yang);
335 newpoint.x=thePoint.z*sinf(yang)+thePoint.x*cosf(yang);
336 thePoint.z=newpoint.z;
337 thePoint.x=newpoint.x;
341 newpoint.x=thePoint.x*cosf(zang)-thePoint.y*sinf(zang);
342 newpoint.y=thePoint.y*cosf(zang)+thePoint.x*sinf(zang);
343 thePoint.x=newpoint.x;
344 thePoint.y=newpoint.y;
348 newpoint.y=thePoint.y*cosf(xang)-thePoint.z*sinf(xang);
349 newpoint.z=thePoint.y*sinf(xang)+thePoint.z*cosf(xang);
350 thePoint.z=newpoint.z;
351 thePoint.y=newpoint.y;
357 inline float square( float f ) { return (f*f) ;}
359 inline bool sphere_line_intersection (
360 float x1, float y1 , float z1,
361 float x2, float y2 , float z2,
362 float x3, float y3 , float z3, float r )
365 // x1,y1,z1 P1 coordinates (point of line)
366 // x2,y2,z2 P2 coordinates (point of line)
367 // x3,y3,z3, r P3 coordinates and radius (sphere)
368 // x,y,z intersection coordinates
370 // This function returns a pointer array which first index indicates
371 // the number of intersection point, followed by coordinate pairs.
373 //~ static float x , y , z;
374 static float a, b, c, /*mu,*/ i ;
376 if(x1>x3+r&&x2>x3+r)return(0);
377 if(x1<x3-r&&x2<x3-r)return(0);
378 if(y1>y3+r&&y2>y3+r)return(0);
379 if(y1<y3-r&&y2<y3-r)return(0);
380 if(z1>z3+r&&z2>z3+r)return(0);
381 if(z1<z3-r&&z2<z3-r)return(0);
382 a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
383 b = 2* ( (x2 - x1)*(x1 - x3)
384 + (y2 - y1)*(y1 - y3)
385 + (z2 - z1)*(z1 - z3) ) ;
386 c = square(x3) + square(y3) +
387 square(z3) + square(x1) +
388 square(y1) + square(z1) -
389 2* ( x3*x1 + y3*y1 + z3*z1 ) - square(r) ;
390 i = b * b - 4 * a * c ;
400 inline bool sphere_line_intersection (
401 XYZ *p1, XYZ *p2, XYZ *p3, float *r )
404 // x1,p1->y,p1->z P1 coordinates (point of line)
405 // p2->x,p2->y,p2->z P2 coordinates (point of line)
406 // p3->x,p3->y,p3->z, r P3 coordinates and radius (sphere)
407 // x,y,z intersection coordinates
409 // This function returns a pointer array which first index indicates
410 // the number of intersection point, followed by coordinate pairs.
412 //~ static float x , y , z;
413 static float a, b, c, /*mu,*/ i ;
415 if(p1->x>p3->x+*r&&p2->x>p3->x+*r)return(0);
416 if(p1->x<p3->x-*r&&p2->x<p3->x-*r)return(0);
417 if(p1->y>p3->y+*r&&p2->y>p3->y+*r)return(0);
418 if(p1->y<p3->y-*r&&p2->y<p3->y-*r)return(0);
419 if(p1->z>p3->z+*r&&p2->z>p3->z+*r)return(0);
420 if(p1->z<p3->z-*r&&p2->z<p3->z-*r)return(0);
421 a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
422 b = 2* ( (p2->x - p1->x)*(p1->x - p3->x)
423 + (p2->y - p1->y)*(p1->y - p3->y)
424 + (p2->z - p1->z)*(p1->z - p3->z) ) ;
425 c = square(p3->x) + square(p3->y) +
426 square(p3->z) + square(p1->x) +
427 square(p1->y) + square(p1->z) -
428 2* ( p3->x*p1->x + p3->y*p1->y + p3->z*p1->z ) - square(*r) ;
429 i = b * b - 4 * a * c ;
439 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){
446 newpoint.z=oldpoint.z*cosf(yang)-oldpoint.x*sinf(yang);
447 newpoint.x=oldpoint.z*sinf(yang)+oldpoint.x*cosf(yang);
448 oldpoint.z=newpoint.z;
449 oldpoint.x=newpoint.x;
453 newpoint.x=oldpoint.x*cosf(zang)-oldpoint.y*sinf(zang);
454 newpoint.y=oldpoint.y*cosf(zang)+oldpoint.x*sinf(zang);
455 oldpoint.x=newpoint.x;
456 oldpoint.y=newpoint.y;
460 newpoint.y=oldpoint.y*cosf(xang)-oldpoint.z*sinf(xang);
461 newpoint.z=oldpoint.y*sinf(xang)+oldpoint.z*cosf(xang);
462 oldpoint.z=newpoint.z;
463 oldpoint.y=newpoint.y;
470 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
475 LineMag = findDistance( LineEnd, LineStart );
477 U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) +
478 ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) +
479 ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) /
480 ( LineMag * LineMag );
482 if( U < 0.0f || U > 1.0f )
483 return 0; // closest point does not fall within the line segment
485 Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x );
486 Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y );
487 Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z );
489 *Distance = findDistance( Point, Intersection );