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.
22 #include "Quaternions.h"
25 quaternion Quat_Mult(quaternion q1, quaternion q2)
28 float a, b, c, d, e, f, g, h;
29 a = (q1.w + q1.x) * (q2.w + q2.x);
30 b = (q1.z - q1.y) * (q2.y - q2.z);
31 c = (q1.w - q1.x) * (q2.y + q2.z);
32 d = (q1.y + q1.z) * (q2.w - q2.x);
33 e = (q1.x + q1.z) * (q2.x + q2.y);
34 f = (q1.x - q1.z) * (q2.x - q2.y);
35 g = (q1.w + q1.y) * (q2.w - q2.z);
36 h = (q1.w - q1.y) * (q2.w + q2.z);
37 QResult.w = b + (-e - f + g + h) / 2;
38 QResult.x = a - (e + f + g + h) / 2;
39 QResult.y = c + (e - f + g - h) / 2;
40 QResult.z = d + (e - f - g + h) / 2;
46 quaternion To_Quat(Matrix_t m)
48 // From Jason Shankel, (C) 2000.
49 static quaternion Quat;
51 static double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD;
56 fourD = 2.0 * fast_sqrt(Tr);
58 q[0] = (m[2][1] - m[1][2]) / fourD;
59 q[1] = (m[0][2] - m[2][0]) / fourD;
60 q[2] = (m[1][0] - m[0][1]) / fourD;
62 if (m[0][0] > m[1][1]) {
67 if (m[2][2] > m[i][i]) {
72 fourD = 2.0 * fast_sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
74 q[j] = (m[j][i] + m[i][j]) / fourD;
75 q[k] = (m[k][i] + m[i][k]) / fourD;
76 q[3] = (m[j][k] - m[k][j]) / fourD;
85 void Quat_2_Matrix(quaternion Quat, Matrix_t m)
87 // From the GLVelocity site (http://glvelocity.gamedev.net)
95 m[0][0] = 1.0f - 2.0f * (fYY + fZZ);
96 m[1][0] = 2.0f * (fX * fY + fW * fZ);
97 m[2][0] = 2.0f * (fX * fZ - fW * fY);
99 m[0][1] = 2.0f * (fX * fY - fW * fZ);
100 m[1][1] = 1.0f - 2.0f * (fXX + fZZ);
101 m[2][1] = 2.0f * (fY * fZ + fW * fX);
103 m[0][2] = 2.0f * (fX * fZ + fW * fY);
104 m[1][2] = 2.0f * (fX * fZ - fW * fX);
105 m[2][2] = 1.0f - 2.0f * (fXX + fYY);
112 quaternion To_Quat(angle_axis Ang_Ax)
114 // From the Quaternion Powers article on gamedev.net
115 static quaternion Quat;
117 Quat.x = Ang_Ax.x * sin(Ang_Ax.angle / 2);
118 Quat.y = Ang_Ax.y * sin(Ang_Ax.angle / 2);
119 Quat.z = Ang_Ax.z * sin(Ang_Ax.angle / 2);
120 Quat.w = cos(Ang_Ax.angle / 2);
123 angle_axis Quat_2_AA(quaternion Quat)
125 static angle_axis Ang_Ax;
126 static float scale, tw;
127 tw = (float)acosf(Quat.w) * 2;
128 scale = (float)sin(tw / 2.0);
129 Ang_Ax.x = Quat.x / scale;
130 Ang_Ax.y = Quat.y / scale;
131 Ang_Ax.z = Quat.z / scale;
133 Ang_Ax.angle = 2.0 * acosf(Quat.w) / (float)PI * 180;
137 quaternion To_Quat(int In_Degrees, euler Euler)
139 // From the gamasutra quaternion article
140 static quaternion Quat;
141 static float cr, cp, cy, sr, sp, sy, cpcy, spsy;
142 //If we are in Degree mode, convert to Radians
144 Euler.x = Euler.x * (float)PI / 180;
145 Euler.y = Euler.y * (float)PI / 180;
146 Euler.z = Euler.z * (float)PI / 180;
148 //Calculate trig identities
149 //Formerly roll, pitch, yaw
150 cr = float(cos(Euler.x / 2));
151 cp = float(cos(Euler.y / 2));
152 cy = float(cos(Euler.z / 2));
153 sr = float(sin(Euler.x / 2));
154 sp = float(sin(Euler.y / 2));
155 sy = float(sin(Euler.z / 2));
159 Quat.w = cr * cpcy + sr * spsy;
160 Quat.x = sr * cpcy - cr * spsy;
161 Quat.y = cr * sp * cy + sr * cp * sy;
162 Quat.z = cr * cp * sy - sr * sp * cy;
167 quaternion QNormalize(quaternion Quat)
170 norm = Quat.x * Quat.x +
174 Quat.x = float(Quat.x / norm);
175 Quat.y = float(Quat.y / norm);
176 Quat.z = float(Quat.z / norm);
177 Quat.w = float(Quat.w / norm);
181 XYZ Quat2Vector(quaternion Quat)
192 tempvec.x = 2.0f * (fX * fZ - fW * fY);
193 tempvec.y = 2.0f * (fY * fZ + fW * fX);
194 tempvec.z = 1.0f - 2.0f * (fX * fX + fY * fY);
199 bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33)
201 static float u0, u1, u2;
202 static float v0, v1, v2;
207 static float pointv[3];
211 static float normalv[3];
232 normalv[0] = normal.x;
233 normalv[1] = normal.y;
234 normalv[2] = normal.z;
236 #define ABS(X) (((X)<0.f)?-(X):(X) )
237 #define MAX(A, B) (((A)<(B))?(B):(A))
238 max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
240 if (max == ABS(normalv[0])) {
244 if (max == ABS(normalv[1])) {
248 if (max == ABS(normalv[2])) {
254 u0 = pointv[i] - p1v[i];
255 v0 = pointv[j] - p1v[j];
256 u1 = p2v[i] - p1v[i];
257 v1 = p2v[j] - p1v[j];
258 u2 = p3v[i] - p1v[i];
259 v2 = p3v[j] - p1v[j];
261 if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
263 if (0.0f <= b && b <= 1.0f) {
264 a = (v0 - b * v2) / v1;
265 if ((a >= 0.0f) && (( a + b ) <= 1.0f))
269 b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
270 if (0.0f <= b && b <= 1.0f) {
271 a = (u0 - b * u2) / u1;
272 if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
280 bool LineFacet(Vector p1, Vector p2, Vector pa, Vector pb, Vector pc, Vector *p)
283 static float a1, a2, a3;
284 static float total, denom, mu;
285 static Vector n, pa1, pa2, pa3;
287 //Calculate the parameters for the plane
288 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
289 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
290 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
292 d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
294 //Calculate the position on the line that intersects the plane
295 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
296 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
298 mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
299 p->x = p1.x + mu * (p2.x - p1.x);
300 p->y = p1.y + mu * (p2.y - p1.y);
301 p->z = p1.z + mu * (p2.z - p1.z);
302 if (mu < 0 || mu > 1) // Intersection not along line segment
305 if (!PointInTriangle( p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)) {
312 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
314 static float u0, u1, u2;
315 static float v0, v1, v2;
319 static bool bInter = 0;
320 static float pointv[3];
324 static float normalv[3];
345 normalv[0] = normal.x;
346 normalv[1] = normal.y;
347 normalv[2] = normal.z;
349 #define ABS(X) (((X)<0.f)?-(X):(X) )
350 #define MAX(A, B) (((A)<(B))?(B):(A))
351 max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
353 if (max == ABS(normalv[0])) {
357 if (max == ABS(normalv[1])) {
361 if (max == ABS(normalv[2])) {
367 u0 = pointv[i] - p1v[i];
368 v0 = pointv[j] - p1v[j];
369 u1 = p2v[i] - p1v[i];
370 v1 = p2v[j] - p1v[j];
371 u2 = p3v[i] - p1v[i];
372 v2 = p3v[j] - p1v[j];
374 if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
376 if (0.0f <= b && b <= 1.0f) {
377 a = (v0 - b * v2) / v1;
378 if ((a >= 0.0f) && (( a + b ) <= 1.0f))
382 b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
383 if (0.0f <= b && b <= 1.0f) {
384 a = (u0 - b * u2) / u1;
385 if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
393 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
396 static float a1, a2, a3;
397 static float total, denom, mu;
398 static XYZ n, pa1, pa2, pa3;
400 //Calculate the parameters for the plane
401 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
402 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
403 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
405 d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
407 //Calculate the position on the line that intersects the plane
408 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
409 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
411 mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
412 p->x = p1.x + mu * (p2.x - p1.x);
413 p->y = p1.y + mu * (p2.y - p1.y);
414 p->z = p1.z + mu * (p2.z - p1.z);
415 if (mu < 0 || mu > 1) // Intersection not along line segment
418 if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
425 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
428 static float a1, a2, a3;
429 static float total, denom, mu;
430 static XYZ n, pa1, pa2, pa3;
432 //Calculate the parameters for the plane
433 n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
434 n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
435 n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
437 d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
439 //Calculate the position on the line that intersects the plane
440 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
441 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
443 mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
444 p->x = p1.x + mu * (p2.x - p1.x);
445 p->y = p1.y + mu * (p2.y - p1.y);
446 p->z = p1.z + mu * (p2.z - p1.z);
447 if (mu < 0 || mu > 1) // Intersection not along line segment
450 if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
457 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ *p)
460 static float a1, a2, a3;
461 static float total, denom, mu;
462 static XYZ pa1, pa2, pa3;
464 //Calculate the parameters for the plane
465 d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
467 //Calculate the position on the line that intersects the plane
468 denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
469 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
471 mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
472 p->x = p1.x + mu * (p2.x - p1.x);
473 p->y = p1.y + mu * (p2.y - p1.y);
474 p->z = p1.z + mu * (p2.z - p1.z);
475 if (mu < 0 || mu > 1) // Intersection not along line segment
478 if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
484 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *p)
487 static float a1, a2, a3;
488 static float total, denom, mu;
489 static XYZ pa1, pa2, pa3, n;
491 //Calculate the parameters for the plane
492 n.x = (pb->y - pa->y) * (pc->z - pa->z) - (pb->z - pa->z) * (pc->y - pa->y);
493 n.y = (pb->z - pa->z) * (pc->x - pa->x) - (pb->x - pa->x) * (pc->z - pa->z);
494 n.z = (pb->x - pa->x) * (pc->y - pa->y) - (pb->y - pa->y) * (pc->x - pa->x);
496 d = - n.x * pa->x - n.y * pa->y - n.z * pa->z;
499 //Calculate the position on the line that intersects the plane
500 denom = n.x * (p2->x - p1->x) + n.y * (p2->y - p1->y) + n.z * (p2->z - p1->z);
501 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
503 mu = - (d + n.x * p1->x + n.y * p1->y + n.z * p1->z) / denom;
504 p->x = p1->x + mu * (p2->x - p1->x);
505 p->y = p1->y + mu * (p2->y - p1->y);
506 p->z = p1->z + mu * (p2->z - p1->z);
507 if (mu < 0 || mu > 1) // Intersection not along line segment
510 if (!PointInTriangle( p, n, pa, pb, pc)) {
516 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *n, XYZ *p)
519 static float a1, a2, a3;
520 static float total, denom, mu;
521 static XYZ pa1, pa2, pa3;
523 //Calculate the parameters for the plane
524 d = - n->x * pa->x - n->y * pa->y - n->z * pa->z;
526 //Calculate the position on the line that intersects the plane
527 denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z);
528 if (fabs(denom) < 0.0000001) // Line and plane don't intersect
530 mu = - (d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom;
531 p->x = p1->x + mu * (p2->x - p1->x);
532 p->y = p1->y + mu * (p2->y - p1->y);
533 p->z = p1->z + mu * (p2->z - p1->z);
534 if (mu < 0 || mu > 1) // Intersection not along line segment
537 if (!PointInTriangle( p, *n, pa, pb, pc)) {