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

Timestamp:

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

Author:

bensch

Message:

merged the bsp-model-stuff back here

Location:

trunk/src/lib/math

Files:

• quaternion.cc (modified) (3 diffs)
• quaternion.h (modified) (2 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 );
 }
+
+
+
+
+
+

trunk/src/lib/math/quaternion.h

@@ -42 +42 @@
   inline Quaternion (const Vector& v, float f) { this->w = f; this->v = v; }
   Quaternion (float m[4][4]);
+  Quaternion (float m[3][3]);
   /** turns a rotation along an axis into a Quaternion @param angle: the amount of radians to rotate @param axis: the axis to rotate around */
   inline Quaternion (float angle, const Vector& axis) { w = cos(angle/2.0); v = axis * sin(angle/2.0); }
@@ -118 +119 @@
 
 
+
 #endif /* __QUATERNION_H_ */