Changeset 2112 in orxonox.OLD for orxonox/branches/chris/src/vector.cc

Timestamp:

Jul 12, 2004, 10:28:25 PM (20 years ago)

Author:

chris

Message:

orxonox/branches/chris: Implemented quaternion class… but something is still f up… the camera keeps pointing into the wrong direction… matrix rotation calculation seems not to be my strenght…

File:

• orxonox/branches/chris/src/vector.cc (modified) (2 diffs)

orxonox/branches/chris/src/vector.cc

@@ -14 +14 @@
    main-programmer: Christian Meyer
    co-programmer: ...
+   
+   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
 */
 
@@ -177 +179 @@
   f = acos( v1 * v2 / (v1.len() * v2.len()));
   return f * 180 / PI;
+}
+
+Quaternion::Quaternion ()
+{
+        w = 1;
+        v = Vector(0,0,0);
+}
+
+Quaternion::Quaternion (float angle, const Vector& axis)
+{
+        w = cos(angle/2);
+        v = axis * sin(angle/2);
+}
+
+Quaternion::Quaternion (const Vector& dir, const Vector& up)
+{
+        Vector z = dir;
+  z.normalize(); 
+  Vector x = up.cross(z);
+  x.normalize(); 
+  Vector y = z.cross(x);
+  
+  float m[4][4];
+  m[0][0] = x.x;
+  m[0][1] = x.y;
+  m[0][2] = x.z;
+  m[0][3] = 0;
+  m[1][0] = y.x;
+  m[1][1] = y.y;
+  m[1][2] = y.z;
+  m[1][3] = 0;
+  m[2][0] = z.x;
+  m[2][1] = z.y;
+  m[2][2] = z.z;
+  m[2][3] = 0;
+  m[3][0] = 0;
+  m[3][1] = 0;
+  m[3][2] = 0;
+  m[3][3] = 1;
+  
+  *this = Quaternion (m);
+}
+
+Quaternion::Quaternion (float roll, float pitch, float yaw)
+{
+        float cr, cp, cy, sr, sp, sy, cpcy, spsy;
+        
+        // calculate trig identities
+        cr = cos(roll/2);
+        cp = cos(pitch/2);
+        cy = cos(yaw/2);
+        
+        sr = sin(roll/2);
+        sp = sin(pitch/2);
+        sy = sin(yaw/2);
+        
+        cpcy = cp * cy;
+        spsy = sp * sy;
+        
+        w = cr * cpcy + sr * spsy;
+        v.x = sr * cpcy - cr * spsy;
+        v.y = cr * sp * cy + sr * cp * sy;
+        v.z = cr * cp * sy - sr * sp * cy;
+}
+
+Quaternion Quaternion::operator*(const Quaternion& q) const
+{
+        float A, B, C, D, E, F, G, H;
+        Quaternion r;
+        
+        A = (w   + v.x)*(q.w   + q.v.x);
+        B = (v.z - v.y)*(q.v.y - q.v.z);
+        C = (w   - v.x)*(q.v.y + q.v.z); 
+        D = (v.y + v.z)*(q.w   - q.v.x);
+        E = (v.x + v.z)*(q.v.x + q.v.y);
+        F = (v.x - v.z)*(q.v.x - q.v.y);
+        G = (w   + v.y)*(q.w   - q.v.z);
+        H = (w   - v.y)*(q.w   + q.v.z);
+        
+        r.w = B +  (-E - F + G + H)/2;
+        r.v.x = A - (E + F + G + H)/2; 
+        r.v.y = C + (E - F + G - H)/2; 
+        r.v.z = D + (E - F - G + H)/2;
+        
+        return r;
+}
+
+Quaternion Quaternion::operator+(const Quaternion& q) const
+{
+        Quaternion r(*this);
+        r.w = r.w + q.w;
+        r.v = r.v + q.v;
+        return r;
+}
+
+Quaternion Quaternion::operator- (const Quaternion& q) const
+{
+        Quaternion r(*this);
+        r.w = r.w - q.w;
+        r.v = r.v - q.v;
+        return r;
+}
+
+Vector Quaternion::apply (Vector& v) const
+{
+        Quaternion q;
+        q.v = v;
+        q.w = 0;
+        q = *this * q * conjugate();
+        return q.v;
+}
+
+Quaternion Quaternion::operator*(const float& f) const
+{
+        Quaternion r(*this);
+        r.w = r.w*f;
+        r.v = r.v*f;
+        return r;
+}
+
+Quaternion Quaternion::operator/(const float& f) const
+{
+        if( f == 0) return Quaternion();
+        Quaternion r(*this);
+        r.w = r.w/f;
+        r.v = r.v/f;
+        return r;
+}
+
+Quaternion Quaternion::conjugate() const
+{
+        Quaternion r(*this);
+        r.v = Vector() - r.v;
+        return r;
+}
+
+float Quaternion::norm() const
+{
+        return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
+}
+
+Quaternion Quaternion::inverse() const
+{
+        float n = norm();
+        if (n != 0)
+        {
+                return conjugate() / norm();
+        }
+        else return Quaternion();
+}
+
+void Quaternion::matrix (float m[4][4]) const
+{
+        float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
+
+        // calculate coefficients
+        x2 = v.x + v.x;
+  y2 = v.y + v.y; 
+        z2 = v.z + v.z;
+        xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
+        yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
+        wx = w * x2; wy = w * y2; wz = w * z2;
+        
+        m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
+        m[2][0] = xz + wy; m[3][0] = 0.0;
+        
+        m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
+        m[2][1] = yz - wx; m[3][1] = 0.0;
+        
+        m[0][2] = xz - wy; m[1][2] = yz + wx;
+        m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
+        
+        m[0][3] = 0; m[1][3] = 0;
+        m[2][3] = 0; m[3][3] = 1;
+}
+
+Quaternion::Quaternion (float m[4][4])
+{
+        
+  float  tr, s, q[4];
+  int    i, j, k;
+
+  int nxt[3] = {1, 2, 0};
+
+  tr = m[0][0] + m[1][1] + m[2][2];
+
+        // check the diagonal
+  if (tr > 0.0) 
+  {
+    s = sqrt (tr + 1.0);
+    w = s / 2.0;
+    s = 0.5 / s;
+    v.x = (m[1][2] - m[2][1]) * s;
+    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;
+    if (m[2][2] > m[i][i]) i = 2;
+    j = nxt[i];
+    k = nxt[j];
+
+    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
+      
+    q[i] = s * 0.5;
+            
+    if (s != 0.0) s = 0.5 / 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];
+  }
 }