]> git.jsancho.org Git - lugaru.git/blob - Source/Math/XYZ.hpp
In case the resolution settings in the config file have wrong values,
[lugaru.git] / Source / Math / XYZ.hpp
1 /*
2 Copyright (C) 2003, 2010 - Wolfire Games
3 Copyright (C) 2010-2017 - 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 #ifndef _QUATERNIONS_HPP_
22 #define _QUATERNIONS_HPP_
23
24 #include "Graphic/gamegl.hpp"
25
26 #include <math.h>
27
28 class XYZ
29 {
30 public:
31     float x;
32     float y;
33     float z;
34     XYZ()
35         : x(0.0f)
36         , y(0.0f)
37         , z(0.0f)
38     {
39     }
40     inline XYZ operator+(XYZ add);
41     inline XYZ operator-(XYZ add);
42     inline XYZ operator*(float add);
43     inline XYZ operator*(XYZ add);
44     inline XYZ operator/(float add);
45     inline void operator+=(XYZ add);
46     inline void operator-=(XYZ add);
47     inline void operator*=(float add);
48     inline void operator*=(XYZ add);
49     inline void operator/=(float add);
50     inline void operator=(float add);
51     inline bool operator==(XYZ add);
52 };
53
54 inline void CrossProduct(XYZ* P, XYZ* Q, XYZ* V);
55 inline void CrossProduct(XYZ P, XYZ Q, XYZ* V);
56 inline void Normalise(XYZ* vectory);
57 inline float normaldotproduct(XYZ point1, XYZ point2);
58 inline float fast_sqrt(register float arg);
59 bool PointInTriangle(XYZ* p, XYZ normal, XYZ* p1, XYZ* p2, XYZ* p3);
60 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p);
61 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p);
62 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ* p);
63 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* n, XYZ* p);
64 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* p);
65 inline void ReflectVector(XYZ* vel, const XYZ* n);
66 inline void ReflectVector(XYZ* vel, const XYZ& n);
67 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
68 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
69 inline float findDistance(XYZ* point1, XYZ* point2);
70 inline float findLength(XYZ* point1);
71 inline float findLengthfast(XYZ* point1);
72 inline float distsq(XYZ* point1, XYZ* point2);
73 inline float distsq(XYZ point1, XYZ point2);
74 inline float distsqflat(XYZ* point1, XYZ* point2);
75 inline float dotproduct(const XYZ* point1, const XYZ* point2);
76 bool sphere_line_intersection(
77     float x1, float y1, float z1,
78     float x2, float y2, float z2,
79     float x3, float y3, float z3, float r);
80 bool sphere_line_intersection(
81     XYZ* p1, XYZ* p2, XYZ* p3, float* r);
82 inline bool DistancePointLine(XYZ* Point, XYZ* LineStart, XYZ* LineEnd, float* Distance, XYZ* Intersection);
83
84 inline void Normalise(XYZ* vectory)
85 {
86     static float d;
87     d = fast_sqrt(vectory->x * vectory->x + vectory->y * vectory->y + vectory->z * vectory->z);
88     if (d == 0) {
89         return;
90     }
91     vectory->x /= d;
92     vectory->y /= d;
93     vectory->z /= d;
94 }
95
96 inline XYZ XYZ::operator+(XYZ add)
97 {
98     static XYZ ne;
99     ne = add;
100     ne.x += x;
101     ne.y += y;
102     ne.z += z;
103     return ne;
104 }
105
106 inline XYZ XYZ::operator-(XYZ add)
107 {
108     static XYZ ne;
109     ne = add;
110     ne.x = x - ne.x;
111     ne.y = y - ne.y;
112     ne.z = z - ne.z;
113     return ne;
114 }
115
116 inline XYZ XYZ::operator*(float add)
117 {
118     static XYZ ne;
119     ne.x = x * add;
120     ne.y = y * add;
121     ne.z = z * add;
122     return ne;
123 }
124
125 inline XYZ XYZ::operator*(XYZ add)
126 {
127     static XYZ ne;
128     ne.x = x * add.x;
129     ne.y = y * add.y;
130     ne.z = z * add.z;
131     return ne;
132 }
133
134 inline XYZ XYZ::operator/(float add)
135 {
136     static XYZ ne;
137     ne.x = x / add;
138     ne.y = y / add;
139     ne.z = z / add;
140     return ne;
141 }
142
143 inline void XYZ::operator+=(XYZ add)
144 {
145     x += add.x;
146     y += add.y;
147     z += add.z;
148 }
149
150 inline void XYZ::operator-=(XYZ add)
151 {
152     x = x - add.x;
153     y = y - add.y;
154     z = z - add.z;
155 }
156
157 inline void XYZ::operator*=(float add)
158 {
159     x = x * add;
160     y = y * add;
161     z = z * add;
162 }
163
164 inline void XYZ::operator*=(XYZ add)
165 {
166     x = x * add.x;
167     y = y * add.y;
168     z = z * add.z;
169 }
170
171 inline void XYZ::operator/=(float add)
172 {
173     x = x / add;
174     y = y / add;
175     z = z / add;
176 }
177
178 inline void XYZ::operator=(float add)
179 {
180     x = add;
181     y = add;
182     z = add;
183 }
184
185 inline bool XYZ::operator==(XYZ add)
186 {
187     if (x == add.x && y == add.y && z == add.z)
188         return 1;
189     return 0;
190 }
191
192 inline void CrossProduct(XYZ* P, XYZ* Q, XYZ* V)
193 {
194     V->x = P->y * Q->z - P->z * Q->y;
195     V->y = P->z * Q->x - P->x * Q->z;
196     V->z = P->x * Q->y - P->y * Q->x;
197 }
198
199 inline void CrossProduct(XYZ P, XYZ Q, XYZ* V)
200 {
201     V->x = P.y * Q.z - P.z * Q.y;
202     V->y = P.z * Q.x - P.x * Q.z;
203     V->z = P.x * Q.y - P.y * Q.x;
204 }
205
206 inline float fast_sqrt(register float arg)
207 {
208     return sqrtf(arg);
209 }
210
211 inline float normaldotproduct(XYZ point1, XYZ point2)
212 {
213     static GLfloat returnvalue;
214     Normalise(&point1);
215     Normalise(&point2);
216     returnvalue = (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
217     return returnvalue;
218 }
219
220 inline void ReflectVector(XYZ* vel, const XYZ* n)
221 {
222     ReflectVector(vel, *n);
223 }
224
225 inline void ReflectVector(XYZ* vel, const XYZ& n)
226 {
227     static XYZ vn;
228     static XYZ vt;
229     static float dotprod;
230
231     dotprod = dotproduct(&n, vel);
232     vn.x = n.x * dotprod;
233     vn.y = n.y * dotprod;
234     vn.z = n.z * dotprod;
235
236     vt.x = vel->x - vn.x;
237     vt.y = vel->y - vn.y;
238     vt.z = vel->z - vn.z;
239
240     vel->x = vt.x - vn.x;
241     vel->y = vt.y - vn.y;
242     vel->z = vt.z - vn.z;
243 }
244
245 inline float dotproduct(const XYZ* point1, const XYZ* point2)
246 {
247     static GLfloat returnvalue;
248     returnvalue = (point1->x * point2->x + point1->y * point2->y + point1->z * point2->z);
249     return returnvalue;
250 }
251
252 inline float findDistance(XYZ* point1, XYZ* point2)
253 {
254     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)));
255 }
256
257 inline float findLength(XYZ* point1)
258 {
259     return (fast_sqrt((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z)));
260 }
261
262 inline float findLengthfast(XYZ* point1)
263 {
264     return ((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z));
265 }
266
267 inline float distsq(XYZ* point1, XYZ* point2)
268 {
269     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));
270 }
271
272 inline float distsq(XYZ point1, XYZ point2)
273 {
274     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));
275 }
276
277 inline float distsqflat(XYZ* point1, XYZ* point2)
278 {
279     return ((point1->x - point2->x) * (point1->x - point2->x) + (point1->z - point2->z) * (point1->z - point2->z));
280 }
281
282 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang)
283 {
284     static XYZ newpoint;
285     if (xang) {
286         xang *= 6.283185f;
287         xang /= 360;
288     }
289     if (yang) {
290         yang *= 6.283185f;
291         yang /= 360;
292     }
293     if (zang) {
294         zang *= 6.283185f;
295         zang /= 360;
296     }
297
298     if (yang) {
299         newpoint.z = thePoint.z * cosf(yang) - thePoint.x * sinf(yang);
300         newpoint.x = thePoint.z * sinf(yang) + thePoint.x * cosf(yang);
301         thePoint.z = newpoint.z;
302         thePoint.x = newpoint.x;
303     }
304
305     if (zang) {
306         newpoint.x = thePoint.x * cosf(zang) - thePoint.y * sinf(zang);
307         newpoint.y = thePoint.y * cosf(zang) + thePoint.x * sinf(zang);
308         thePoint.x = newpoint.x;
309         thePoint.y = newpoint.y;
310     }
311
312     if (xang) {
313         newpoint.y = thePoint.y * cosf(xang) - thePoint.z * sinf(xang);
314         newpoint.z = thePoint.y * sinf(xang) + thePoint.z * cosf(xang);
315         thePoint.z = newpoint.z;
316         thePoint.y = newpoint.y;
317     }
318
319     return thePoint;
320 }
321
322 inline float square(float f)
323 {
324     return (f * f);
325 }
326
327 inline bool sphere_line_intersection(
328     float x1, float y1, float z1,
329     float x2, float y2, float z2,
330     float x3, float y3, float z3, float r)
331 {
332
333     // x1,y1,z1  P1 coordinates (point of line)
334     // x2,y2,z2  P2 coordinates (point of line)
335     // x3,y3,z3, r  P3 coordinates and radius (sphere)
336     // x,y,z   intersection coordinates
337     //
338     // This function returns a pointer array which first index indicates
339     // the number of intersection point, followed by coordinate pairs.
340
341     //~ static float x , y , z;
342     static float a, b, c, /*mu,*/ i;
343
344     if (x1 > x3 + r && x2 > x3 + r)
345         return (0);
346     if (x1 < x3 - r && x2 < x3 - r)
347         return (0);
348     if (y1 > y3 + r && y2 > y3 + r)
349         return (0);
350     if (y1 < y3 - r && y2 < y3 - r)
351         return (0);
352     if (z1 > z3 + r && z2 > z3 + r)
353         return (0);
354     if (z1 < z3 - r && z2 < z3 - r)
355         return (0);
356     a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
357     b = 2 * ((x2 - x1) * (x1 - x3) + (y2 - y1) * (y1 - y3) + (z2 - z1) * (z1 - z3));
358     c = square(x3) + square(y3) +
359         square(z3) + square(x1) +
360         square(y1) + square(z1) -
361         2 * (x3 * x1 + y3 * y1 + z3 * z1) - square(r);
362     i = b * b - 4 * a * c;
363
364     if (i < 0.0) {
365         // no intersection
366         return (0);
367     }
368     return (1);
369 }
370
371 inline bool sphere_line_intersection(
372     XYZ* p1, XYZ* p2, XYZ* p3, float* r)
373 {
374
375     // x1,p1->y,p1->z  P1 coordinates (point of line)
376     // p2->x,p2->y,p2->z  P2 coordinates (point of line)
377     // p3->x,p3->y,p3->z, r  P3 coordinates and radius (sphere)
378     // x,y,z   intersection coordinates
379     //
380     // This function returns a pointer array which first index indicates
381     // the number of intersection point, followed by coordinate pairs.
382
383     //~ static float x , y , z;
384     static float a, b, c, /*mu,*/ i;
385
386     if (p1->x > p3->x + *r && p2->x > p3->x + *r)
387         return (0);
388     if (p1->x < p3->x - *r && p2->x < p3->x - *r)
389         return (0);
390     if (p1->y > p3->y + *r && p2->y > p3->y + *r)
391         return (0);
392     if (p1->y < p3->y - *r && p2->y < p3->y - *r)
393         return (0);
394     if (p1->z > p3->z + *r && p2->z > p3->z + *r)
395         return (0);
396     if (p1->z < p3->z - *r && p2->z < p3->z - *r)
397         return (0);
398     a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
399     b = 2 * ((p2->x - p1->x) * (p1->x - p3->x) + (p2->y - p1->y) * (p1->y - p3->y) + (p2->z - p1->z) * (p1->z - p3->z));
400     c = square(p3->x) + square(p3->y) +
401         square(p3->z) + square(p1->x) +
402         square(p1->y) + square(p1->z) -
403         2 * (p3->x * p1->x + p3->y * p1->y + p3->z * p1->z) - square(*r);
404     i = b * b - 4 * a * c;
405
406     if (i < 0.0) {
407         // no intersection
408         return (0);
409     }
410     return (1);
411 }
412
413 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang)
414 {
415     static XYZ newpoint;
416     static XYZ oldpoint;
417
418     oldpoint = thePoint;
419
420     if (yang != 0) {
421         newpoint.z = oldpoint.z * cosf(yang) - oldpoint.x * sinf(yang);
422         newpoint.x = oldpoint.z * sinf(yang) + oldpoint.x * cosf(yang);
423         oldpoint.z = newpoint.z;
424         oldpoint.x = newpoint.x;
425     }
426
427     if (zang != 0) {
428         newpoint.x = oldpoint.x * cosf(zang) - oldpoint.y * sinf(zang);
429         newpoint.y = oldpoint.y * cosf(zang) + oldpoint.x * sinf(zang);
430         oldpoint.x = newpoint.x;
431         oldpoint.y = newpoint.y;
432     }
433
434     if (xang != 0) {
435         newpoint.y = oldpoint.y * cosf(xang) - oldpoint.z * sinf(xang);
436         newpoint.z = oldpoint.y * sinf(xang) + oldpoint.z * cosf(xang);
437         oldpoint.z = newpoint.z;
438         oldpoint.y = newpoint.y;
439     }
440
441     return oldpoint;
442 }
443
444 inline bool DistancePointLine(XYZ* Point, XYZ* LineStart, XYZ* LineEnd, float* Distance, XYZ* Intersection)
445 {
446     float LineMag;
447     float U;
448
449     LineMag = findDistance(LineEnd, LineStart);
450
451     U = (((Point->x - LineStart->x) * (LineEnd->x - LineStart->x)) +
452          ((Point->y - LineStart->y) * (LineEnd->y - LineStart->y)) +
453          ((Point->z - LineStart->z) * (LineEnd->z - LineStart->z))) /
454         (LineMag * LineMag);
455
456     if (U < 0.0f || U > 1.0f)
457         return 0; // closest point does not fall within the line segment
458
459     Intersection->x = LineStart->x + U * (LineEnd->x - LineStart->x);
460     Intersection->y = LineStart->y + U * (LineEnd->y - LineStart->y);
461     Intersection->z = LineStart->z + U * (LineEnd->z - LineStart->z);
462
463     *Distance = findDistance(Point, Intersection);
464
465     return 1;
466 }
467
468 #endif