#ifndef _QUATERNION_H_ #define _QUATERNION_H_ #include "XYZpoint.h" #include ////////////////////////////////////////////////////////////////////////// // CLASS PURPOSE: Defines a Quaternion, which is essentially a vector // // with 1 real and 3 imaginary components. Quaternions // // provide a concise representation for rotations. // // This class defines a Quaternion object and provides // // basic Quaternion operations // ////////////////////////////////////////////////////////////////////////// class Quaternion { private: double R; double I; double J; double K; // R is the real component; I, J and K are the imaginary // components public: Quaternion(void) {R=1; I=J=K=0;} // void constructor, identity rotation // constructors for setting RIJK directly Quaternion(double a, double b, double c, double d) {R=a; I=b; J=c; K=d;} Quaternion(float vect[4]) {R=vect[0]; I=vect[1]; J=vect[2]; K=vect[3];} Quaternion(double vect[4]) {R=vect[0]; I=vect[1]; J=vect[2]; K=vect[3];} // Eliminated the following not-very-useful constructor to reduce // programmer errors: // Quaternion(const XYZpoint &A) {R=0; I=A.X; J=A.Y; K=A.Z;} // constructor, sets I, J, K from X, Y, Z of point, R = 0 void axis_vector(XYZpoint &vect) const; XYZpoint axis_vector(void) const; // returns the axis vector(I,J,K) of the Quaternion(R,I,J,K) inline double& r(void) {return R;} inline double& i(void) {return I;} inline double& j(void) {return J;} inline double& k(void) {return K;} // sets the values of the private members inline const double& r(void) const {return R;} inline const double& i(void) const {return I;} inline const double& j(void) const {return J;} inline const double& k(void) const {return K;} // returns the values of the private members inline bool is_finite(void) const { return isfinite(R) && isfinite(I) && isfinite(J) && isfinite(K); } inline bool operator == (const Quaternion& q) const { return R==q.R && I==q.I && J==q.J && K==q.K; } ///////////////////////////////////////////////////////////////// // The following sets of 3 routines provide 3 versions of the add // subtract and times operators ///////////////////////////////////////////////////////////////// Quaternion operator+(const Quaternion &Q) const; // operator to add 2 Quaternions. // Return by value - copy overhead friend Quaternion& add(Quaternion &dest, const Quaternion& a, const Quaternion& b); // Most efficient addition routine, no copy overhead // in return. Requires dest, a and b to be created by // caller routine. // Return by reference - no copy overhead Quaternion* add(const Quaternion* Q) const; // Performs addition and returns a pointer to a new Quaternion // created to store the sum // Return by reference - no copy overhead in return Quaternion& operator+=(const Quaternion &Q); //////////////////////////////////////////////////////////////////////// Quaternion operator-(const Quaternion &Q) const; // operator to subtract 2 Quaternions // Return by value - copy overhead friend Quaternion& subtract(Quaternion &dest, const Quaternion& a, const Quaternion& b); // Most efficient subtraction routine, no copy overhead in // return. Requires dest, a and b to be created by caller // routine // Return by reference - no copy overhead Quaternion* subtract(const Quaternion* Q) const; // Performs subtraction and returns a pointer to a new // Quaternion created to store the diff // Return by reference - no copy overhead in return Quaternion& operator-=(const Quaternion &Q); //////////////////////////////////////////////////////////////////////// Quaternion operator*(const Quaternion &Q) const; // operator for cross-product of 2 Quaternions // Return by value - copy overhead friend Quaternion& cross(Quaternion &dest, const Quaternion& a, const Quaternion& b); // Most efficient product routine, no copy overhead in // return. Requires dest, a and b to be created by caller // routine // Return by reference - no copy overhead Quaternion* cross(const Quaternion* Q) const; // Performs cross-product and returns a pointer to a new // Quaternion created to store the product // Return by reference - no copy overhead in return //////////////////////////////////////////////////////////////////////// Quaternion operator*(const XYZpoint &A) const; // operator for cross-product of a Quaternion and a point // Return by value - copy overhead friend Quaternion& cross(Quaternion &dest, const XYZpoint& a, const Quaternion& b); // Most efficient product routine, no copy overhead in // return. Requires dest, a and b to be created by caller // routine // Return by reference - no copy overhead Quaternion* cross(const XYZpoint *A) const; // Performs cross-product and returns a pointer to a new // Quaternion created to store the product // Return by reference - no copy overhead in return //////////////////////////////////////////////////////////////////////// inline double operator&(const Quaternion &Q) {return dot(*this, Q);} // operator for dot-product of 2 Quaternions operator XYZpoint() const; // casts a Quaternion(0,I,J,K) to point(I,J,K) friend double dot(const Quaternion &P, const Quaternion &Q); // finds dot-product of Quaternions P and Q Quaternion conjugate(void) const; // finds conjugate (R,-I,-J,-K) of (R,I,J,K) // returns by value - copy overhead Quaternion& conjugate(Quaternion &destn) const; // finds conjugate (R,-I,-J,-K) of (R,I,J,K), stores // it in destn Quaternion -- passing by reference, // no copy overhead // multiply or divide a Quaternion by a scaling factor Quaternion& operator *= (double scale) ; Quaternion operator * (double scale) const; Quaternion& operator /= (double scale) ; Quaternion operator / (double scale) const; inline double magnitude(void) const { return sqrt(R*R + I*I + J*J + K*K); } inline double mag2(void) const { return R*R + I*I + J*J + K*K; } // scale the quaternion to a unit quaternion, return *this Quaternion& unitize(void); friend ostream &operator<<(ostream &stream, const Quaternion& Q); friend istream &operator>>(istream &stream, Quaternion &Q); // I/O operations }; inline Quaternion& add(Quaternion& dest, const Quaternion& a, const Quaternion& b) { dest.R = a.R+b.R; dest.I = a.I+b.I; dest.J = a.J+b.J; dest.K = a.K+b.K; return dest; } inline Quaternion* Quaternion::add(const Quaternion* Q) const { Quaternion* ret = new Quaternion; ::add(*ret, *this, *Q); return ret; } inline Quaternion Quaternion::operator+(const Quaternion &Q) const { Quaternion sum; return ::add(sum, *this, Q); } inline Quaternion& Quaternion::operator+=(const Quaternion &Q) { R += Q.R; I += Q.I; J += Q.J; K += Q.K; return *this; } inline Quaternion& subtract(Quaternion& dest, const Quaternion& a, const Quaternion& b) { dest.R = a.R-b.R; dest.I = a.I-b.I; dest.J = a.J-b.J; dest.K = a.K-b.K; return dest; } inline Quaternion* Quaternion::subtract(const Quaternion* Q) const { Quaternion* ret = new Quaternion; ::subtract(*ret, *this, *Q); return ret; } inline Quaternion Quaternion::operator-(const Quaternion &Q) const { Quaternion diff; return ::subtract(diff, *this, Q); } inline Quaternion& Quaternion::operator-=(const Quaternion &Q) { R -= Q.R; I -= Q.I; J -= Q.J; K -= Q.K; return *this; } inline Quaternion& cross(Quaternion& dest, const Quaternion& a, const Quaternion& b) { dest.r() = a.r()*b.r() - a.i()*b.i() - a.j()*b.j() - a.k()*b.k(); dest.i() = a.r()*b.i() + a.i()*b.r() + a.j()*b.k() - a.k()*b.j(); dest.j() = a.r()*b.j() - a.i()*b.k() + a.j()*b.r() + a.k()*b.i(); dest.k() = a.r()*b.k() + a.i()*b.j() - a.j()*b.i() + a.k()*b.r(); return dest; } inline Quaternion* Quaternion::cross(const Quaternion* Q) const { Quaternion* ret = new Quaternion; ::cross(*ret, *this, *Q); return ret; } inline Quaternion Quaternion::operator*(const Quaternion &Q) const { Quaternion prod; return ::cross(prod, *this, Q); } inline Quaternion& cross(Quaternion& dest, const Quaternion& a, const XYZpoint& b) { dest.r() = 0 - a.i()*b.X - a.j()*b.Y - a.k()*b.Z; dest.i() = a.r()*b.X + 0 + a.j()*b.Z - a.k()*b.Y; dest.j() = a.r()*b.Y - a.i()*b.Z + 0 + a.k()*b.X; dest.k() = a.r()*b.Z + a.i()*b.Y - a.j()*b.X + 0 ; return dest; } inline Quaternion* Quaternion::cross(const XYZpoint* Q) const { Quaternion* ret = new Quaternion; ::cross(*ret, *this, *Q); return ret; } inline Quaternion Quaternion::operator*(const XYZpoint &Q) const { Quaternion prod; return ::cross(prod, *this, Q); } inline Quaternion::operator XYZpoint() const { XYZpoint temp; //assert(r()==0); temp.X = i(); temp.Y = j(); temp.Z = k(); return temp; } inline void Quaternion::axis_vector(XYZpoint& A) const { A.X = i(); A.Y = j(); A.Z = k(); } inline XYZpoint Quaternion::axis_vector(void) const { XYZpoint A; axis_vector(A); return A; } inline Quaternion Quaternion::conjugate(void) const { Quaternion ret; ret.r() = R; ret.i() = -I; ret.j() = -J; ret.k() = -K; return ret; } inline Quaternion& Quaternion::conjugate(Quaternion &destn) const { destn.r() = R; destn.i() = -I; destn.j() = -J; destn.k() = -K; return destn; } inline double dot(const Quaternion &Q1, const Quaternion &Q2) { return Q1.r()*Q2.r() + Q1.i()*Q2.i() + Q1.j()*Q2.j() + Q1.k()*Q2.k(); } inline Quaternion& Quaternion::operator*=(double scale) { R *= scale; I *= scale; J *= scale; K *= scale; return *this; } inline Quaternion Quaternion::operator *(double scale) const { Quaternion ret=*this; ret *= scale; return ret; } inline Quaternion& Quaternion::operator/=(double scale) { assert(scale!=0); R /= scale; I /= scale; J /= scale; K /= scale; return *this; } inline Quaternion Quaternion::operator /(double scale) const { Quaternion ret=*this; ret /= scale; return ret; } inline Quaternion& Quaternion::unitize(void) { double scale = 1./ magnitude(); R *= scale; I *= scale; J *= scale; K *= scale; return *this; } // CHANGE LOG: // 30 March 2004 Kevin Karplus // Modified constructor so that parameters don't shadow members. // 24 Oct 2004 Kevin Karplus // Added operator == // Wed Jun 18 07:50:54 PDT 2008 Kevin Karplus // Added is_finite test. #endif