quaternion.h

#ifndef CMU462_QUATERNION_H
#define CMU462_QUATERNION_H

#include "CMU462.h"
#include "matrix3x3.h"
#include "matrix4x4.h"

#include <iosfwd>

namespace CMU462 {

class Quaternion : public Vector4D {
 public:

  Quaternion( ) : Vector4D( 0.0, 0.0, 0.0, 1.0 ) { }

  Quaternion(const Vector3D& v, double w) : Vector4D(v.x, v.y, v.z, w) { }

  Quaternion(const Vector4D& v) : Vector4D(v.x, v.y, v.z, v.w) { }

  Quaternion(double x, double y, double z, double w) : Vector4D(x, y, z, w) { }

  void from_axis_angle(const Vector3D& axis, double radians) {
    radians /= 2;
    const Vector3D& nAxis = axis.unit();
    double sinTheta = sin(radians);
    x = sinTheta * nAxis.x;
    y = sinTheta * nAxis.y;
    z = sinTheta * nAxis.z;
    w = cos(radians);
    this->normalize();
  }

  Vector3D complex() const { return Vector3D(x, y, z); }
  void setComplex(const Vector3D& c)
  {
    x = c.x;
    y = c.y;
    z = c.z;
  }

  double real() const { return w; }
  void setReal(double r) { w = r; }

  Quaternion conjugate(void) const
  {
    return Quaternion(-complex(), real());
  }

  Quaternion inverse(void) const
  {
    return conjugate() / norm();
  }


  Quaternion product(const Quaternion& rhs) const {
    return Quaternion(y*rhs.z - z*rhs.y + x*rhs.w + w*rhs.x,
                      z*rhs.x - x*rhs.z + y*rhs.w + w*rhs.y,
                      x*rhs.y - y*rhs.x + z*rhs.w + w*rhs.z,
                      w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z);
  }

  Quaternion operator*(const Quaternion& rhs) const {
    return product(rhs);
  }

  Matrix4x4 matrix() const {

    double m[16] = {
       w,  -z,  y, x,
       z,   w, -x, y,
      -y,   x,  w, z,
      -x,  -y, -z, w
    };

    return Matrix4x4(m);
  }

  Matrix4x4 rightMatrix() const {
    double m[16] = {
      +w, -z,  y, -x,
      +z,  w, -x, -y,
      -y,  x,  w, -z,
      +x,  y,  z,  w
    };

    return Matrix4x4(m);
  }

  Vector4D vector() const { return Vector4D(x, y, z, w); }

  Matrix3x3 rotationMatrix() const {
    double m[9] = {
      1-2*y*y-2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w,
      2*x*y + 2*z*w, 1-2*x*x-2*z*z, 2*y*z - 2*x*w,
      2*x*z - 2*y*w, 2*y*z + 2*x*w, 1-2*x*x-2*y*y
    };

    return Matrix3x3(m);
  }


  Vector3D scaledAxis(void) const{

    Quaternion q1 = (Quaternion)unit();

    // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/

    double angle = 2 * acos(q1.w);

    // s must be positive, because q1 <= 1, due to normalization.
    double s = sqrt(1-q1.w*q1.w);

    // Avoid dividing by 0.
    if (s < 0.001)
    {
      // if s close to zero then direction of axis not important
      // if it is important that axis is normalised then replace with x=1; y=z=0;

      return Vector3D(q1.x, q1.y, q1.z);
    }
    else
    {
      // normalise axis
      return Vector3D(q1.x / s, q1.y / s, q1.z / s);
    }

    // NEVER getsgg HERE.

  }

  void scaledAxis(const Vector3D& vec_in)
  {
    double theta = vec_in.norm();

    // Small magnitudes are handled via the default vector.
    if (theta > 0.0001)
    {
      double s = sin(theta / 2.0);
      Vector3D W(vec_in / theta * s);
      x = W.x;
      y = W.y;
      z = W.z;
      w = cos(theta / 2.0);
    }
    else
    {
      x = y = z = 0;
      w = 1.0;
    }
  }

  Vector3D rotatedVector(const Vector3D& v) const {
    return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
  }



  void euler(const Vector3D& euler) {
    double c1 = cos(euler[2] * 0.5);
    double c2 = cos(euler[1] * 0.5);
    double c3 = cos(euler[0] * 0.5);
    double s1 = sin(euler[2] * 0.5);
    double s2 = sin(euler[1] * 0.5);
    double s3 = sin(euler[0] * 0.5);

    x = c1*c2*s3 - s1*s2*c3;
    y = c1*s2*c3 + s1*c2*s3;
    z = s1*c2*c3 - c1*s2*s3;
    w = c1*c2*c3 + s1*s2*s3;
  }

  Vector3D euler(void) const
  {
    Vector3D euler;
    const static double PI_OVER_2 = M_PI * 0.5;
    double sqw, sqx, sqy, sqz;

    // quick conversion to Euler angles to give tilt to user
    sqw = w*w;
    sqx = x*x;
    sqy = y*y;
    sqz = z*z;

    euler[1] = asin(2.0 * (w*y - x*z));
    if (PI_OVER_2 - fabs(euler[1]) > EPS_D) {
      euler[2] = atan2(2.0 * (x*y + w*z),
                       sqx - sqy - sqz + sqw);
      euler[0] = atan2(2.0 * (w*x + y*z),
                       sqw - sqx - sqy + sqz);
    }
    else
    {
      // compute heading from local 'down' vector
      euler[2] = atan2(2*y*z - 2*x*w,
                       2*x*z + 2*y*w);
      euler[0] = 0.0;

      // If facing down, reverse yaw
      if (euler[1] < 0)
      {
        euler[2] = M_PI - euler[2];
      }
    }

    return euler;
  }

  void decoupleZ(Quaternion* Qxy, Quaternion* Qz) const {
    Vector3D ztt(0,0,1);
    Vector3D zbt = this->rotatedVector(ztt);
    Vector3D axis_xy = cross(ztt, zbt);
    double axis_norm = axis_xy.norm();

    double axis_theta = acos(clamp(zbt.z, -1.0, 1.0));
    if (axis_norm > 0.00001)
    {
      axis_xy = axis_xy * (axis_theta/axis_norm); // limit is *1
    }

    Qxy->scaledAxis(axis_xy);
    *Qz = (Qxy->conjugate() * (*this));
  }

  Quaternion slerp(const Quaternion& q1, double t)
  {
    return slerp(*this, q1, t);
  }

  static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, double t) {

    double omega = acos(clamp(q0.x*q1.x +
                              q0.y*q1.y +
                              q0.z*q1.z +
                              q0.w*q1.w, -1.0, 1.0));
    if (fabs(omega) < 1e-10)
    {
      omega = 1e-10;
    }
    double som = sin(omega);
    double st0 = sin((1-t) * omega) / som;
    double st1 = sin(t * omega) / som;

    return Quaternion(q0.x*st0 + q1.x*st1,
                      q0.y*st0 + q1.y*st1,
                      q0.z*st0 + q1.z*st1,
                      q0.w*st0 + q1.w*st1);
  }


};

Quaternion operator*(double s, const Quaternion& q);

} // namespace CMU462

#endif /* CMU462_QUATERNION_H */