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 modify
   7 it under the terms of the GNU General Public License as published by
   8 the Free Software Foundation; either version 2 of the License, or
   9 (at your option) any later version.
  10
  11 Lugaru 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.  See the
  14 GNU General Public License for more details.
  15
  16 You should have received a copy of the GNU General Public License
  17 along with Lugaru.  If not, see <http://www.gnu.org/licenses/>.
  18 */
  19
  20 #include "Quaternions.h"
  21
  22 // Functions
  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
  42
  43
  44 quaternion To_Quat(Matrix_t m)
  45 {
  46     // From Jason Shankel, (C) 2000.
  47     static quaternion Quat;
  48
  49     static double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD;
  50     static double q[4];
  51
  52     static int i, j, k;
  53     if (Tr >= 1.0) {
  54         fourD = 2.0 * fast_sqrt(Tr);
  55         q[3] = fourD / 4.0;
  56         q[0] = (m[2][1] - m[1][2]) / fourD;
  57         q[1] = (m[0][2] - m[2][0]) / fourD;
  58         q[2] = (m[1][0] - m[0][1]) / fourD;
  59     } else {
  60         if (m[0][0] > m[1][1]) {
  61             i = 0;
  62         } else {
  63             i = 1;
  64         }
  65         if (m[2][2] > m[i][i]) {
  66             i = 2;
  67         }
  68         j = (i + 1) % 3;
  69         k = (j + 1) % 3;
  70         fourD = 2.0 * fast_sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
  71         q[i] = fourD / 4.0;
  72         q[j] = (m[j][i] + m[i][j]) / fourD;
  73         q[k] = (m[k][i] + m[i][k]) / fourD;
  74         q[3] = (m[j][k] - m[k][j]) / fourD;
  75     }
  76
  77     Quat.x = q[0];
  78     Quat.y = q[1];
  79     Quat.z = q[2];
  80     Quat.w = q[3];
  81     return Quat;
  82 }
  83 void Quat_2_Matrix(quaternion Quat, Matrix_t m)
  84 {
  85     // From the GLVelocity site (http://glvelocity.gamedev.net)
  86     float fW = Quat.w;
  87     float fX = Quat.x;
  88     float fY = Quat.y;
  89     float fZ = Quat.z;
  90     float fXX = fX * fX;
  91     float fYY = fY * fY;
  92     float fZZ = fZ * fZ;
  93     m[0][0] = 1.0f - 2.0f * (fYY + fZZ);
  94     m[1][0] = 2.0f * (fX * fY + fW * fZ);
  95     m[2][0] = 2.0f * (fX * fZ - fW * fY);
  96     m[3][0] = 0.0f;
  97     m[0][1] = 2.0f * (fX * fY - fW * fZ);
  98     m[1][1] = 1.0f - 2.0f * (fXX + fZZ);
  99     m[2][1] = 2.0f * (fY * fZ + fW * fX);
 100     m[3][1] = 0.0f;
 101     m[0][2] = 2.0f * (fX * fZ + fW * fY);
 102     m[1][2] = 2.0f * (fX * fZ - fW * fX);
 103     m[2][2] = 1.0f - 2.0f * (fXX + fYY);
 104     m[3][2] = 0.0f;
 105     m[0][3] = 0.0f;
 106     m[1][3] = 0.0f;
 107     m[2][3] = 0.0f;
 108     m[3][3] = 1.0f;
 109 }
 110 quaternion To_Quat(angle_axis Ang_Ax)
 111 {
 112     // From the Quaternion Powers article on gamedev.net
 113     static quaternion Quat;
 114
 115     Quat.x = Ang_Ax.x * sin(Ang_Ax.angle / 2);
 116     Quat.y = Ang_Ax.y * sin(Ang_Ax.angle / 2);
 117     Quat.z = Ang_Ax.z * sin(Ang_Ax.angle / 2);
 118     Quat.w = cos(Ang_Ax.angle / 2);
 119     return Quat;
 120 }
 121 angle_axis Quat_2_AA(quaternion Quat)
 122 {
 123     static angle_axis Ang_Ax;
 124     static float scale, tw;
 125     tw = (float)acosf(Quat.w) * 2;
 126     scale = (float)sin(tw / 2.0);
 127     Ang_Ax.x = Quat.x / scale;
 128     Ang_Ax.y = Quat.y / scale;
 129     Ang_Ax.z = Quat.z / scale;
 130
 131     Ang_Ax.angle = 2.0 * acosf(Quat.w) / (float)PI * 180;
 132     return Ang_Ax;
 133 }
 134
 135 quaternion To_Quat(int In_Degrees, euler Euler)
 136 {
 137     // From the gamasutra quaternion article
 138     static quaternion Quat;
 139     static float cr, cp, cy, sr, sp, sy, cpcy, spsy;
 140     //If we are in Degree mode, convert to Radians
 141     if (In_Degrees) {
 142         Euler.x = Euler.x * (float)PI / 180;
 143         Euler.y = Euler.y * (float)PI / 180;
 144         Euler.z = Euler.z * (float)PI / 180;
 145     }
 146     //Calculate trig identities
 147     //Formerly roll, pitch, yaw
 148     cr = float(cos(Euler.x / 2));
 149     cp = float(cos(Euler.y / 2));
 150     cy = float(cos(Euler.z / 2));
 151     sr = float(sin(Euler.x / 2));
 152     sp = float(sin(Euler.y / 2));
 153     sy = float(sin(Euler.z / 2));
 154
 155     cpcy = cp * cy;
 156     spsy = sp * sy;
 157     Quat.w = cr * cpcy + sr * spsy;
 158     Quat.x = sr * cpcy - cr * spsy;
 159     Quat.y = cr * sp * cy + sr * cp * sy;
 160     Quat.z = cr * cp * sy - sr * sp * cy;
 161
 162     return Quat;
 163 }
 164
 165 quaternion QNormalize(quaternion Quat)
 166 {
 167     static float norm;
 168     norm =  Quat.x * Quat.x +
 169             Quat.y * Quat.y +
 170             Quat.z * Quat.z +
 171             Quat.w * Quat.w;
 172     Quat.x = float(Quat.x / norm);
 173     Quat.y = float(Quat.y / norm);
 174     Quat.z = float(Quat.z / norm);
 175     Quat.w = float(Quat.w / norm);
 176     return Quat;
 177 }
 178
 179 XYZ Quat2Vector(quaternion Quat)
 180 {
 181     QNormalize(Quat);
 182
 183     float fW = Quat.w;
 184     float fX = Quat.x;
 185     float fY = Quat.y;
 186     float fZ = Quat.z;
 187
 188     XYZ tempvec;
 189
 190     tempvec.x = 2.0f * (fX * fZ - fW * fY);
 191     tempvec.y = 2.0f * (fY * fZ + fW * fX);
 192     tempvec.z = 1.0f - 2.0f * (fX * fX + fY * fY);
 193
 194     return tempvec;
 195 }
 196
 197 bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33)
 198 {
 199     static float u0, u1, u2;
 200     static float v0, v1, v2;
 201     static float a, b;
 202     static float max;
 203     static int i, j;
 204     static bool bInter;
 205     static float pointv[3];
 206     static float p1v[3];
 207     static float p2v[3];
 208     static float p3v[3];
 209     static float normalv[3];
 210
 211     bInter = 0;
 212
 213     pointv[0] = p->x;
 214     pointv[1] = p->y;
 215     pointv[2] = p->z;
 216
 217
 218     p1v[0] = p11;
 219     p1v[1] = p12;
 220     p1v[2] = p13;
 221
 222     p2v[0] = p21;
 223     p2v[1] = p22;
 224     p2v[2] = p23;
 225
 226     p3v[0] = p31;
 227     p3v[1] = p32;
 228     p3v[2] = p33;
 229
 230     normalv[0] = normal.x;
 231     normalv[1] = normal.y;
 232     normalv[2] = normal.z;
 233
 234 #define ABS(X) (((X)<0.f)?-(X):(X) )
 235 #define MAX(A, B) (((A)<(B))?(B):(A))
 236     max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
 237 #undef MAX
 238     if (max == ABS(normalv[0])) {
 239         i = 1;    // y, z
 240         j = 2;
 241     }
 242     if (max == ABS(normalv[1])) {
 243         i = 0;    // x, z
 244         j = 2;
 245     }
 246     if (max == ABS(normalv[2])) {
 247         i = 0;    // x, y
 248         j = 1;
 249     }
 250 #undef ABS
 251
 252     u0 = pointv[i] - p1v[i];
 253     v0 = pointv[j] - p1v[j];
 254     u1 = p2v[i] - p1v[i];
 255     v1 = p2v[j] - p1v[j];
 256     u2 = p3v[i] - p1v[i];
 257     v2 = p3v[j] - p1v[j];
 258
 259     if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
 260         b = u0 / u2;
 261         if (0.0f <= b && b <= 1.0f) {
 262             a = (v0 - b * v2) / v1;
 263             if ((a >= 0.0f) && (( a + b ) <= 1.0f))
 264                 bInter = 1;
 265         }
 266     } else {
 267         b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
 268         if (0.0f <= b && b <= 1.0f) {
 269             a = (u0 - b * u2) / u1;
 270             if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
 271                 bInter = 1;
 272         }
 273     }
 274
 275     return bInter;
 276 }
 277
 278 bool LineFacet(Vector p1, Vector p2, Vector pa, Vector pb, Vector pc, Vector *p)
 279 {
 280     static float d;
 281     static float denom, mu;
 282     static Vector n, pa1, pa2, pa3;
 283
 284     //Calculate the parameters for the plane
 285     n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
 286     n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
 287     n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
 288     n.Normalize();
 289     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 290
 291     //Calculate the position on the line that intersects the plane
 292     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 293     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 294         return 0;
 295     mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 296     p->x = p1.x + mu * (p2.x - p1.x);
 297     p->y = p1.y + mu * (p2.y - p1.y);
 298     p->z = p1.z + mu * (p2.z - p1.z);
 299     if (mu < 0 || mu > 1)   // Intersection not along line segment
 300         return 0;
 301
 302     if (!PointInTriangle( p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)) {
 303         return 0;
 304     }
 305
 306     return 1;
 307 }
 308
 309 bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
 310 {
 311     static float u0, u1, u2;
 312     static float v0, v1, v2;
 313     static float a, b;
 314     static float max;
 315     static int i, j;
 316     static bool bInter = 0;
 317     static float pointv[3];
 318     static float p1v[3];
 319     static float p2v[3];
 320     static float p3v[3];
 321     static float normalv[3];
 322
 323     bInter = 0;
 324
 325     pointv[0] = p->x;
 326     pointv[1] = p->y;
 327     pointv[2] = p->z;
 328
 329
 330     p1v[0] = p1->x;
 331     p1v[1] = p1->y;
 332     p1v[2] = p1->z;
 333
 334     p2v[0] = p2->x;
 335     p2v[1] = p2->y;
 336     p2v[2] = p2->z;
 337
 338     p3v[0] = p3->x;
 339     p3v[1] = p3->y;
 340     p3v[2] = p3->z;
 341
 342     normalv[0] = normal.x;
 343     normalv[1] = normal.y;
 344     normalv[2] = normal.z;
 345
 346 #define ABS(X) (((X)<0.f)?-(X):(X) )
 347 #define MAX(A, B) (((A)<(B))?(B):(A))
 348     max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
 349 #undef MAX
 350     if (max == ABS(normalv[0])) {
 351         i = 1;    // y, z
 352         j = 2;
 353     }
 354     if (max == ABS(normalv[1])) {
 355         i = 0;    // x, z
 356         j = 2;
 357     }
 358     if (max == ABS(normalv[2])) {
 359         i = 0;    // x, y
 360         j = 1;
 361     }
 362 #undef ABS
 363
 364     u0 = pointv[i] - p1v[i];
 365     v0 = pointv[j] - p1v[j];
 366     u1 = p2v[i] - p1v[i];
 367     v1 = p2v[j] - p1v[j];
 368     u2 = p3v[i] - p1v[i];
 369     v2 = p3v[j] - p1v[j];
 370
 371     if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
 372         b = u0 / u2;
 373         if (0.0f <= b && b <= 1.0f) {
 374             a = (v0 - b * v2) / v1;
 375             if ((a >= 0.0f) && (( a + b ) <= 1.0f))
 376                 bInter = 1;
 377         }
 378     } else {
 379         b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
 380         if (0.0f <= b && b <= 1.0f) {
 381             a = (u0 - b * u2) / u1;
 382             if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
 383                 bInter = 1;
 384         }
 385     }
 386
 387     return bInter;
 388 }
 389
 390 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
 391 {
 392     static float d;
 393     static float denom, mu;
 394     static XYZ n, pa1, pa2, pa3;
 395
 396     //Calculate the parameters for the plane
 397     n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
 398     n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
 399     n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
 400     Normalise(&n);
 401     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 402
 403     //Calculate the position on the line that intersects the plane
 404     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 405     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 406         return 0;
 407     mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 408     p->x = p1.x + mu * (p2.x - p1.x);
 409     p->y = p1.y + mu * (p2.y - p1.y);
 410     p->z = p1.z + mu * (p2.z - p1.z);
 411     if (mu < 0 || mu > 1)   // Intersection not along line segment
 412         return 0;
 413
 414     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 415         return 0;
 416     }
 417
 418     return 1;
 419 }
 420
 421 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ *p)
 422 {
 423     static float d;
 424     static float denom, mu;
 425     static XYZ n, pa1, pa2, pa3;
 426
 427     //Calculate the parameters for the plane
 428     n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
 429     n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
 430     n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
 431     Normalise(&n);
 432     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 433
 434     //Calculate the position on the line that intersects the plane
 435     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 436     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 437         return 0;
 438     mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 439     p->x = p1.x + mu * (p2.x - p1.x);
 440     p->y = p1.y + mu * (p2.y - p1.y);
 441     p->z = p1.z + mu * (p2.z - p1.z);
 442     if (mu < 0 || mu > 1)   // Intersection not along line segment
 443         return 0;
 444
 445     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 446         return 0;
 447     }
 448
 449     return 1;
 450 }
 451
 452 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ *p)
 453 {
 454     static float d;
 455     static float denom, mu;
 456     static XYZ pa1, pa2, pa3;
 457
 458     //Calculate the parameters for the plane
 459     d = - n.x * pa.x - n.y * pa.y - n.z * pa.z;
 460
 461     //Calculate the position on the line that intersects the plane
 462     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 463     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 464         return 0;
 465     mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 466     p->x = p1.x + mu * (p2.x - p1.x);
 467     p->y = p1.y + mu * (p2.y - p1.y);
 468     p->z = p1.z + mu * (p2.z - p1.z);
 469     if (mu < 0 || mu > 1)   // Intersection not along line segment
 470         return 0;
 471
 472     if (!PointInTriangle( p, n, &pa, &pb, &pc)) {
 473         return 0;
 474     }
 475     return 1;
 476 }
 477
 478 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *p)
 479 {
 480     static float d;
 481     static float denom, mu;
 482     static XYZ pa1, pa2, pa3, n;
 483
 484     //Calculate the parameters for the plane
 485     n.x = (pb->y - pa->y) * (pc->z - pa->z) - (pb->z - pa->z) * (pc->y - pa->y);
 486     n.y = (pb->z - pa->z) * (pc->x - pa->x) - (pb->x - pa->x) * (pc->z - pa->z);
 487     n.z = (pb->x - pa->x) * (pc->y - pa->y) - (pb->y - pa->y) * (pc->x - pa->x);
 488     Normalise(&n);
 489     d = - n.x * pa->x - n.y * pa->y - n.z * pa->z;
 490
 491
 492     //Calculate the position on the line that intersects the plane
 493     denom = n.x * (p2->x - p1->x) + n.y * (p2->y - p1->y) + n.z * (p2->z - p1->z);
 494     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 495         return 0;
 496     mu = - (d + n.x * p1->x + n.y * p1->y + n.z * p1->z) / denom;
 497     p->x = p1->x + mu * (p2->x - p1->x);
 498     p->y = p1->y + mu * (p2->y - p1->y);
 499     p->z = p1->z + mu * (p2->z - p1->z);
 500     if (mu < 0 || mu > 1)   // Intersection not along line segment
 501         return 0;
 502
 503     if (!PointInTriangle( p, n, pa, pb, pc)) {
 504         return 0;
 505     }
 506     return 1;
 507 }
 508
 509 float LineFacetd(XYZ *p1, XYZ *p2, XYZ *pa, XYZ *pb, XYZ *pc, XYZ *n, XYZ *p)
 510 {
 511     static float d;
 512     static float denom, mu;
 513     static XYZ pa1, pa2, pa3;
 514
 515     //Calculate the parameters for the plane
 516     d = - n->x * pa->x - n->y * pa->y - n->z * pa->z;
 517
 518     //Calculate the position on the line that intersects the plane
 519     denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z);
 520     if (fabs(denom) < 0.0000001)        // Line and plane don't intersect
 521         return 0;
 522     mu = - (d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom;
 523     p->x = p1->x + mu * (p2->x - p1->x);
 524     p->y = p1->y + mu * (p2->y - p1->y);
 525     p->z = p1->z + mu * (p2->z - p1->z);
 526     if (mu < 0 || mu > 1)   // Intersection not along line segment
 527         return 0;
 528
 529     if (!PointInTriangle( p, *n, pa, pb, pc)) {
 530         return 0;
 531     }
 532     return 1;
 533 }
 534
 535