2 #ifndef _QUATERNIONS_H_
3 #define _QUATERNIONS_H_
11 #include "PhysicsMath.h"
14 /**> Quaternion Structures <**/
15 #define PI 3.14159265355555897932384626
18 #define deg2rad .0174532925
20 //using namespace std;
21 typedef float Matrix_t [4][4];
40 XYZ() : x(0.0f), y(0.0f), z(0.0f) {}
41 inline XYZ operator+(XYZ add);
42 inline XYZ operator-(XYZ add);
43 inline XYZ operator*(float add);
44 inline XYZ operator*(XYZ add);
45 inline XYZ operator/(float add);
46 inline void operator+=(XYZ add);
47 inline void operator-=(XYZ add);
48 inline void operator*=(float add);
49 inline void operator*=(XYZ add);
50 inline void operator/=(float add);
51 inline void operator=(float add);
52 inline void vec(Vector add);
53 inline bool operator==(XYZ add);
56 /*********************> Quaternion Function definition <********/
57 quaternion To_Quat(int Degree_Flag, euler Euler);
58 quaternion To_Quat(angle_axis Ang_Ax);
59 quaternion To_Quat(Matrix_t m);
60 angle_axis Quat_2_AA(quaternion Quat);
61 void Quat_2_Matrix(quaternion Quat, Matrix_t m);
62 quaternion Normalize(quaternion Quat);
63 quaternion Quat_Mult(quaternion q1, quaternion q2);
64 quaternion QNormalize(quaternion Quat);
65 XYZ Quat2Vector(quaternion Quat);
67 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V);
68 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V);
69 inline void Normalise(XYZ *vectory);
70 inline float normaldotproduct(XYZ point1, XYZ point2);
71 inline float fast_sqrt (register float arg);
72 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3);
73 bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
74 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p);
75 float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ n, XYZ *p);
76 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc,XYZ *n, XYZ *p);
77 float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *p);
78 bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33);
79 bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p);
80 inline void ReflectVector(XYZ *vel, XYZ *n);
81 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
82 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
83 inline float findDistance(XYZ *point1, XYZ *point2);
84 inline float findLength(XYZ *point1);
85 inline float findLengthfast(XYZ *point1);
86 inline float findDistancefast(XYZ *point1, XYZ *point2);
87 inline float findDistancefast(XYZ point1, XYZ point2);
88 inline float findDistancefastflat(XYZ *point1, XYZ *point2);
89 inline float dotproduct(XYZ *point1, XYZ *point2);
90 bool sphere_line_intersection (
91 float x1, float y1 , float z1,
92 float x2, float y2 , float z2,
93 float x3, float y3 , float z3, float r );
94 bool sphere_line_intersection (
95 XYZ *p1, XYZ *p2, XYZ *p3, float *r );
96 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection );
99 inline void Normalise(XYZ *vectory) {
101 d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z);
108 inline XYZ XYZ::operator+(XYZ add){
117 inline XYZ XYZ::operator-(XYZ add){
126 inline XYZ XYZ::operator*(float add){
134 inline XYZ XYZ::operator*(XYZ add){
142 inline XYZ XYZ::operator/(float add){
150 inline void XYZ::operator+=(XYZ add){
156 inline void XYZ::operator-=(XYZ add){
162 inline void XYZ::operator*=(float add){
168 inline void XYZ::operator*=(XYZ add){
174 inline void XYZ::operator/=(float add){
180 inline void XYZ::operator=(float add){
186 inline void XYZ::vec(Vector add){
192 inline bool XYZ::operator==(XYZ add){
193 if(x==add.x&&y==add.y&&z==add.z)return 1;
197 inline void CrossProduct(XYZ *P, XYZ *Q, XYZ *V){
198 V->x = P->y * Q->z - P->z * Q->y;
199 V->y = P->z * Q->x - P->x * Q->z;
200 V->z = P->x * Q->y - P->y * Q->x;
203 inline void CrossProduct(XYZ P, XYZ Q, XYZ *V){
204 V->x = P.y * Q.z - P.z * Q.y;
205 V->y = P.z * Q.x - P.x * Q.z;
206 V->z = P.x * Q.y - P.y * Q.x;
209 inline float fast_sqrt (register float arg)
212 // Can replace with slower return std::sqrt(arg);
213 register float result;
215 if (arg == 0.0) return 0.0;
218 frsqrte result,arg // Calculate Square root
221 // Newton Rhapson iterations.
222 result = result + 0.5 * result * (1.0 - arg * result * result);
223 result = result + 0.5 * result * (1.0 - arg * result * result);
231 inline float normaldotproduct(XYZ point1, XYZ point2){
232 static GLfloat returnvalue;
235 returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z);
239 inline void ReflectVector(XYZ *vel, XYZ *n)
243 static float dotprod;
245 dotprod=dotproduct(n,vel);
254 vel->x = vt.x - vn.x;
255 vel->y = vt.y - vn.y;
256 vel->z = vt.z - vn.z;
259 inline float dotproduct(XYZ *point1, XYZ *point2){
260 static GLfloat returnvalue;
261 returnvalue=(point1->x*point2->x+point1->y*point2->y+point1->z*point2->z);
265 inline float findDistance(XYZ *point1, XYZ *point2){
266 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)));
269 inline float findLength(XYZ *point1){
270 return(fast_sqrt((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z)));
274 inline float findLengthfast(XYZ *point1){
275 return((point1->x)*(point1->x)+(point1->y)*(point1->y)+(point1->z)*(point1->z));
278 inline float findDistancefast(XYZ *point1, XYZ *point2){
279 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));
282 inline float findDistancefast(XYZ point1, XYZ point2){
283 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));
286 inline float findDistancefastflat(XYZ *point1, XYZ *point2){
287 return((point1->x-point2->x)*(point1->x-point2->x)+(point1->z-point2->z)*(point1->z-point2->z));
290 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
307 newpoint.z=thePoint.z*cosf(yang)-thePoint.x*sinf(yang);
308 newpoint.x=thePoint.z*sinf(yang)+thePoint.x*cosf(yang);
309 thePoint.z=newpoint.z;
310 thePoint.x=newpoint.x;
314 newpoint.x=thePoint.x*cosf(zang)-thePoint.y*sinf(zang);
315 newpoint.y=thePoint.y*cosf(zang)+thePoint.x*sinf(zang);
316 thePoint.x=newpoint.x;
317 thePoint.y=newpoint.y;
321 newpoint.y=thePoint.y*cosf(xang)-thePoint.z*sinf(xang);
322 newpoint.z=thePoint.y*sinf(xang)+thePoint.z*cosf(xang);
323 thePoint.z=newpoint.z;
324 thePoint.y=newpoint.y;
330 inline float square( float f ) { return (f*f) ;}
332 inline bool sphere_line_intersection (
333 float x1, float y1 , float z1,
334 float x2, float y2 , float z2,
335 float x3, float y3 , float z3, float r )
338 // x1,y1,z1 P1 coordinates (point of line)
339 // x2,y2,z2 P2 coordinates (point of line)
340 // x3,y3,z3, r P3 coordinates and radius (sphere)
341 // x,y,z intersection coordinates
343 // This function returns a pointer array which first index indicates
344 // the number of intersection point, followed by coordinate pairs.
346 static float x , y , z;
347 static float a, b, c, mu, i ;
349 if(x1>x3+r&&x2>x3+r)return(0);
350 if(x1<x3-r&&x2<x3-r)return(0);
351 if(y1>y3+r&&y2>y3+r)return(0);
352 if(y1<y3-r&&y2<y3-r)return(0);
353 if(z1>z3+r&&z2>z3+r)return(0);
354 if(z1<z3-r&&z2<z3-r)return(0);
355 a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
356 b = 2* ( (x2 - x1)*(x1 - x3)
357 + (y2 - y1)*(y1 - y3)
358 + (z2 - z1)*(z1 - z3) ) ;
359 c = square(x3) + square(y3) +
360 square(z3) + square(x1) +
361 square(y1) + square(z1) -
362 2* ( x3*x1 + y3*y1 + z3*z1 ) - square(r) ;
363 i = b * b - 4 * a * c ;
373 inline bool sphere_line_intersection (
374 XYZ *p1, XYZ *p2, XYZ *p3, float *r )
377 // x1,p1->y,p1->z P1 coordinates (point of line)
378 // p2->x,p2->y,p2->z P2 coordinates (point of line)
379 // p3->x,p3->y,p3->z, r P3 coordinates and radius (sphere)
380 // x,y,z intersection coordinates
382 // This function returns a pointer array which first index indicates
383 // the number of intersection point, followed by coordinate pairs.
385 static float x , y , z;
386 static float a, b, c, mu, i ;
388 if(p1->x>p3->x+*r&&p2->x>p3->x+*r)return(0);
389 if(p1->x<p3->x-*r&&p2->x<p3->x-*r)return(0);
390 if(p1->y>p3->y+*r&&p2->y>p3->y+*r)return(0);
391 if(p1->y<p3->y-*r&&p2->y<p3->y-*r)return(0);
392 if(p1->z>p3->z+*r&&p2->z>p3->z+*r)return(0);
393 if(p1->z<p3->z-*r&&p2->z<p3->z-*r)return(0);
394 a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
395 b = 2* ( (p2->x - p1->x)*(p1->x - p3->x)
396 + (p2->y - p1->y)*(p1->y - p3->y)
397 + (p2->z - p1->z)*(p1->z - p3->z) ) ;
398 c = square(p3->x) + square(p3->y) +
399 square(p3->z) + square(p1->x) +
400 square(p1->y) + square(p1->z) -
401 2* ( p3->x*p1->x + p3->y*p1->y + p3->z*p1->z ) - square(*r) ;
402 i = b * b - 4 * a * c ;
412 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){
419 newpoint.z=oldpoint.z*cosf(yang)-oldpoint.x*sinf(yang);
420 newpoint.x=oldpoint.z*sinf(yang)+oldpoint.x*cosf(yang);
421 oldpoint.z=newpoint.z;
422 oldpoint.x=newpoint.x;
426 newpoint.x=oldpoint.x*cosf(zang)-oldpoint.y*sinf(zang);
427 newpoint.y=oldpoint.y*cosf(zang)+oldpoint.x*sinf(zang);
428 oldpoint.x=newpoint.x;
429 oldpoint.y=newpoint.y;
433 newpoint.y=oldpoint.y*cosf(xang)-oldpoint.z*sinf(xang);
434 newpoint.z=oldpoint.y*sinf(xang)+oldpoint.z*cosf(xang);
435 oldpoint.z=newpoint.z;
436 oldpoint.y=newpoint.y;
443 inline bool DistancePointLine( XYZ *Point, XYZ *LineStart, XYZ *LineEnd, float *Distance, XYZ *Intersection )
448 LineMag = findDistance( LineEnd, LineStart );
450 U = ( ( ( Point->x - LineStart->x ) * ( LineEnd->x - LineStart->x ) ) +
451 ( ( Point->y - LineStart->y ) * ( LineEnd->y - LineStart->y ) ) +
452 ( ( Point->z - LineStart->z ) * ( LineEnd->z - LineStart->z ) ) ) /
453 ( LineMag * LineMag );
455 if( U < 0.0f || U > 1.0f )
456 return 0; // closest point does not fall within the line segment
458 Intersection->x = LineStart->x + U * ( LineEnd->x - LineStart->x );
459 Intersection->y = LineStart->y + U * ( LineEnd->y - LineStart->y );
460 Intersection->z = LineStart->z + U * ( LineEnd->z - LineStart->z );
462 *Distance = findDistance( Point, Intersection );