Source/Quaternions.cpp

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