1 /*
2 * Quaterions.cpp
3 * Copyright (C) 2007 by Bryan Duff <duff0097@gmail.com>
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
18 * USA
19 */
20 #include "Quaternions.h"
21
22 // Functions
Quat_Mult(quaternion q1,quaternion q2)23 quaternion Quat_Mult(quaternion q1, quaternion q2)
24 {
25 quaternion QResult;
26 float a, b, c, d, e, f, g, h;
27 a = (q1.w + q1.x) * (q2.w + q2.x);
28 b = (q1.z - q1.y) * (q2.y - q2.z);
29 c = (q1.w - q1.x) * (q2.y + q2.z);
30 d = (q1.y + q1.z) * (q2.w - q2.x);
31 e = (q1.x + q1.z) * (q2.x + q2.y);
32 f = (q1.x - q1.z) * (q2.x - q2.y);
33 g = (q1.w + q1.y) * (q2.w - q2.z);
34 h = (q1.w - q1.y) * (q2.w + q2.z);
35 QResult.w = b + (-e - f + g + h) / 2;
36 QResult.x = a - (e + f + g + h) / 2;
37 QResult.y = c + (e - f + g - h) / 2;
38 QResult.z = d + (e - f - g + h) / 2;
39 return QResult;
40 }
41
operator +(XYZ add)42 XYZ XYZ::operator+(XYZ add)
43 {
44 XYZ ne;
45 ne = add;
46 ne.x += x;
47 ne.y += y;
48 ne.z += z;
49 return ne;
50 }
51
operator -(XYZ add)52 XYZ XYZ::operator-(XYZ add)
53 {
54 XYZ ne;
55 ne = add;
56 ne.x = x - ne.x;
57 ne.y = y - ne.y;
58 ne.z = z - ne.z;
59 return ne;
60 }
61
operator *(float add)62 XYZ XYZ::operator*(float add)
63 {
64 XYZ ne;
65 ne.x = x * add;
66 ne.y = y * add;
67 ne.z = z * add;
68 return ne;
69 }
70
operator *(XYZ add)71 XYZ XYZ::operator*(XYZ add)
72 {
73 XYZ ne;
74 ne.x = x * add.x;
75 ne.y = y * add.y;
76 ne.z = z * add.z;
77 return ne;
78 }
79
operator /(float add)80 XYZ XYZ::operator/(float add)
81 {
82 XYZ ne;
83 ne.x = x / add;
84 ne.y = y / add;
85 ne.z = z / add;
86 return ne;
87 }
88
operator +=(XYZ add)89 void XYZ::operator+=(XYZ add)
90 {
91 x += add.x;
92 y += add.y;
93 z += add.z;
94 }
95
operator -=(XYZ add)96 void XYZ::operator-=(XYZ add)
97 {
98 x = x - add.x;
99 y = y - add.y;
100 z = z - add.z;
101 }
102
operator *=(float add)103 void XYZ::operator*=(float add)
104 {
105 x = x * add;
106 y = y * add;
107 z = z * add;
108 }
109
operator *=(XYZ add)110 void XYZ::operator*=(XYZ add)
111 {
112 x = x * add.x;
113 y = y * add.y;
114 z = z * add.z;
115 }
116
operator /=(float add)117 void XYZ::operator/=(float add)
118 {
119 x = x / add;
120 y = y / add;
121 z = z / add;
122 }
123
operator =(float add)124 void XYZ::operator=(float add)
125 {
126 x = add;
127 y = add;
128 z = add;
129 }
130
vec(Vector add)131 void XYZ::vec(Vector add)
132 {
133 x = add.x;
134 y = add.y;
135 z = add.z;
136 }
137
operator ==(XYZ add)138 bool XYZ::operator==(XYZ add)
139 {
140 if(x == add.x && y == add.y && z == add.z)
141 return 1;
142 return 0;
143 }
144
To_Quat(Matrix_t m)145 quaternion To_Quat(Matrix_t m)
146 {
147 // From Jason Shankel, (C) 2000.
148 quaternion Quat;
149
150 double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD;
151 double q[4];
152
153 int i, j, k;
154 if(Tr >= 1.0) {
155 fourD = 2.0 * fast_sqrt(Tr);
156 q[3] = fourD / 4.0;
157 q[0] = (m[2][1] - m[1][2]) / fourD;
158 q[1] = (m[0][2] - m[2][0]) / fourD;
159 q[2] = (m[1][0] - m[0][1]) / fourD;
160 } else {
161 if(m[0][0] > m[1][1]) {
162 i = 0;
163 } else {
164 i = 1;
165 }
166 if(m[2][2] > m[i][i]) {
167 i = 2;
168 }
169 j = (i + 1) % 3;
170 k = (j + 1) % 3;
171 fourD = 2.0 * fast_sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
172 q[i] = fourD / 4.0;
173 q[j] = (m[j][i] + m[i][j]) / fourD;
174 q[k] = (m[k][i] + m[i][k]) / fourD;
175 q[3] = (m[j][k] - m[k][j]) / fourD;
176 }
177
178 Quat.x = q[0];
179 Quat.y = q[1];
180 Quat.z = q[2];
181 Quat.w = q[3];
182 return Quat;
183 }
Quat_2_Matrix(quaternion Quat,Matrix_t m)184 void Quat_2_Matrix(quaternion Quat, Matrix_t m)
185 {
186 // From the GLVelocity site (http://glvelocity.gamedev.net)
187 float fW = Quat.w;
188 float fX = Quat.x;
189 float fY = Quat.y;
190 float fZ = Quat.z;
191 float fXX = fX * fX;
192 float fYY = fY * fY;
193 float fZZ = fZ * fZ;
194 m[0][0] = 1.0f - 2.0f * (fYY + fZZ);
195 m[1][0] = 2.0f * (fX * fY + fW * fZ);
196 m[2][0] = 2.0f * (fX * fZ - fW * fY);
197 m[3][0] = 0.0f;
198 m[0][1] = 2.0f * (fX * fY - fW * fZ);
199 m[1][1] = 1.0f - 2.0f * (fXX + fZZ);
200 m[2][1] = 2.0f * (fY * fZ + fW * fX);
201 m[3][1] = 0.0f;
202 m[0][2] = 2.0f * (fX * fZ + fW * fY);
203 m[1][2] = 2.0f * (fX * fZ - fW * fX);
204 m[2][2] = 1.0f - 2.0f * (fXX + fYY);
205 m[3][2] = 0.0f;
206 m[0][3] = 0.0f;
207 m[1][3] = 0.0f;
208 m[2][3] = 0.0f;
209 m[3][3] = 1.0f;
210 }
To_Quat(angle_axis Ang_Ax)211 quaternion To_Quat(angle_axis Ang_Ax)
212 {
213 // From the Quaternion Powers article on gamedev.net
214 quaternion Quat;
215
216 Quat.x = Ang_Ax.x * sin(Ang_Ax.angle / 2);
217 Quat.y = Ang_Ax.y * sin(Ang_Ax.angle / 2);
218 Quat.z = Ang_Ax.z * sin(Ang_Ax.angle / 2);
219 Quat.w = cos(Ang_Ax.angle / 2);
220 return Quat;
221 }
Quat_2_AA(quaternion Quat)222 angle_axis Quat_2_AA(quaternion Quat)
223 {
224 angle_axis Ang_Ax;
225 float scale, tw;
226 tw = (float)acos(Quat.w) * 2;
227 scale = (float)sin(tw / 2.0);
228 Ang_Ax.x = Quat.x / scale;
229 Ang_Ax.y = Quat.y / scale;
230 Ang_Ax.z = Quat.z / scale;
231
232 Ang_Ax.angle = 2.0 * acos(Quat.w) / (float)PI *180;
233 return Ang_Ax;
234 }
235
To_Quat(int In_Degrees,euler Euler)236 quaternion To_Quat(int In_Degrees, euler Euler)
237 {
238 // From the gamasutra quaternion article
239 quaternion Quat;
240 float cr, cp, cy, sr, sp, sy, cpcy, spsy;
241 //If we are in Degree mode, convert to Radians
242 if(In_Degrees) {
243 Euler.x = Euler.x * (float)PI / 180;
244 Euler.y = Euler.y * (float)PI / 180;
245 Euler.z = Euler.z * (float)PI / 180;
246 }
247 //Calculate trig identities
248 //Formerly roll, pitch, yaw
249 cr = float (cos(Euler.x / 2));
250 cp = float (cos(Euler.y / 2));
251 cy = float (cos(Euler.z / 2));
252 sr = float (sin(Euler.x / 2));
253 sp = float (sin(Euler.y / 2));
254 sy = float (sin(Euler.z / 2));
255
256 cpcy = cp * cy;
257 spsy = sp * sy;
258 Quat.w = cr * cpcy + sr * spsy;
259 Quat.x = sr * cpcy - cr * spsy;
260 Quat.y = cr * sp * cy + sr * cp * sy;
261 Quat.z = cr * cp * sy - sr * sp * cy;
262
263 return Quat;
264 }
265
QNormalize(quaternion Quat)266 quaternion QNormalize(quaternion Quat)
267 {
268 float norm;
269 norm = Quat.x * Quat.x + Quat.y * Quat.y + Quat.z * Quat.z + Quat.w * Quat.w;
270 Quat.x = float (Quat.x / norm);
271 Quat.y = float (Quat.y / norm);
272 Quat.z = float (Quat.z / norm);
273 Quat.w = float (Quat.w / norm);
274 return Quat;
275 }
276
Quat2Vector(quaternion Quat)277 XYZ Quat2Vector(quaternion Quat)
278 {
279 QNormalize(Quat);
280
281 float fW = Quat.w;
282 float fX = Quat.x;
283 float fY = Quat.y;
284 float fZ = Quat.z;
285
286 XYZ tempvec;
287
288 tempvec.x = 2.0f * (fX * fZ - fW * fY);
289 tempvec.y = 2.0f * (fY * fZ + fW * fX);
290 tempvec.z = 1.0f - 2.0f * (fX * fX + fY * fY);
291
292 return tempvec;
293 }
294
CrossProduct(XYZ P,XYZ Q,XYZ * V)295 void CrossProduct(XYZ P, XYZ Q, XYZ * V)
296 {
297 V->x = P.y * Q.z - P.z * Q.y;
298 V->y = P.z * Q.x - P.x * Q.z;
299 V->z = P.x * Q.y - P.y * Q.x;
300 }
301
Normalise(XYZ * vectory)302 void Normalise(XYZ * vectory)
303 {
304 float d =
305 fast_sqrt(vectory->x * vectory->x + vectory->y * vectory->y +
306 vectory->z * vectory->z);
307 if(d == 0) {
308 return;
309 }
310 vectory->x /= d;
311 vectory->y /= d;
312 vectory->z /= d;
313 }
314
fast_sqrt(register float arg)315 float fast_sqrt(register float arg)
316 {
317 return sqrt(arg);
318 }
319
normaldotproduct(XYZ point1,XYZ point2)320 float normaldotproduct(XYZ point1, XYZ point2)
321 {
322 GLfloat returnvalue;
323 Normalise(&point1);
324 Normalise(&point2);
325 returnvalue =
326 (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
327 return returnvalue;
328 }
329
330 extern float u0, u1, u2;
331 extern float v0, v1, v2;
332 extern float a, b;
333 extern float max;
334 extern int i, j;
335 extern bool bInter;
336 extern float pointv[3];
337 extern float p1v[3];
338 extern float p2v[3];
339 extern float p3v[3];
340 extern float normalv[3];
341
PointInTriangle(Vector * p,Vector normal,float p11,float p12,float p13,float p21,float p22,float p23,float p31,float p32,float p33)342 bool PointInTriangle(Vector * p, Vector normal, float p11, float p12, float p13,
343 float p21, float p22, float p23, float p31, float p32,
344 float p33)
345 {
346 bInter = 0;
347
348 pointv[0] = p->x;
349 pointv[1] = p->y;
350 pointv[2] = p->z;
351
352
353 p1v[0] = p11;
354 p1v[1] = p12;
355 p1v[2] = p13;
356
357 p2v[0] = p21;
358 p2v[1] = p22;
359 p2v[2] = p23;
360
361 p3v[0] = p31;
362 p3v[1] = p32;
363 p3v[2] = p33;
364
365 normalv[0] = normal.x;
366 normalv[1] = normal.y;
367 normalv[2] = normal.z;
368
369 #define ABS(X) (((X)<0.f)?-(X):(X) )
370 #define MAX(A, B) (((A)<(B))?(B):(A))
371 max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
372 #undef MAX
373 if(max == ABS(normalv[0])) {
374 i = 1;
375 j = 2;
376 } // y, z
377 if(max == ABS(normalv[1])) {
378 i = 0;
379 j = 2;
380 } // x, z
381 if(max == ABS(normalv[2])) {
382 i = 0;
383 j = 1;
384 } // x, y
385 #undef ABS
386
387 u0 = pointv[i] - p1v[i];
388 v0 = pointv[j] - p1v[j];
389 u1 = p2v[i] - p1v[i];
390 v1 = p2v[j] - p1v[j];
391 u2 = p3v[i] - p1v[i];
392 v2 = p3v[j] - p1v[j];
393
394 if(u1 > -1.0e-05f && u1 < 1.0e-05f) // == 0.0f)
395 {
396 b = u0 / u2;
397 if(0.0f <= b && b <= 1.0f) {
398 a = (v0 - b * v2) / v1;
399 if((a >= 0.0f) && ((a + b) <= 1.0f))
400 bInter = 1;
401 }
402 } else {
403 b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
404 if(0.0f <= b && b <= 1.0f) {
405 a = (u0 - b * u2) / u1;
406 if((a >= 0.0f) && ((a + b) <= 1.0f))
407 bInter = 1;
408 }
409 }
410
411 return bInter;
412 }
413
LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector * p)414 bool LineFacet(Vector p1, Vector p2, Vector pa, Vector pb, Vector pc,
415 Vector * p)
416 {
417 float d;
418 float denom, mu;
419 Vector n, pa1, pa2, pa3;
420
421 //Calculate the parameters for the plane
422 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
423 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
424 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
425 n.Normalize();
426 d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
427
428 //Calculate the position on the line that intersects the plane
429 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
430 if(abs(denom) < 0.0000001) // Line and plane don't intersect
431 return 0;
432 mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
433 p->x = p1.x + mu * (p2.x - p1.x);
434 p->y = p1.y + mu * (p2.y - p1.y);
435 p->z = p1.z + mu * (p2.z - p1.z);
436 if(mu < 0 || mu > 1) // Intersection not along line segment
437 return 0;
438
439 if(!PointInTriangle
440 (p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)) {
441 return 0;
442 }
443
444 return 1;
445 }
446
PointInTriangle(XYZ * p,XYZ normal,XYZ * p1,XYZ * p2,XYZ * p3)447 bool PointInTriangle(XYZ * p, XYZ normal, XYZ * p1, XYZ * p2, XYZ * p3)
448 {
449 bInter = 0;
450
451 pointv[0] = p->x;
452 pointv[1] = p->y;
453 pointv[2] = p->z;
454
455
456 p1v[0] = p1->x;
457 p1v[1] = p1->y;
458 p1v[2] = p1->z;
459
460 p2v[0] = p2->x;
461 p2v[1] = p2->y;
462 p2v[2] = p2->z;
463
464 p3v[0] = p3->x;
465 p3v[1] = p3->y;
466 p3v[2] = p3->z;
467
468 normalv[0] = normal.x;
469 normalv[1] = normal.y;
470 normalv[2] = normal.z;
471
472 #define ABS(X) (((X)<0.f)?-(X):(X) )
473 #define MAX(A, B) (((A)<(B))?(B):(A))
474 max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
475 #undef MAX
476 if(max == ABS(normalv[0])) {
477 i = 1;
478 j = 2;
479 } // y, z
480 if(max == ABS(normalv[1])) {
481 i = 0;
482 j = 2;
483 } // x, z
484 if(max == ABS(normalv[2])) {
485 i = 0;
486 j = 1;
487 } // x, y
488 #undef ABS
489
490 u0 = pointv[i] - p1v[i];
491 v0 = pointv[j] - p1v[j];
492 u1 = p2v[i] - p1v[i];
493 v1 = p2v[j] - p1v[j];
494 u2 = p3v[i] - p1v[i];
495 v2 = p3v[j] - p1v[j];
496
497 if(u1 > -1.0e-05f && u1 < 1.0e-05f) // == 0.0f)
498 {
499 b = u0 / u2;
500 if(0.0f <= b && b <= 1.0f) {
501 a = (v0 - b * v2) / v1;
502 if((a >= 0.0f) && ((a + b) <= 1.0f))
503 bInter = 1;
504 }
505 } else {
506 b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
507 if(0.0f <= b && b <= 1.0f) {
508 a = (u0 - b * u2) / u1;
509 if((a >= 0.0f) && ((a + b) <= 1.0f))
510 bInter = 1;
511 }
512 }
513
514 return bInter;
515 }
516
LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ * p)517 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ * p)
518 {
519 float d;
520 float denom, mu;
521 XYZ n;
522
523 //Calculate the parameters for the plane
524 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
525 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
526 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
527 Normalise(&n);
528 d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
529
530 //Calculate the position on the line that intersects the plane
531 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
532 if(abs(denom) < 0.0000001) // Line and plane don't intersect
533 return 0;
534 mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
535 p->x = p1.x + mu * (p2.x - p1.x);
536 p->y = p1.y + mu * (p2.y - p1.y);
537 p->z = p1.z + mu * (p2.z - p1.z);
538 if(mu < 0 || mu > 1) // Intersection not along line segment
539 return 0;
540
541 if(!PointInTriangle(p, n, &pa, &pb, &pc)) {
542 return 0;
543 }
544
545 return 1;
546 }
547
548 extern float d;
549 extern float a1, a2, a3;
550 extern float total, denom, mu;
551 extern XYZ pa1, pa2, pa3, n;
552
LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ * p)553 bool LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ * p)
554 {
555
556 //Calculate the parameters for the plane
557 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
558 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
559 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
560 Normalise(&n);
561 d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
562
563 //Calculate the position on the line that intersects the plane
564 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
565 if(abs(denom) < 0.0000001) // Line and plane don't intersect
566 return 0;
567 mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
568 p->x = p1.x + mu * (p2.x - p1.x);
569 p->y = p1.y + mu * (p2.y - p1.y);
570 p->z = p1.z + mu * (p2.z - p1.z);
571 if(mu < 0 || mu > 1) // Intersection not along line segment
572 return 0;
573
574 if(!PointInTriangle(p, n, &pa, &pb, &pc)) {
575 return 0;
576 }
577
578 return 1;
579 }
580
LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ n,XYZ * p)581 bool LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ * p)
582 {
583
584 //Calculate the parameters for the plane
585 d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
586
587 //Calculate the position on the line that intersects the plane
588 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
589 if(abs(denom) < 0.0000001) // Line and plane don't intersect
590 return 0;
591 mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
592 p->x = p1.x + mu * (p2.x - p1.x);
593 p->y = p1.y + mu * (p2.y - p1.y);
594 p->z = p1.z + mu * (p2.z - p1.z);
595 if(mu < 0 || mu > 1) // Intersection not along line segment
596 return 0;
597
598 if(!PointInTriangle(p, n, &pa, &pb, &pc)) {
599 return 0;
600 }
601 return 1;
602 }
603
604 // Does the line intersect with a triangle?
605 // Find the intersection point on the plane, is that point in the triangle?
LineFacetd(XYZ * p1,XYZ * p2,XYZ * pa,XYZ * pb,XYZ * pc,XYZ * n,XYZ * p)606 bool LineFacetd(XYZ * p1, XYZ * p2, XYZ * pa, XYZ * pb, XYZ * pc, XYZ * n,
607 XYZ * p)
608 {
609
610 //Calculate the parameters for the plane
611 d = -n->x * pa->x - n->y * pa->y - n->z * pa->z;
612
613 //Calculate the position on the line that intersects the plane
614 denom =
615 n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z);
616 if(abs(denom) < 0.0000001) // Line and plane don't intersect
617 return 0;
618 mu = -(d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom;
619 p->x = p1->x + mu * (p2->x - p1->x);
620 p->y = p1->y + mu * (p2->y - p1->y);
621 p->z = p1->z + mu * (p2->z - p1->z);
622 if(mu < 0 || mu > 1) // Intersection not along line segment
623 return 0;
624
625 if(!PointInTriangle(p, *n, pa, pb, pc)) {
626 return 0;
627 }
628 return 1;
629 }
630
ReflectVector(XYZ * vel,XYZ * n)631 void ReflectVector(XYZ * vel, XYZ * n)
632 {
633 XYZ vn;
634 XYZ vt;
635 float dotprod;
636
637 dotprod = dotproduct(*n, *vel);
638 vn.x = n->x * dotprod;
639 vn.y = n->y * dotprod;
640 vn.z = n->z * dotprod;
641
642 vt.x = vel->x - vn.x;
643 vt.y = vel->y - vn.y;
644 vt.z = vel->z - vn.z;
645
646 vel->x = vt.x - vn.x;
647 vel->y = vt.y - vn.y;
648 vel->z = vt.z - vn.z;
649 }
650
dotproduct(XYZ point1,XYZ point2)651 float dotproduct(XYZ point1, XYZ point2)
652 {
653 GLfloat returnvalue;
654 returnvalue =
655 (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
656 return returnvalue;
657 }
658
findDistance(XYZ point1,XYZ point2)659 float findDistance(XYZ point1, XYZ point2)
660 {
661 return (fast_sqrt
662 ((point1.x - point2.x) * (point1.x - point2.x) +
663 (point1.y - point2.y) * (point1.y - point2.y) + (point1.z -
664 point2.z) *
665 (point1.z - point2.z)));
666 }
667
findLength(XYZ point1)668 float findLength(XYZ point1)
669 {
670 return (fast_sqrt
671 ((point1.x) * (point1.x) + (point1.y) * (point1.y) +
672 (point1.z) * (point1.z)));
673 }
674
675
findLengthfast(XYZ point1)676 float findLengthfast(XYZ point1)
677 {
678 return ((point1.x) * (point1.x) + (point1.y) * (point1.y) +
679 (point1.z) * (point1.z));
680 }
681
findDistancefast(XYZ point1,XYZ point2)682 float findDistancefast(XYZ point1, XYZ point2)
683 {
684 return ((point1.x - point2.x) * (point1.x - point2.x) +
685 (point1.y - point2.y) * (point1.y - point2.y) + (point1.z -
686 point2.z) *
687 (point1.z - point2.z));
688 }
689
DoRotation(XYZ thePoint,float xang,float yang,float zang)690 XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang)
691 {
692 XYZ newpoint;
693 if(xang) {
694 xang *= 6.283185;
695 xang /= 360;
696 }
697 if(yang) {
698 yang *= 6.283185;
699 yang /= 360;
700 }
701 if(zang) {
702 zang *= 6.283185;
703 zang /= 360;
704 }
705
706 if(xang) {
707 newpoint.y = thePoint.y * cos(xang) - thePoint.z * sin(xang);
708 newpoint.z = thePoint.y * sin(xang) + thePoint.z * cos(xang);
709 thePoint.z = newpoint.z;
710 thePoint.y = newpoint.y;
711 }
712
713 if(yang) {
714 newpoint.z = thePoint.z * cos(yang) - thePoint.x * sin(yang);
715 newpoint.x = thePoint.z * sin(yang) + thePoint.x * cos(yang);
716 thePoint.z = newpoint.z;
717 thePoint.x = newpoint.x;
718 }
719
720 if(zang) {
721 newpoint.x = thePoint.x * cos(zang) - thePoint.y * sin(zang);
722 newpoint.y = thePoint.y * cos(zang) + thePoint.x * sin(zang);
723 thePoint.x = newpoint.x;
724 thePoint.y = newpoint.y;
725 }
726
727 return thePoint;
728 }
729
square(float f)730 inline float square(float f)
731 {
732 return (f * f);
733 }
734
sphere_line_intersection(float x1,float y1,float z1,float x2,float y2,float z2,float x3,float y3,float z3,float r)735 bool sphere_line_intersection(float x1, float y1, float z1,
736 float x2, float y2, float z2,
737 float x3, float y3, float z3, float r)
738 {
739
740 // x1,y1,z1 P1 coordinates (point of line)
741 // x2,y2,z2 P2 coordinates (point of line)
742 // x3,y3,z3, r P3 coordinates and radius (sphere)
743 // x,y,z intersection coordinates
744 //
745 // This function returns a pointer array which first index indicates
746 // the number of intersection point, followed by coordinate pairs.
747
748 float a, b, c, i;
749
750 if(x1 > x3 + r && x2 > x3 + r)
751 return (0);
752 if(x1 < x3 - r && x2 < x3 - r)
753 return (0);
754 if(y1 > y3 + r && y2 > y3 + r)
755 return (0);
756 if(y1 < y3 - r && y2 < y3 - r)
757 return (0);
758 if(z1 > z3 + r && z2 > z3 + r)
759 return (0);
760 if(z1 < z3 - r && z2 < z3 - r)
761 return (0);
762 a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
763 b = 2 * ((x2 - x1) * (x1 - x3)
764 + (y2 - y1) * (y1 - y3)
765 + (z2 - z1) * (z1 - z3));
766 c = square(x3) + square(y3) +
767 square(z3) + square(x1) +
768 square(y1) + square(z1) - 2 * (x3 * x1 + y3 * y1 + z3 * z1) - square(r);
769 i = b * b - 4 * a * c;
770
771 if(i < 0.0) {
772 // no intersection
773 return (0);
774 }
775
776 return (1);
777 }
778
DoRotationRadian(XYZ thePoint,float xang,float yang,float zang)779 XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang)
780 {
781 XYZ newpoint;
782 XYZ oldpoint;
783
784 oldpoint = thePoint;
785
786 if(xang) {
787 newpoint.y = oldpoint.y * cos(xang) - oldpoint.z * sin(xang);
788 newpoint.z = oldpoint.y * sin(xang) + oldpoint.z * cos(xang);
789 oldpoint.z = newpoint.z;
790 oldpoint.y = newpoint.y;
791 }
792
793 if(yang) {
794 newpoint.z = oldpoint.z * cos(yang) - oldpoint.x * sin(yang);
795 newpoint.x = oldpoint.z * sin(yang) + oldpoint.x * cos(yang);
796 oldpoint.z = newpoint.z;
797 oldpoint.x = newpoint.x;
798 }
799
800 if(zang) {
801 newpoint.x = oldpoint.x * cos(zang) - oldpoint.y * sin(zang);
802 newpoint.y = oldpoint.y * cos(zang) + oldpoint.x * sin(zang);
803 oldpoint.x = newpoint.x;
804 oldpoint.y = newpoint.y;
805 }
806
807 return oldpoint;
808
809 }
810