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

Timestamp:

Jan 19, 2006, 12:27:45 PM (18 years ago)

Author:

bensch

Message:

orxonox/trunk: taken the quaternion outside of Vector.cc to quaternion.cc/h

File:

• trunk/src/lib/math/quaternion.cc (copied) (copied from trunk/src/lib/math/vector.cc) (3 diffs)

trunk/src/lib/math/quaternion.cc

@@ -21 +21 @@
 #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
 
-#include "vector.h"
+#include "quaternion.h"
 #ifdef DEBUG
   #include "debug.h"
@@ -30 +30 @@
 
 using namespace std;
-
-/////////////
-/* VECTORS */
-/////////////
-/**
- *  returns the this-vector normalized to length 1.0
- * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.
- */
-Vector Vector::getNormalized() const
-{
-  float l = this->len();
-  if(unlikely(l == 1.0 || l == 0.0))
-    return *this;
-  else
-    return (*this / l);
-}
-
-/**
- *  Vector is looking in the positive direction on all axes after this
-*/
-Vector Vector::abs()
-{
-  Vector v(fabs(x), fabs(y), fabs(z));
-  return v;
-}
-
-
-
-/**
- *  Outputs the values of the Vector
- */
-void Vector::debug() const
-{
-  PRINT(0)("x: %f; y: %f; z: %f", x, y, z);
-  PRINT(0)(" lenght: %f", len());
-  PRINT(0)("\n");
-}
 
 /////////////////
@@ -274 +237 @@
   PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );
 }
-
-/**
- *  create a rotation from a vector
- * @param v: a vector
-*/
-Rotation::Rotation (const Vector& v)
-{
-  Vector x = Vector( 1, 0, 0);
-  Vector axis = x.cross( v);
-  axis.normalize();
-  float angle = angleRad( x, v);
-  float ca = cos(angle);
-  float sa = sin(angle);
-  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
-  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
-  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
-  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
-  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
-  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
-  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
-  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
-  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
-}
-
-/**
- *  creates a rotation from an axis and an angle (radians!)
- * @param axis: the rotational axis
- * @param angle: the angle in radians
-*/
-Rotation::Rotation (const Vector& axis, float angle)
-{
-  float ca, sa;
-  ca = cos(angle);
-  sa = sin(angle);
-  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
-  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
-  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
-  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
-  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
-  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
-  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
-  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
-  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
-}
-
-/**
- *  creates a rotation from euler angles (pitch/yaw/roll)
- * @param pitch: rotation around z (in radians)
- * @param yaw: rotation around y (in radians)
- * @param roll: rotation around x (in radians)
-*/
-Rotation::Rotation ( float pitch, float yaw, float roll)
-{
-  float cy, sy, cr, sr, cp, sp;
-  cy = cos(yaw);
-  sy = sin(yaw);
-  cr = cos(roll);
-  sr = sin(roll);
-  cp = cos(pitch);
-  sp = sin(pitch);
-  m[0] = cy*cr;
-  m[1] = -cy*sr;
-  m[2] = sy;
-  m[3] = cp*sr+sp*sy*cr;
-  m[4] = cp*cr-sp*sr*sy;
-  m[5] = -sp*cy;
-  m[6] = sp*sr-cp*sy*cr;
-  m[7] = sp*cr+cp*sy*sr;
-  m[8] = cp*cy;
-}
-
-/**
- *  creates a nullrotation (an identity rotation)
-*/
-Rotation::Rotation ()
-{
-  m[0] = 1.0f;
-  m[1] = 0.0f;
-  m[2] = 0.0f;
-  m[3] = 0.0f;
-  m[4] = 1.0f;
-  m[5] = 0.0f;
-  m[6] = 0.0f;
-  m[7] = 0.0f;
-  m[8] = 1.0f;
-}
-
-/**
- *  fills the specified buffer with a 4x4 glmatrix
- * @param buffer: Pointer to an array of 16 floats
-
-   Use this to get the rotation in a gl-compatible format
-*/
-void Rotation::glmatrix (float* buffer)
-{
-        buffer[0] = m[0];
-        buffer[1] = m[3];
-        buffer[2] = m[6];
-        buffer[3] = m[0];
-        buffer[4] = m[1];
-        buffer[5] = m[4];
-        buffer[6] = m[7];
-        buffer[7] = m[0];
-        buffer[8] = m[2];
-        buffer[9] = m[5];
-        buffer[10] = m[8];
-        buffer[11] = m[0];
-        buffer[12] = m[0];
-        buffer[13] = m[0];
-        buffer[14] = m[0];
-        buffer[15] = m[1];
-}
-
-/**
- *  multiplies two rotational matrices
- * @param r: another Rotation
- * @return the matrix product of the Rotations
-
-   Use this to rotate one rotation by another
-*/
-Rotation Rotation::operator* (const Rotation& r)
-{
-        Rotation p;
-
-        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
-        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
-        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
-
-        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
-        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
-        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
-
-        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
-        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
-        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
-
-        return p;
-}
-
-
-/**
- *  rotates the vector by the given rotation
- * @param v: a vector
- * @param r: a rotation
- * @return the rotated vector
-*/
-Vector rotateVector( const Vector& v, const Rotation& r)
-{
-  Vector t;
-
-  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
-  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
-  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
-
-  return t;
-}
-
-/**
- *  calculate the distance between two lines
- * @param l: the other line
- * @return the distance between the lines
-*/
-float Line::distance (const Line& l) const
-{
-  float q, d;
-  Vector n = a.cross(l.a);
-  q = n.dot(r-l.r);
-  d = n.len();
-  if( d == 0.0) return 0.0;
-  return q/d;
-}
-
-/**
- *  calculate the distance between a line and a point
- * @param v: the point
- * @return the distance between the Line and the point
-*/
-float Line::distancePoint (const Vector& v) const
-{
-  Vector d = v-r;
-  Vector u = a * d.dot( a);
-  return (d - u).len();
-}
-
-/**
- *  calculate the distance between a line and a point
- * @param v: the point
- * @return the distance between the Line and the point
- */
-float Line::distancePoint (const sVec3D& v) const
-{
-  Vector s(v[0], v[1], v[2]);
-  Vector d = s - r;
-  Vector u = a * d.dot( a);
-  return (d - u).len();
-}
-
-/**
- *  calculate the two points of minimal distance of two lines
- * @param l: the other line
- * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
-*/
-Vector* Line::footpoints (const Line& l) const
-{
-  Vector* fp = new Vector[2];
-  Plane p = Plane (r + a.cross(l.a), r, r + a);
-  fp[1] = p.intersectLine (l);
-  p = Plane (fp[1], l.a);
-  fp[0] = p.intersectLine (*this);
-  return fp;
-}
-
-/**
-  \brief calculate the length of a line
-  \return the lenght of the line
-*/
-float Line::len() const
-{
-  return a.len();
-}
-
-/**
- *  rotate the line by given rotation
- * @param rot: a rotation
-*/
-void Line::rotate (const Rotation& rot)
-{
-  Vector t = a + r;
-  t = rotateVector( t, rot);
-  r = rotateVector( r, rot),
-  a = t - r;
-}
-
-/**
- *  create a plane from three points
- * @param a: first point
- * @param b: second point
- * @param c: third point
-*/
-Plane::Plane (const Vector& a, const Vector& b, const Vector& c)
-{
-  n = (a-b).cross(c-b);
-  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
-}
-
-/**
- *  create a plane from anchor point and normal
- * @param norm: normal vector
- * @param p: anchor point
-*/
-Plane::Plane (const Vector& norm, const Vector& p)
-{
-  n = norm;
-  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
-}
-
-
-/**
-  *  create a plane from anchor point and normal
-  * @param norm: normal vector
-  * @param p: anchor point
-*/
-Plane::Plane (const Vector& norm, const sVec3D& g)
-{
-  Vector p(g[0], g[1], g[2]);
-  n = norm;
-  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
-}
-
-
-/**
- *  returns the intersection point between the plane and a line
- * @param l: a line
-*/
-Vector Plane::intersectLine (const Line& l) const
-{
-  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
-  float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z);
-  return l.r + (l.a * t);
-}
-
-/**
- *  returns the distance between the plane and a point
- * @param p: a Point
- * @return the distance between the plane and the point (can be negative)
-*/
-float Plane::distancePoint (const Vector& p) const
-{
-  float l = n.len();
-  if( l == 0.0) return 0.0;
-  return (n.dot(p) + k) / n.len();
-}
-
-
-/**
- *  returns the distance between the plane and a point
- * @param p: a Point
- * @return the distance between the plane and the point (can be negative)
- */
-float Plane::distancePoint (const sVec3D& p) const
-{
-  Vector s(p[0], p[1], p[2]);
-  float l = n.len();
-  if( l == 0.0) return 0.0;
-  return (n.dot(s) + k) / n.len();
-}
-
-
-/**
- *  returns the side a point is located relative to a Plane
- * @param p: a Point
- * @return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane
-*/
-float Plane::locatePoint (const Vector& p) const
-{
-  return (n.dot(p) + k);
-}
-