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