/***************************************************************
 * PrQuaternion.h
 *
 * This class provides Quaternion.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/20/97: K.C. Chang: added inline methods.
 * 11/05/97: K.C. Chang: created.
 */

#ifndef _PrQuaternion_h
#define _PrQuaternion_h

#include "PrGlobalDefn.h"
#include "PrVector3.h"

class PrMatrix3;

class PrQuaternion
{
  public:
    PrQuaternion() : w_(1.0) {} // initialized to [I]
    PrQuaternion(const PrQuaternion &q) : w_(q.w_), v_(q.v_) {}
    PrQuaternion(Float w, const PrVector3 &v) : w_(w), v_(v) {}
    PrQuaternion(const PrVector3 &axis,Float angle);
    PrQuaternion(Float w,Float x,Float y,Float z);
    ~PrQuaternion() {}

    void identity();

  PrVector3 v() { return v_; }
  Float &w() { return w_; }

    PrQuaternion &operator=(const PrQuaternion &q);

    int operator==(const PrQuaternion &q);

    PrQuaternion operator-() const;
    void negate(PrQuaternion &dest) const;

    PrQuaternion operator~() const;
    void inverse(PrQuaternion &dest) const;

    PrQuaternion operator+(const PrQuaternion &q) const;
    void add(const PrQuaternion &q,PrQuaternion &dest) const;

    PrQuaternion operator-(const PrQuaternion &q) const;
    void subtract(const PrQuaternion &q,PrQuaternion &dest) const;

    PrQuaternion operator*(const PrQuaternion &q) const;
    void multiply(const PrQuaternion &q,PrQuaternion &dest) const;

    PrQuaternion operator*(Float s) const;
    void multiply(Float s,PrQuaternion &dest) const;

    PrVector3 operator*(const PrVector3 &v) const;
    void multiply(const PrVector3 &v,PrVector3 &dest) const;

    PrQuaternion &operator+=(const PrQuaternion &q);
    PrQuaternion &operator-=(const PrQuaternion &q);
    PrQuaternion &operator*=(const PrQuaternion &q);
    PrQuaternion &operator*=(Float s);

    Float dot(const PrQuaternion &q) const;

    void normalize();

    void values(const PrVector3 &axis,Float angle);
    inline void values(Float w,Float x,Float y,Float z);

    // convert to axis-angle notation
    void axisAngle(PrVector3 &axis, Float &angle) const;

    PrMatrix3 matrix() const;
    void matrix(PrMatrix3 &dest) const;

    PrVector3 column(int col) const;
    void column(int col, PrVector3 &dest) const;

    PrVector3 thirdColumn() const { return column(2); }
    void thirdColumn(PrVector3 &dest) const { column(2,dest); }

    // dPhi = Einv ( q - qd) = -2*q_tilde^T qd <-- since (q_tilde^T q) = 0
    // E = 0.5*q_tilde
    // Einv = (EtE)inv Et = 4Et = 2*qtilde^T <-- since (q_tilde^T q_tilde) = I
    // q_tilde = [ -x -y -z; w -z  y; z  w -x; -y  x  w ] 
    // q_tilde^T = [ -x  w  z -y; -y -z  w  x; -z  y -x  w ] 	       
    void angularError(const PrQuaternion &qd,PrVector3 &dPhi) const;
    // dq = E omega
    // dq = 0.5 q_tilde omega
    void velocity(const PrVector3 &omega,PrQuaternion &dq) const;

    void display(char *name=NULL);

  private:
    Float w_;
    PrVector3 v_;
};

// inline methods

inline PrQuaternion::PrQuaternion(Float w,Float x,Float y,Float z)
{
    values(w,x,y,z);
}

inline PrQuaternion::PrQuaternion(const PrVector3 &axis,Float angle)
{
    values(axis,angle);
}

inline void PrQuaternion::identity()
{
    w_ = 1.0;
    v_.values(0.0,0.0,0.0);
}

inline PrQuaternion &PrQuaternion::operator=(const PrQuaternion &q)
{
    if (this != &q)
      {
	  w_ = q.w_;
	  v_ = q.v_;
      }
    return (*this);
}

inline void PrQuaternion::values(Float w,Float x,Float y,Float z)
{
    w_ = w;
    v_.values(x,y,z); 
}

#endif // _PrQuaternion_h

