Source/Quaternions.cpp

   1 /*
   2 Copyright (C) 2003, 2010 - Wolfire Games
   3
   4 This file is part of Lugaru.
   5
   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.
  10
  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.
  14
  15 See the GNU General Public License for more details.
  16
  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.
  20 */
  21
  22 #include "Quaternions.h"
  23
  24 // Functions
  25 quaternion Quat_Mult(quaternion q1, quaternion q2)
  26 {
  27     quaternion QResult;
  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;
  41     return QResult;
  42 }
  43
  44
  45
  46 quaternion To_Quat(Matrix_t m)
  47 {
  48     // From Jason Shankel, (C) 2000.
  49     static quaternion Quat;
  50
  51     static double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD;
  52     static double q[4];
  53
  54     static int i, j, k;
  55     if (Tr >= 1.0) {
  56         fourD = 2.0 * fast_sqrt(Tr);
  57         q[3] = fourD / 4.0;
  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;
  61     } else {
  62         if (m[0][0] > m[1][1]) {
  63             i = 0;
  64         } else {
  65             i = 1;
  66         }
  67         if (m[2][2] > m[i][i]) {
  68             i = 2;
  69         }
  70         j = (i + 1) % 3;
  71         k = (j + 1) % 3;
  72         fourD = 2.0 * fast_sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
  73         q[i] = fourD / 4.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;
  77     }
  78
  79     Quat.x = q[0];
  80     Quat.y = q[1];
  81     Quat.z = q[2];
  82     Quat.w = q[3];
  83     return Quat;
  84 }
  85 void Quat_2_Matrix(quaternion Quat, Matrix_t m)
  86 {
  87     // From the GLVelocity site (http://glvelocity.gamedev.net)
  88     float fW = Quat.w;
  89     float fX = Quat.x;
  90     float fY = Quat.y;
  91     float fZ = Quat.z;
  92     float fXX = fX * fX;
  93     float fYY = fY * fY;
  94     float fZZ = fZ * fZ;
  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);
  98     m[3][0] = 0.0f;
  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);
 102     m[3][1] = 0.0f;
 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);
 106     m[3][2] = 0.0f;
 107     m[0][3] = 0.0f;
 108     m[1][3] = 0.0f;
 109     m[2][3] = 0.0f;
 110     m[3][3] = 1.0f;
 111 }
 112 quaternion To_Quat(angle_axis Ang_Ax)
 113 {
 114     // From the Quaternion Powers article on gamedev.net
 115     static quaternion Quat;
 116
 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);
 121     return Quat;
 122 }
 123 angle_axis Quat_2_AA(quaternion Quat)
 124 {
 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;
 132
 133     Ang_Ax.angle = 2.0 * acosf(Quat.w) / (float)PI * 180;
 134     return Ang_Ax;
 135 }
 136
 137 quaternion To_Quat(int In_Degrees, euler Euler)
 138 {
 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
 143     if (In_Degrees) {
 144         Euler.x = Euler.x * (float)PI / 180;
 145         Euler.y = Euler.y * (float)PI / 180;
 146         Euler.z = Euler.z * (float)PI / 180;
 147     }
 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));
 156
 157     cpcy = cp * cy;
 158     spsy = sp * sy;
 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;
 163
 164     return Quat;
 165 }
 166
 167 quaternion QNormalize(quaternion Quat)
 168 {
 169     static float norm;
 170     norm =  Quat.x * Quat.x +
 171             Quat.y * Quat.y +
 172             Quat.z * Quat.z +
 173             Quat.w * Quat.w;
 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);
 178     return Quat;
 179 }
 180
 181 XYZ Quat2Vector(quaternion Quat)
 182 {
 183     QNormalize(Quat);
 184
 185     float fW = Quat.w;
 186     float fX = Quat.x;
 187     float fY = Quat.y;
 188     float fZ = Quat.z;
 189
 190     XYZ tempvec;
 191
 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);
 195
 196     return tempvec;
 197 }
 198
 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)
 200 {
 201     static float u0, u1, u2;
 202     static float v0, v1, v2;
 203     static float a, b;
 204     static float max;
 205     static int i, j;
 206     static bool bInter;
 207     static float pointv[3];
 208     static float p1v[3];
 209     static float p2v[3];
 210     static float p3v[3];
 211     static float normalv[3];
 212
 213     bInter = 0;
 214
 215     pointv[0] = p->x;
 216     pointv[1] = p->y;
 217     pointv[2] = p->z;
 218
 219
 220     p1v[0] = p11;
 221     p1v[1] = p12;
 222     p1v[2] = p13;
 223
 224     p2v[0] = p21;
 225     p2v[1] = p22;
 226     p2v[2] = p23;
 227
 228     p3v[0] = p31;
 229     p3v[1] = p32;
 230     p3v[2] = p33;
 231
 232     normalv[0] = normal.x;
 233     normalv[1] = normal.y;
 234     normalv[2] = normal.z;
 235
 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]));
 239 #undef MAX
 240     if (max == ABS(normalv[0])) {
 241         i = 1;    // y, z
 242         j = 2;
 243     }
 244     if (max == ABS(normalv[1])) {
 245         i = 0;    // x, z
 246         j = 2;
 247     }
 248     if (max == ABS(normalv[2])) {
 249         i = 0;    // x, y
 250         j = 1;
 251     }
 252 #undef ABS
 253
 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];
 260
 261     if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
 262         b = u0 / u2;
 263         if (0.0f <= b && b <= 1.0f) {
 264             a = (v0 - b * v2) / v1;
 265             if ((a >= 0.0f) && (( a + b ) <= 1.0f))
 266                 bInter = 1;
 267         }
 268     } else {
 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 ))
 273                 bInter = 1;
 274         }
 275     }
 276
 277     return bInter;
 278 }
 279
 280 bool LineFacet(Vector p1, Vector p2, Vector pa, Vector pb, Vector pc, Vector *p)
 281 {
 282     static float d;
 283     static float a1, a2, a3;
 284     static float total, denom, mu;
 285     static Vector n, pa1, pa2, pa3;
 286
 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);
 291     n.Normalize();
 292     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 293
 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
 297         return 0;
 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
 303         return 0;
 304
 305     if (!PointInTriangle( p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)) {
 306         return 0;
 307     }
 308
 309     return 1;
 310 }
 311
 312 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
 313 {
 314     static float u0, u1, u2;
 315     static float v0, v1, v2;
 316     static float a, b;
 317     static float max;
 318     static int i, j;
 319     static bool bInter = 0;
 320     static float pointv[3];
 321     static float p1v[3];
 322     static float p2v[3];
 323     static float p3v[3];
 324     static float normalv[3];
 325
 326     bInter = 0;
 327
 328     pointv[0] = p->x;
 329     pointv[1] = p->y;
 330     pointv[2] = p->z;
 331
 332
 333     p1v[0] = p1->x;
 334     p1v[1] = p1->y;
 335     p1v[2] = p1->z;
 336
 337     p2v[0] = p2->x;
 338     p2v[1] = p2->y;
 339     p2v[2] = p2->z;
 340
 341     p3v[0] = p3->x;
 342     p3v[1] = p3->y;
 343     p3v[2] = p3->z;
 344
 345     normalv[0] = normal.x;
 346     normalv[1] = normal.y;
 347     normalv[2] = normal.z;
 348
 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]));
 352 #undef MAX
 353     if (max == ABS(normalv[0])) {
 354         i = 1;    // y, z
 355         j = 2;
 356     }
 357     if (max == ABS(normalv[1])) {
 358         i = 0;    // x, z
 359         j = 2;
 360     }
 361     if (max == ABS(normalv[2])) {
 362         i = 0;    // x, y
 363         j = 1;
 364     }
 365 #undef ABS
 366
 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];
 373
 374     if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
 375         b = u0 / u2;
 376         if (0.0f <= b && b <= 1.0f) {
 377             a = (v0 - b * v2) / v1;
 378             if ((a >= 0.0f) && (( a + b ) <= 1.0f))
 379                 bInter = 1;
 380         }
 381     } else {
 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 ))
 386                 bInter = 1;
 387         }
 388     }
 389
 390     return bInter;
 391 }
 392
 393 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
 394 {
 395     static float d;
 396     static float a1, a2, a3;
 397     static float total, denom, mu;
 398     static XYZ n, pa1, pa2, pa3;
 399
 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);
 404     Normalise(&n);
 405     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 406
 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
 410         return 0;
 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
 416         return 0;
 417
 418     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 419         return 0;
 420     }
 421
 422     return 1;
 423 }
 424
 425 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
 426 {
 427     static float d;
 428     static float a1, a2, a3;
 429     static float total, denom, mu;
 430     static XYZ n, pa1, pa2, pa3;
 431
 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);
 436     Normalise(&n);
 437     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 438
 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
 442         return 0;
 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
 448         return 0;
 449
 450     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 451         return 0;
 452     }
 453
 454     return 1;
 455 }
 456
 457 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ *p)
 458 {
 459     static float d;
 460     static float a1, a2, a3;
 461     static float total, denom, mu;
 462     static XYZ pa1, pa2, pa3;
 463
 464     //Calculate the parameters for the plane
 465     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 466
 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
 470         return 0;
 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
 476         return 0;
 477
 478     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 479         return 0;
 480     }
 481     return 1;
 482 }
 483
 484 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *p)
 485 {
 486     static float d;
 487     static float a1, a2, a3;
 488     static float total, denom, mu;
 489     static XYZ pa1, pa2, pa3, n;
 490
 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);
 495     Normalise(&n);
 496     d = - n.x * pa->x - n.y * pa->y - n.z * pa->z;
 497
 498
 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
 502         return 0;
 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
 508         return 0;
 509
 510     if (!PointInTriangle( p, n, pa, pb, pc)) {
 511         return 0;
 512     }
 513     return 1;
 514 }
 515
 516 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *n, XYZ *p)
 517 {
 518     static float d;
 519     static float a1, a2, a3;
 520     static float total, denom, mu;
 521     static XYZ pa1, pa2, pa3;
 522
 523     //Calculate the parameters for the plane
 524     d = - n->x * pa->x - n->y * pa->y - n->z * pa->z;
 525
 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
 529         return 0;
 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
 535         return 0;
 536
 537     if (!PointInTriangle( p, *n, pa, pb, pc)) {
 538         return 0;
 539     }
 540     return 1;
 541 }
 542
 543