Re: [AD] matrix_to_quat and get_align_matrix

[Elias Pschernig <allefant@xxxxxxxxxx>]

Um, and I forgot the patch.

Index: src/quat.c
===================================================================
RCS file: /cvsroot/alleg/allegro/src/quat.c,v
retrieving revision 1.8
diff -u -p -r1.8 quat.c
--- src/quat.c	14 Dec 2003 18:57:07 -0000	1.8
+++ src/quat.c	18 Oct 2004 00:44:20 -0000
@@ -260,63 +260,37 @@ void quat_to_matrix(AL_CONST QUAT *q, MA
  */
 void matrix_to_quat(AL_CONST MATRIX_f *m, QUAT *q)
 {
-   float diag;
-   float s;
-   int   i;
-   int   j;
-   int   k;
-   float out[4];
-   static int next[3] = { 1, 2, 0 };
-   ASSERT(m);
-   ASSERT(q);
-
-   diag = m->v[0][0] + m->v[1][1] + m->v[2][2];
+   float trace = m->v[0][0] + m->v[1][1] + m->v[2][2] + 1.0f;
 
-   if (diag > 0.0f) {
-      s    = (float)(sqrt(diag + 1.0));
-      q->w = s / 2.0;
-      s    = 0.5 / s;
-      q->x = (m->v[1][2] - m->v[2][1]) * s;
-      q->y = (m->v[2][0] - m->v[0][2]) * s;
-      q->z = (m->v[0][1] - m->v[1][0]) * s;
+   if (trace > EPSILON) {
+      float s = 0.5f / sqrtf(trace);
+      q->w = 0.25f / s;
+      q->x = (m->v[2][1] - m->v[1][2]) * s;
+      q->y = (m->v[0][2] - m->v[2][0]) * s;
+      q->z = (m->v[1][0] - m->v[0][1]) * s;
    }
    else {
-      i = 0;
-
-      if (m->v[1][1] > m->v[0][0]) {
-	 i = 1;
+      if (m->v[0][0] > m->v[1][1] && m->v[0][0] > m->v[2][2]) {
+         float s = 2.0f * sqrtf(1.0f + m->v[0][0] - m->v[1][1] - m->v[2][2]);
+         q->x = 0.25f * s;
+         q->y = (m->v[0][1] + m->v[1][0]) / s;
+         q->z = (m->v[0][2] + m->v[2][0]) / s;
+         q->w = (m->v[1][2] - m->v[2][1]) / s;
       }
-
-      if (m->v[2][2] > m->v[i][i]) {
-	 i = 2;
+      else if (m->v[1][1] > m->v[2][2]) {
+         float s = 2.0f * sqrtf(1.0f + m->v[1][1] - m->v[0][0] - m->v[2][2]);
+         q->x = (m->v[0][1] + m->v[1][0]) / s;
+         q->y = 0.25f * s;
+         q->z = (m->v[1][2] + m->v[2][1]) / s;
+         q->w = (m->v[0][2] - m->v[2][0]) / s;
+      }
+      else {
+         float s = 2.0f * sqrtf(1.0f + m->v[2][2] - m->v[0][0] - m->v[1][1]);
+         q->x = (m->v[0][2] + m->v[2][0]) / s;
+         q->y = (m->v[1][2] + m->v[2][1]) / s;
+         q->z = 0.25f * s;
+         q->w = (m->v[0][1] - m->v[1][0]) / s;
       }
-
-      j = next[i];
-      k = next[j];
-
-      s = m->v[i][i] - (m->v[j][j] + m->v[k][k]);
-
-      /* NOTE: Passing non-orthonormalized matrices can result in odd things
-       *       happening, like the calculation of s below trying to find the
-       *       square-root of a negative number, which is imaginary.  Some
-       *       implementations of sqrt will crash, while others return this
-       *       as not-a-number (NaN). NaN could be very subtle because it will
-       *       not throw an exception on Intel processors.
-       */
-      ASSERT(s > 0.0);
-
-      s = (float)(sqrt(s) + 1.0);
-
-      out[i] = s / 2.0;
-      s      = 0.5 / s;
-      out[j] = (m->v[i][j] + m->v[j][i]) * s;
-      out[k] = (m->v[i][k] + m->v[k][i]) * s;
-      out[3] = (m->v[j][k] - m->v[k][j]) * s;
-
-      q->x = out[0];
-      q->y = out[1];
-      q->z = out[2];
-      q->w = out[3];
    }
 }
 
Index: src/math3d.c
===================================================================
RCS file: /cvsroot/alleg/allegro/src/math3d.c,v
retrieving revision 1.8
diff -u -p -r1.8 math3d.c
--- src/math3d.c	14 Dec 2003 18:57:07 -0000	1.8
+++ src/math3d.c	18 Oct 2004 00:44:22 -0000
@@ -365,10 +365,10 @@ void get_align_matrix(MATRIX *m, fixed x
    ASSERT(m);
 
    normalize_vector(&xfront, &yfront, &zfront);
-   normalize_vector(&xup, &yup, &zup);
-
-   cross_product(xfront, yfront, zfront, xup, yup, zup, &xright, &yright, &zright);
-   cross_product(xright, yright, zright, xfront, yfront, zfront, &xup, &yup, &zup);
+   cross_product(xup, yup, zup, xfront, yfront, zfront, &xright, &yright, &zright);
+   normalize_vector(&xright, &yright, &zright);
+   cross_product(xfront, yfront, zfront, xright, yright, zright, &xup, &yup, &zup);
+   /* No need to normalize up here, since right and front are perpendicular and normalized. */
 
    m->v[0][0] = xright; 
    m->v[0][1] = xup; 
@@ -396,10 +396,10 @@ void get_align_matrix_f(MATRIX_f *m, flo
    ASSERT(m);
 
    normalize_vector_f(&xfront, &yfront, &zfront);
-   normalize_vector_f(&xup, &yup, &zup);
-
-   cross_product_f(xfront, yfront, zfront, xup, yup, zup, &xright, &yright, &zright);
-   cross_product_f(xright, yright, zright, xfront, yfront, zfront, &xup, &yup, &zup);
+   cross_product_f(xup, yup, zup, xfront, yfront, zfront, &xright, &yright, &zright);
+   normalize_vector_f(&xright, &yright, &zright);
+   cross_product_f(xfront, yfront, zfront, xright, yright, zright, &xup, &yup, &zup);
+   /* No need to normalize up here, since right and front are perpendicular and normalized. */
 
    m->v[0][0] = xright; 
    m->v[0][1] = xup; 
Index: examples/exquat.c
===================================================================
RCS file: /cvsroot/alleg/allegro/examples/exquat.c,v
retrieving revision 1.13
diff -u -p -r1.13 exquat.c
--- examples/exquat.c	4 Aug 2004 14:02:04 -0000	1.13
+++ examples/exquat.c	18 Oct 2004 00:44:22 -0000
@@ -9,7 +9,7 @@
  *    solved by using Allegro's quaternion operations.
  *
  *    In this program, two cubes are rotated between random orientations.
- *    Notice that although they are have the same beginning and ending
+ *    Notice that although they have the same beginning and ending
  *    orientations, they do not follow the same path between orientations.
  *
  *    One cube is being rotated by directly incrementing or decrementing
@@ -99,7 +99,8 @@ EULER e_in;
  * It should be noted that the world coordinate system in this
  * program is oriented like it is in most math books. X and Y
  * are oriented like a floor and Z refers to the height above
- * that floor.
+ * that floor. (Mathematically, this is known as right-handed
+ * coordinate system.)
  *
  * N - North
  * S - South
@@ -371,7 +372,7 @@ int main(void)
 	 /* create a matrix for the starting orientation. This time
 	  * we create it using quaternions.  This is to demonstrate
 	  * that the quaternion gotten with get_rotation_quat will
-	  * generate the save matrix as that gotten by get_rotation_matrix
+	  * generate the same matrix as that gotten by get_rotation_matrix
 	  */
 	 get_rotation_quat(&q_from, e_from.x, e_from.y, e_from.z);
 	 quat_to_matrix(&q_from, &rotation);
@@ -414,13 +415,12 @@ int main(void)
 				 tmp_points, path_edges, index+1, index,
 				 palette_color[5]);
 
-	 /* update the screen */
-	 vsync();
-
          acquire_bitmap(screen);
 	 blit(euler_buffer, screen, 0, 0, 0,   120, 320, 240);
 	 blit(quat_buffer,  screen, 0, 0, 320, 120, 320, 240);
          release_bitmap(screen);
+
+	 rest (1);
       }
 
       /* handle user input */
@@ -454,5 +454,4 @@ int main(void)
       }
    }
 }
-
 END_OF_MAIN()