Changeset 8724 in orxonox.OLD for trunk/src/lib/math/quaternion.cc

Timestamp:

Jun 22, 2006, 3:14:58 PM (18 years ago)

Author:

bensch

Message:

merged the bsp-model-stuff back here

File:

• trunk/src/lib/math/quaternion.cc (modified) (3 diffs)

trunk/src/lib/math/quaternion.cc

@@ -133 +133 @@
 
 
+
 /**
  * @brief Slerps this QUaternion performs a smooth move.
@@ -298 +299 @@
 }
 
+
+/**
+ * Creates a quaternion from a 3x3 rotation matrix.
+ * @param mat The 3x3 source rotation matrix.
+ * @return The equivalent 4 float quaternion.
+ */
+Quaternion::Quaternion(float mat[3][3])
+{
+  int   NXT[] = {1, 2, 0};
+  float q[4];
+
+  // check the diagonal
+  float tr = mat[0][0] + mat[1][1] + mat[2][2];
+  if( tr > 0.0f) {
+    float s = (float)sqrtf(tr + 1.0f);
+    this->w = s * 0.5f;
+    s = 0.5f / s;
+    this->v.x = (mat[1][2] - mat[2][1]) * s;
+    this->v.y = (mat[2][0] - mat[0][2]) * s;
+    this->v.z = (mat[0][1] - mat[1][0]) * s;
+  }
+  else
+  {
+    // diagonal is negative
+    // get biggest diagonal element
+    int i = 0;
+    if (mat[1][1] > mat[0][0]) i = 1;
+    if (mat[2][2] > mat[i][i]) i = 2;
+    //setup index sequence
+    int j = NXT[i];
+    int k = NXT[j];
+
+    float s = (float)sqrtf((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0f);
+
+    q[i] = s * 0.5f;
+
+    if (s != 0.0f) s = 0.5f / s;
+
+    q[j] = (mat[i][j] + mat[j][i]) * s;
+    q[k] = (mat[i][k] + mat[k][i]) * s;
+    q[3] = (mat[j][k] - mat[k][j]) * s;
+
+    this->v.x = q[0];
+    this->v.y = q[1];
+    this->v.z = q[2];
+    this->w   = q[3];
+  }
+}
+
 /**
  * @brief outputs some nice formated debug information about this quaternion
@@ -314 +364 @@
   PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
 }
+
+
+
+
+
+