PhxQuaternion.h

#ifndef PHX_QUATERNION_H
#define PHX_QUATERNION_H

#include <Phx/PhxConfig.h>
#include <Phx/PhxTypes.h>
#include <Phx/Util/PhxMatrix.h>
#include <math.h>

namespace Phx {

  class Quaternion {
    double mData[4];
    
  public:

    Quaternion() {
      mData[0] = 1;
      mData[1] = mData[2] = mData[3] = 0;
    }

    Quaternion(double w, const Vector3& v) {
      mData[0] = w;
      mData[1] = v.element(0,0);
      mData[2] = v.element(1,0);
      mData[3] = v.element(2,0);
    }

    Quaternion(const double* array) {
      if (!array) {
        std::clog << "Constructing quaternion from 0 array." << std::endl;
        throw RangeException();
      }
      for (uint32_t i = 0; i < 4; i++) {
        mData[i] = array[i];
      }
    }

    Quaternion(double w, double x, double y, double z) {
      mData[0] = w;
      mData[1] = x;
      mData[2] = y;
      mData[3] = z;
    }

    double x() const { return mData[1]; }
    double y() const { return mData[2]; }
    double z() const { return mData[3]; }
    double w() const { return real(); }

    Vector3 complex() const { return Vector3(mData+1); }
    double real() const { return mData[0]; }

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

    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);
    }

    Quaternion operator*(double s) const {
      return Quaternion(real()*s, complex()*s);
    }

    Quaternion operator/(double s) const {
      if (s == 0) std::clog << "Dividing quaternion by 0." << std::endl;
      return Quaternion(real()/s, complex()/s);
    }

    Matrix4 matrix() const {
      double m[16] = {
        x(),  w(), -z(),  y(),
        y(),  z(),  w(), -x(),
        z(), -y(),  x(),  w(),
        w(), -x(), -y(), -z()
      };
      return Matrix4(m);
    }

    Vector4 vector() const { return Vector4(mData); }

    double norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+
                                      mData[2]*mData[2]+mData[3]*mData[3]); }

    Matrix3 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 Matrix3(m);
    }

  };


}; // namespace Phx


#endif /* PHX_QUATERNION_H */

---