home *** CD-ROM | disk | FTP | other *** search
/ Enter 2005 March / ENTER.ISO / files / fwp-0.0.6-win32-installer.exe / quaternionmath.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  2004-12-06  |  6.2 KB  |  197 lines
  1. #include "quaternionmath.h"
  2.  
  3. #include <math.h>
  4. #include <stdio.h>
  5.  
  6. #include "vectormath.h"
  7.  
  8.  
  9. float* quaternionToMatrix(quaternion_t q, float* matrix){
  10. //    if(!matrix)
  11. //        return;
  12.  
  13.     matrix[ 0] = 1.0f - 2.0f * ( q[1] * q[1] + q[2] * q[2] );
  14.     matrix[ 1] = 2.0f * ( q[0] * q[1] - q[3] * q[2] );
  15.     matrix[ 2] = 2.0f * ( q[0] * q[2] + q[3] * q[1] );
  16.     matrix[ 3] = 0.0f;
  17.  
  18.     matrix[ 4] = 2.0f * ( q[0] * q[1] + q[3] * q[2] );
  19.     matrix[ 5] = 1.0f - 2.0f * ( q[0] * q[0] + q[2] * q[2] );
  20.     matrix[ 6] = 2.0f * ( q[1] * q[2] - q[3] * q[0] );
  21.     matrix[ 7] = 0.0f;
  22.  
  23.     matrix[ 8] = 2.0f * ( q[0] * q[2] - q[3] * q[1] );
  24.     matrix[ 9] = 2.0f * ( q[1] * q[2] + q[3] * q[0] );
  25.     matrix[10] = 1.0f - 2.0f * ( q[0] * q[0] + q[1] * q[1] );
  26.     matrix[11] = 0.0f;
  27.  
  28.     matrix[12] = 0;
  29.     matrix[13] = 0;
  30.     matrix[14] = 0;
  31.     matrix[15] = 1.0f;
  32.  
  33.     return matrix;
  34. }
  35.  
  36.  
  37.  
  38. float* quaternionFromMatrix(float* matrix, quaternion_t q){
  39.     /*
  40.     float diagSum=matrix[0]+matrix[5]+matrix[10]+1.0f;
  41.     if(diagSum<0.0){
  42. //        printf("BOING!!\n");
  43.         return q;    // THINKABOUTME: oder auf null setzten, um Fehler anzuzeigen?
  44.     }
  45.  
  46.     q[3]=(float)(2.0 * sqrt(diagSum));
  47.     float s1 = (matrix[0]+1 - 2*q[3]*q[3])/2;
  48.     s1 = s1 < 0.0 ? -s1 : s1;
  49.     float s2 = (matrix[5]+1 - 2*q[3]*q[3])/2;
  50.     s2 = s2 < 0.0 ? -s2 : s2;
  51.     float s3 = (matrix[10]+1 - 2*q[3]*q[3])/2;
  52.     s3 = s3 < 0.0 ? -s3 : s3;
  53.     q[0]=(float)sqrt(s1);
  54.     q[1]=(float)sqrt(s2);
  55.     q[2]=(float)sqrt(s3);
  56.  
  57. //    printf("sqrt(%f)\n", (matrix[0]+1 - 2*q[3]*q[3])/2);
  58. //    printf("sqrt(%f)\n", (matrix[5]+1 - 2*q[3]*q[3])/2);
  59. //    printf("sqrt(%f)\n", (matrix[10]+1 - 2*q[3]*q[3])/2);
  60.  
  61.     return q;
  62. //    printf("x: %f; y: %f; z: %f; w: %f; length: %f\n", x,y,z,w,x*x+y*y+z*z+w*w);
  63. */
  64.  
  65.     float x,y,z,w;
  66.  
  67.     float diagonal = matrix[0] + matrix[5] + matrix[10] + 1;
  68.     float scale = 0.0f;
  69.  
  70.     // If the diagonal is greater than zero
  71.     if(diagonal > 0.00000001)
  72.     {
  73.         // Calculate the scale of the diagonal
  74.         scale = float(sqrt(diagonal ) * 2);
  75.  
  76.         // Calculate the x, y, x and w of the quaternion through the respective equation
  77.         x = ( matrix[9] - matrix[6] ) / scale;
  78.         y = ( matrix[2] - matrix[8] ) / scale;
  79.         z = ( matrix[4] - matrix[1] ) / scale;
  80.         w = 0.25f * scale;
  81.     }
  82.     else
  83.     {
  84.         // If the first element of the diagonal is the greatest value
  85.         if ( matrix[0] > matrix[5] && matrix[0] > matrix[10] )
  86.         {
  87.             // Find the scale according to the first element, and double that value
  88.             scale  = (float)sqrt( 1.0f + matrix[0] - matrix[5] - matrix[10] ) * 2.0f;
  89.  
  90.             // Calculate the x, y, x and w of the quaternion through the respective equation
  91.             x = 0.25f * scale;
  92.             y = (matrix[4] + matrix[1] ) / scale;
  93.             z = (matrix[2] + matrix[8] ) / scale;
  94.             w = (matrix[9] - matrix[6] ) / scale;
  95.         }
  96.         // Else if the second element of the diagonal is the greatest value
  97.         else if ( matrix[5] > matrix[10] )
  98.         {
  99.             // Find the scale according to the second element, and double that value
  100.             scale  = (float)sqrt( 1.0f + matrix[5] - matrix[0] - matrix[10] ) * 2.0f;
  101.  
  102.             // Calculate the x, y, x and w of the quaternion through the respective equation
  103.             x = (matrix[4] + matrix[1] ) / scale;
  104.             y = 0.25f * scale;
  105.             z = (matrix[9] + matrix[6] ) / scale;
  106.             w = (matrix[2] - matrix[8] ) / scale;
  107.         }
  108.         // Else the third element of the diagonal is the greatest value
  109.         else
  110.         {
  111.             // Find the scale according to the third element, and double that value
  112.             scale  = (float)sqrt( 1.0f + matrix[10] - matrix[0] - matrix[5] ) * 2.0f;
  113.  
  114.             // Calculate the x, y, x and w of the quaternion through the respective equation
  115.             x = (matrix[2] + matrix[8] ) / scale;
  116.             y = (matrix[9] + matrix[6] ) / scale;
  117.             z = 0.25f * scale;
  118.             w = (matrix[4] - matrix[1] ) / scale;
  119.         }
  120.     }
  121.  
  122.     q[0] = x;
  123.     q[1] = y;
  124.     q[2] = z;
  125.     q[3] = w;
  126.  
  127.     return q;
  128. }
  129.  
  130.  
  131.  
  132. float* quaternionSlerp(quaternion_t q1, quaternion_t q2, float t, quaternion_t r){
  133.     quaternion_t tmp;
  134.  
  135.  
  136.     // Here we do a check to make sure the 2 quaternions aren't the same, return q1 if they are
  137.     if(        fabs(q1[0]-q2[0]) < 0.00001 
  138.         &&    fabs(q1[1]-q2[1]) < 0.00001
  139.         &&    fabs(q1[2]-q2[2]) < 0.00001
  140.         &&    fabs(q1[3]-q2[3]) < 0.00001
  141.         ){
  142.         vectorCopy4d(q1, r);
  143.         return q1;
  144.     }
  145.  
  146.     // Following the (b.a) part of the equation, we do a dot product between q1 and q2.
  147.     // We can do a dot product because the same math applied for a 3D vector as a 4D vector.
  148.     float result = vectorDotP4d(q1, q2);
  149.  
  150.     // If the dot product is less than 0, the angle is greater than 90 degrees
  151.     if(result < 0.0f){
  152.         // Negate the second quaternion and the result of the dot product
  153.         vectorScale4d(-1.0f, q2, tmp);
  154.         result = -result;
  155.     }else{
  156.         vectorCopy4d(q2, tmp);
  157.     }
  158.  
  159.     // Set the first and second scale for the interpolation (linear!)
  160.     float scale1 = 1 - t;
  161.     float scale2 = t;
  162.  
  163.     // Check if the angle between the 2 quaternions was big enough to warrant such calculations
  164.     if(1 - result > 0.1f){
  165.         // Get the angle between the 2 quaternions, and then store the sin() of that angle
  166.         float theta = (float)acos(result);
  167.         float sinTheta = (float)sin(theta);
  168.  
  169.         // Calculate the scale for q1 and q2, according to the angle and it's sine value
  170.         scale1 = (float)sin( ( 1 - t ) * theta) / sinTheta;
  171.         scale2 = (float)sin( ( t * theta) ) / sinTheta;
  172.  
  173. //        printf("theta: %f, sin_theta: %f, scale1: %f, scale2: %f\n", theta, sinTheta, scale1, scale2);
  174.     }
  175.  
  176.     vectorLinCombi4d(scale1, q1, scale2, tmp, r);
  177. //    printf("q1: [%f %f %f %f]\n", q1[0], q1[1], q1[2], q1[3]);
  178. //    printf("q2: [%f %f %f %f]\n", q2[0], q2[1], q2[2], q2[3]);
  179. //    printf("r : [%f %f %f %f]\n", r[0], r[1], r[2], r[3]);
  180.     // Return the interpolated quaternion
  181.     return r;
  182. }
  183.  
  184.  
  185. void slerpMatrix(float* m1, float* m2, float t, float* r){
  186.     quaternion_t q1,q2,qi;
  187.     
  188.     quaternionFromMatrix(m1, q1);
  189.     quaternionFromMatrix(m2, q2);
  190.     quaternionSlerp(q1, q2, t, qi);
  191.  
  192.     quaternionToMatrix(qi, r);
  193.  
  194.     vectorLinCombi3d((1-t), &m1[12], t, &m2[12], &r[12]);    // FIXME: animationen sehen scheisse aus, aber Kollision klappt!!
  195. //    vectorCopy3d(&m1[12], &r[12]);
  196. }
  197.