]> git.jsancho.org Git - lugaru.git/blob - Source/Quaternions.h
Added a class for Hotspots to avoid using several arrays
[lugaru.git] / 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