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

Timestamp:

Apr 19, 2006, 2:55:29 PM (19 years ago)

Author:

bensch

Message:

orxonox/trunk: new Quaternion Functionality. Also added <abs-dir> to TurbineHover

Location:

trunk/src/lib/math

Files:

• quaternion.cc (modified) (12 diffs)
• quaternion.h (modified) (1 diff)

trunk/src/lib/math/quaternion.cc

@@ -35 +35 @@
 /////////////////
 /**
- *  calculates a lookAt rotation
+ * @brief calculates a lookAt rotation
  * @param dir: the direction you want to look
  * @param up: specify what direction up should be
-
-   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
-   the same way as dir. If you want to use this with cameras, you'll have to reverse the
-   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
-   can use this for meshes as well (then you do not have to reverse the vector), but keep
-   in mind that if you do that, the model's front has to point in +z direction, and left
-   and right should be -x or +x respectively or the mesh wont rotate correctly.
+ *
+ * Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
+ * the same way as dir. If you want to use this with cameras, you'll have to reverse the
+ * dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
+ * can use this for meshes as well (then you do not have to reverse the vector), but keep
+ * in mind that if you do that, the model's front has to point in +z direction, and left
+ * and right should be -x or +x respectively or the mesh wont rotate correctly.
  *
  * @TODO !!! OPTIMIZE THIS !!!
@@ -76 +76 @@
 
 /**
- *  calculates a rotation from euler angles
+ * @brief calculates a rotation from euler angles
  * @param roll: the roll in radians
  * @param pitch: the pitch in radians
@@ -104 +104 @@
 
 /**
- *  convert the Quaternion to a 4x4 rotational glMatrix
+ * @brief convert the Quaternion to a 4x4 rotational glMatrix
  * @param m: a buffer to store the Matrix in
  */
@@ -134 +134 @@
 
 /**
- * Slerps this QUaternion performs a smooth move.
+ * @brief Slerps this QUaternion performs a smooth move.
  * @param toQuat to this Quaternion
  * @param t \% inth the the direction[0..1]
@@ -167 +167 @@
   scale1 = sin(t * omega) / sinom;
   this->v = Vector(scale0 * this->v.x + scale1 * tol[0],
-                    scale0 * this->v.y + scale1 * tol[1],
-                    scale0 * this->v.z + scale1 * tol[2]);
+                   scale0 * this->v.y + scale1 * tol[1],
+                   scale0 * this->v.z + scale1 * tol[2]);
   this->w = scale0 * this->w + scale1 * tol[3];
 }
@@ -174 +174 @@
 
 /**
- *  performs a smooth move.
+ * @brief performs a smooth move.
  * @param from  where
  * @param to where
@@ -189 +189 @@
 
   if( cosom < 0.0 )
-    {
-      cosom = -cosom;
-      tol[0] = -to.v.x;
-      tol[1] = -to.v.y;
-      tol[2] = -to.v.z;
-      tol[3] = -to.w;
-    }
+  {
+    cosom = -cosom;
+    tol[0] = -to.v.x;
+    tol[1] = -to.v.y;
+    tol[2] = -to.v.z;
+    tol[3] = -to.w;
+  }
   else
-    {
-      tol[0] = to.v.x;
-      tol[1] = to.v.y;
-      tol[2] = to.v.z;
-      tol[3] = to.w;
-    }
+  {
+    tol[0] = to.v.x;
+    tol[1] = to.v.y;
+    tol[2] = to.v.z;
+    tol[3] = to.w;
+  }
 
   omega = acos(cosom);
@@ -209 +209 @@
   scale1 = sin(t * omega) / sinom;
   return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
-                    scale0 * from.v.y + scale1 * tol[1],
-                    scale0 * from.v.z + scale1 * tol[2]),
+                           scale0 * from.v.y + scale1 * tol[1],
+                           scale0 * from.v.z + scale1 * tol[2]),
                     scale0 * from.w + scale1 * tol[3]);
 }
 
-
-/**
- *  convert a rotational 4x4 glMatrix into a Quaternion
+/**
+ * @returns the heading
+ */
+float Quaternion::getHeading() const
+{
+  float pole = this->v.x*this->v.y + this->v.z*this->w;
+  if (fabsf(pole) != 0.5)
+    return atan2(2.0* (v.y*w - v.x*v.z), 1 - 2.0*(v.y*v.y - v.z*v.z));
+  else if (pole == .5) // North Pole
+    return 2.0 * atan2(v.x, w);
+  else // South Pole
+    return -2.0 * atan2(v.x, w);
+}
+
+/**
+ * @returns the Attitude
+ */
+float Quaternion::getAttitude() const
+{
+  return asin(2.0 * (v.x*v.y + v.z*w));
+}
+
+/**
+ * @returns the Bank
+ */
+float Quaternion::getBank() const
+{
+  if (fabsf(this->v.x*this->v.y + this->v.z*this->w) != 0.5)
+    return atan2(2.0*(v.x*w-v.y*v.z) , 1 - 2.0*(v.x*v.x - v.z*v.z));
+  else
+    return 0.0f;
+}
+
+
+/**
+ * @brief convert a rotational 4x4 glMatrix into a Quaternion
  * @param m: a 4x4 matrix in glMatrix order
  */
@@ -229 +262 @@
   tr = m[0][0] + m[1][1] + m[2][2];
 
-        // check the diagonal
+  // check the diagonal
   if (tr > 0.0)
   {
@@ -238 +271 @@
     v.y = (m[2][0] - m[0][2]) * s;
     v.z = (m[0][1] - m[1][0]) * s;
-        }
-        else
-        {
-                // diagonal is negative
-        i = 0;
-        if (m[1][1] > m[0][0]) i = 1;
+  }
+  else
+  {
+    // diagonal is negative
+    i = 0;
+    if (m[1][1] > m[0][0]) i = 1;
     if (m[2][2] > m[i][i]) i = 2;
     j = nxt[i];
@@ -254 +287 @@
     if (s != 0.0) s = 0.5 / s;
 
-          q[3] = (m[j][k] - m[k][j]) * s;
+    q[3] = (m[j][k] - m[k][j]) * s;
     q[j] = (m[i][j] + m[j][i]) * s;
     q[k] = (m[i][k] + m[k][i]) * s;
 
-        v.x = q[0];
-        v.y = q[1];
-        v.z = q[2];
-        w = q[3];
-  }
-}
-
-/**
- *  outputs some nice formated debug information about this quaternion
+    v.x = q[0];
+    v.y = q[1];
+    v.z = q[2];
+    w = q[3];
+  }
+}
+
+/**
+ * @brief outputs some nice formated debug information about this quaternion
 */
 void Quaternion::debug() const
@@ -273 +306 @@
 }
 
+/**
+ * @brief another better Quaternion Debug Function.
+ */
 void Quaternion::debug2() const
 {

trunk/src/lib/math/quaternion.h

@@ -96 +96 @@
   inline void normalize() { float n = this->norm(); this->v /= n; this->w/=n; };
 
+  float getHeading() const;
+  float getAttitude() const;
+  float getBank() const;
   /** @returns the rotational axis of this Quaternion */
   inline Vector getSpacialAxis() const { return this->v / sin(acos(w));/*sqrt(v.x*v.x + v.y*v.y + v.z+v.z);*/ };