src/math/Quaternion.hpp

/*
 * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
 *
 * The University of Notre Dame grants you ("Licensee") a
 * non-exclusive, royalty free, license to use, modify and
 * redistribute this software in source and binary code form, provided
 * that the following conditions are met:
 *
 * 1. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. All express or implied conditions, representations and
 * warranties, including any implied warranty of merchantability,
 * fitness for a particular purpose or non-infringement, are hereby
 * excluded.  The University of Notre Dame and its licensors shall not
 * be liable for any damages suffered by licensee as a result of
 * using, modifying or distributing the software or its
 * derivatives. In no event will the University of Notre Dame or its
 * licensors be liable for any lost revenue, profit or data, or for
 * direct, indirect, special, consequential, incidental or punitive
 * damages, however caused and regardless of the theory of liability,
 * arising out of the use of or inability to use software, even if the
 * University of Notre Dame has been advised of the possibility of
 * such damages.
 */
 
/**
 * @file Quaternion.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */

#ifndef MATH_QUATERNION_HPP
#define MATH_QUATERNION_HPP

#include "math/Vector.hpp"
#include "math/SquareMatrix.hpp"

namespace oopse{

  /**
   * @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
   * Quaternion is a sort of a higher-level complex number.
   * It is defined as Q = w + x*i + y*j + z*k,
   * where w, x, y, and z are numbers of type T (e.g. double), and
   * i*i = -1; j*j = -1; k*k = -1;
   * i*j = k; j*k = i; k*i = j;
   */
  template<typename Real>
  class Quaternion : public Vector<Real, 4> {
  public:
    Quaternion() : Vector<Real, 4>() {}

    /** Constructs and initializes a Quaternion from w, x, y, z values */     
    Quaternion(Real w, Real x, Real y, Real z) {
      this->data_[0] = w;
      this->data_[1] = x;
      this->data_[2] = y;
      this->data_[3] = z;                
    }
            
    /** Constructs and initializes a Quaternion from a  Vector<Real,4> */     
    Quaternion(const Vector<Real,4>& v) 
      : Vector<Real, 4>(v){
      }

    /** copy assignment */
    Quaternion& operator =(const Vector<Real, 4>& v){
      if (this == & v)
        return *this;

      Vector<Real, 4>::operator=(v);
                
      return *this;
    }

    /**
     * Returns the value of the first element of this quaternion.
     * @return the value of the first element of this quaternion
     */
    Real w() const {
      return this->data_[0];
    }

    /**
     * Returns the reference of the first element of this quaternion.
     * @return the reference of the first element of this quaternion
     */
    Real& w() {
      return this->data_[0];    
    }

    /**
     * Returns the value of the first element of this quaternion.
     * @return the value of the first element of this quaternion
     */
    Real x() const {
      return this->data_[1];
    }

    /**
     * Returns the reference of the second element of this quaternion.
     * @return the reference of the second element of this quaternion
     */
    Real& x() {
      return this->data_[1];    
    }

    /**
     * Returns the value of the thirf element of this quaternion.
     * @return the value of the third element of this quaternion
     */
    Real y() const {
      return this->data_[2];
    }

    /**
     * Returns the reference of the third element of this quaternion.
     * @return the reference of the third element of this quaternion
     */           
    Real& y() {
      return this->data_[2];    
    }

    /**
     * Returns the value of the fourth element of this quaternion.
     * @return the value of the fourth element of this quaternion
     */
    Real z() const {
      return this->data_[3];
    }
    /**
     * Returns the reference of the fourth element of this quaternion.
     * @return the reference of the fourth element of this quaternion
     */
    Real& z() {
      return this->data_[3];    
    }

    /**
     * Tests if this quaternion is equal to other quaternion
     * @return true if equal, otherwise return false
     * @param q quaternion to be compared
     */
    inline bool operator ==(const Quaternion<Real>& q) {

      for (unsigned int i = 0; i < 4; i ++) {
        if (!equal(this->data_[i], q[i])) {
          return false;
        }
      }
                
      return true;
    }
            
    /**
     * Returns the inverse of this quaternion
     * @return inverse
     * @note since quaternion is a complex number, the inverse of quaternion
     * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
     */
    Quaternion<Real> inverse() {
      Quaternion<Real> q;
      Real d = this->lengthSquare();
                
      q.w() = w() / d;
      q.x() = -x() / d;
      q.y() = -y() / d;
      q.z() = -z() / d;
                
      return q;
    }

    /**
     * Sets the value to the multiplication of itself and another quaternion
     * @param q the other quaternion
     */
    void mul(const Quaternion<Real>& q) {
      Quaternion<Real> tmp(*this);

      this->data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
      this->data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
      this->data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
      this->data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);                
    }

    void mul(const Real& s) {
      this->data_[0] *= s;
      this->data_[1] *= s;
      this->data_[2] *= s;
      this->data_[3] *= s;
    }

    /** Set the value of this quaternion to the division of itself by another quaternion */
    void div(Quaternion<Real>& q) {
      mul(q.inverse());
    }

    void div(const Real& s) {
      this->data_[0] /= s;
      this->data_[1] /= s;
      this->data_[2] /= s;
      this->data_[3] /= s;
    }
            
    Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
      mul(q);
      return *this;
    }

    Quaternion<Real>& operator *=(const Real& s) {
      mul(s);
      return *this;
    }
            
    Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
      *this *= q.inverse();
      return *this;
    }

    Quaternion<Real>& operator /=(const Real& s) {
      div(s);
      return *this;
    }            
    /**
     * Returns the conjugate quaternion of this quaternion
     * @return the conjugate quaternion of this quaternion
     */
    Quaternion<Real> conjugate() {
      return Quaternion<Real>(w(), -x(), -y(), -z());            
    }

    /**
     * Returns the corresponding rotation matrix (3x3)
     * @return a 3x3 rotation matrix
     */
    SquareMatrix<Real, 3> toRotationMatrix3() {
      SquareMatrix<Real, 3> rotMat3;

      Real w2;
      Real x2;
      Real y2;
      Real z2;

      if (!this->isNormalized())
        this->normalize();
                
      w2 = w() * w();
      x2 = x() * x();
      y2 = y() * y();
      z2 = z() * z();

      rotMat3(0, 0) = w2 + x2 - y2 - z2;
      rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
      rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );

      rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
      rotMat3(1, 1) = w2 - x2 + y2 - z2;
      rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );

      rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
      rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
      rotMat3(2, 2) = w2 - x2 -y2 +z2;

      return rotMat3;
    }

  };//end Quaternion


    /**
     * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
     * @return  the vaule of scalar multiplication of this vector
     * @param q the source quaternion
     * @param s the scalar value
     */
  template<typename Real, unsigned int Dim>                 
  Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {       
    Quaternion<Real> result(q);
    result.mul(s);
    return result;           
  }
    
  /**
   * Returns the vaule of scalar multiplication of this quaterion q (q * s). 
   * @return  the vaule of scalar multiplication of this vector
   * @param s the scalar value
   * @param q the source quaternion
   */  
  template<typename Real, unsigned int Dim>
  Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
    Quaternion<Real> result(q);
    result.mul(s);
    return result;           
  }    

  /**
   * Returns the multiplication of two quaternion
   * @return the multiplication of two quaternion
   * @param q1 the first quaternion
   * @param q2 the second quaternion
   */
  template<typename Real>
  inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
    Quaternion<Real> result(q1);
    result *= q2;
    return result;
  }

  /**
   * Returns the division of two quaternion
   * @param q1 divisor
   * @param q2 dividen
   */

  template<typename Real>
  inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
    return q1 * q2.inverse();
  }

  /**
   * Returns the value of the division of a scalar by a quaternion
   * @return the value of the division of a scalar by a quaternion
   * @param s scalar
   * @param q quaternion
   * @note for a quaternion q, 1/q = q.inverse()
   */
  template<typename Real>
  Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {

    Quaternion<Real> x;
    x = q.inverse();
    x *= s;
    return x;
  }
    
  template <class T>
  inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
    return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
  }
    
  typedef Quaternion<double> Quat4d;
}
#endif //MATH_QUATERNION_HPP 
Revision:	507
Committed:	Fri Apr 15 22:04:00 2005 UTC (20 years, 6 months ago) by gezelter
File size:	10773 byte(s)
Log Message:	xemacs has been drafted to perform our indentation services
#	User	Rev	Content
1	gezelter	507	/*
2	gezelter	246	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	92	*
4	gezelter	246	* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40	tim	92	*/
41	gezelter	246
42	tim	92	/**
43			* @file Quaternion.hpp
44			* @author Teng Lin
45			* @date 10/11/2004
46			* @version 1.0
47			*/
48
49			#ifndef MATH_QUATERNION_HPP
50			#define MATH_QUATERNION_HPP
51
52	tim	93	#include "math/Vector.hpp"
53	tim	110	#include "math/SquareMatrix.hpp"
54	tim	93
55	tim	92	namespace oopse{
56
57	gezelter	507	/**
58			* @class Quaternion Quaternion.hpp "math/Quaternion.hpp"
59			* Quaternion is a sort of a higher-level complex number.
60			* It is defined as Q = w + xi + yj + z*k,
61			* where w, x, y, and z are numbers of type T (e.g. double), and
62			* ii = -1; jj = -1; k*k = -1;
63			* ij = k; jk = i; k*i = j;
64			*/
65			template<typename Real>
66			class Quaternion : public Vector<Real, 4> {
67			public:
68			Quaternion() : Vector<Real, 4>() {}
69	tim	92
70	gezelter	507	/** Constructs and initializes a Quaternion from w, x, y, z values */
71			Quaternion(Real w, Real x, Real y, Real z) {
72			this->data_[0] = w;
73			this->data_[1] = x;
74			this->data_[2] = y;
75			this->data_[3] = z;
76			}
77	tim	93
78	gezelter	507	/** Constructs and initializes a Quaternion from a Vector<Real,4> */
79			Quaternion(const Vector<Real,4>& v)
80			: Vector<Real, 4>(v){
81			}
82	tim	92
83	gezelter	507	/** copy assignment */
84			Quaternion& operator =(const Vector<Real, 4>& v){
85			if (this == & v)
86			return *this;
87	tim	93
88	gezelter	507	Vector<Real, 4>::operator=(v);
89	tim	93
90	gezelter	507	return *this;
91			}
92	tim	93
93	gezelter	507	/**
94			* Returns the value of the first element of this quaternion.
95			* @return the value of the first element of this quaternion
96			*/
97			Real w() const {
98			return this->data_[0];
99			}
100	tim	93
101	gezelter	507	/**
102			* Returns the reference of the first element of this quaternion.
103			* @return the reference of the first element of this quaternion
104			*/
105			Real& w() {
106			return this->data_[0];
107			}
108	tim	93
109	gezelter	507	/**
110			* Returns the value of the first element of this quaternion.
111			* @return the value of the first element of this quaternion
112			*/
113			Real x() const {
114			return this->data_[1];
115			}
116	tim	93
117	gezelter	507	/**
118			* Returns the reference of the second element of this quaternion.
119			* @return the reference of the second element of this quaternion
120			*/
121			Real& x() {
122			return this->data_[1];
123			}
124	tim	93
125	gezelter	507	/**
126			* Returns the value of the thirf element of this quaternion.
127			* @return the value of the third element of this quaternion
128			*/
129			Real y() const {
130			return this->data_[2];
131			}
132	tim	93
133	gezelter	507	/**
134			* Returns the reference of the third element of this quaternion.
135			* @return the reference of the third element of this quaternion
136			*/
137			Real& y() {
138			return this->data_[2];
139			}
140	tim	93
141	gezelter	507	/**
142			* Returns the value of the fourth element of this quaternion.
143			* @return the value of the fourth element of this quaternion
144			*/
145			Real z() const {
146			return this->data_[3];
147			}
148			/**
149			* Returns the reference of the fourth element of this quaternion.
150			* @return the reference of the fourth element of this quaternion
151			*/
152			Real& z() {
153			return this->data_[3];
154			}
155	tim	93
156	gezelter	507	/**
157			* Tests if this quaternion is equal to other quaternion
158			* @return true if equal, otherwise return false
159			* @param q quaternion to be compared
160			*/
161			inline bool operator ==(const Quaternion<Real>& q) {
162	tim	110
163	gezelter	507	for (unsigned int i = 0; i < 4; i ++) {
164			if (!equal(this->data_[i], q[i])) {
165			return false;
166			}
167			}
168	tim	110
169	gezelter	507	return true;
170			}
171	tim	110
172	gezelter	507	/**
173			* Returns the inverse of this quaternion
174			* @return inverse
175			* @note since quaternion is a complex number, the inverse of quaternion
176			* q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(\|q\|^2)
177			*/
178			Quaternion<Real> inverse() {
179			Quaternion<Real> q;
180			Real d = this->lengthSquare();
181	tim	93
182	gezelter	507	q.w() = w() / d;
183			q.x() = -x() / d;
184			q.y() = -y() / d;
185			q.z() = -z() / d;
186	tim	93
187	gezelter	507	return q;
188			}
189	tim	93
190	gezelter	507	/**
191			* Sets the value to the multiplication of itself and another quaternion
192			* @param q the other quaternion
193			*/
194			void mul(const Quaternion<Real>& q) {
195			Quaternion<Real> tmp(*this);
196	tim	93
197	gezelter	507	this->data_[0] = (tmp[0]q[0]) -(tmp[1]q[1]) - (tmp[2]q[2]) - (tmp[3]q[3]);
198			this->data_[1] = (tmp[0]q[1]) + (tmp[1]q[0]) + (tmp[2]q[3]) - (tmp[3]q[2]);
199			this->data_[2] = (tmp[0]q[2]) + (tmp[2]q[0]) + (tmp[3]q[1]) - (tmp[1]q[3]);
200			this->data_[3] = (tmp[0]q[3]) + (tmp[3]q[0]) + (tmp[1]q[2]) - (tmp[2]q[1]);
201			}
202	tim	93
203	gezelter	507	void mul(const Real& s) {
204			this->data_[0] *= s;
205			this->data_[1] *= s;
206			this->data_[2] *= s;
207			this->data_[3] *= s;
208			}
209	tim	93
210	gezelter	507	/** Set the value of this quaternion to the division of itself by another quaternion */
211			void div(Quaternion<Real>& q) {
212			mul(q.inverse());
213			}
214	tim	110
215	gezelter	507	void div(const Real& s) {
216			this->data_[0] /= s;
217			this->data_[1] /= s;
218			this->data_[2] /= s;
219			this->data_[3] /= s;
220			}
221	tim	93
222	gezelter	507	Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
223			mul(q);
224			return *this;
225			}
226	tim	110
227	gezelter	507	Quaternion<Real>& operator *=(const Real& s) {
228			mul(s);
229			return *this;
230			}
231	tim	93
232	gezelter	507	Quaternion<Real>& operator /=(Quaternion<Real>& q) {
233			this = q.inverse();
234			return *this;
235			}
236	tim	110
237	gezelter	507	Quaternion<Real>& operator /=(const Real& s) {
238			div(s);
239			return *this;
240			}
241			/**
242			* Returns the conjugate quaternion of this quaternion
243			* @return the conjugate quaternion of this quaternion
244			*/
245			Quaternion<Real> conjugate() {
246			return Quaternion<Real>(w(), -x(), -y(), -z());
247			}
248	tim	93
249	gezelter	507	/**
250			* Returns the corresponding rotation matrix (3x3)
251			* @return a 3x3 rotation matrix
252			*/
253			SquareMatrix<Real, 3> toRotationMatrix3() {
254			SquareMatrix<Real, 3> rotMat3;
255	tim	93
256	gezelter	507	Real w2;
257			Real x2;
258			Real y2;
259			Real z2;
260	tim	93
261	gezelter	507	if (!this->isNormalized())
262			this->normalize();
263	tim	93
264	gezelter	507	w2 = w() * w();
265			x2 = x() * x();
266			y2 = y() * y();
267			z2 = z() * z();
268	tim	93
269	gezelter	507	rotMat3(0, 0) = w2 + x2 - y2 - z2;
270			rotMat3(0, 1) = 2.0 * ( x() * y() + w() * z() );
271			rotMat3(0, 2) = 2.0 * ( x() * z() - w() * y() );
272	tim	93
273	gezelter	507	rotMat3(1, 0) = 2.0 * ( x() * y() - w() * z() );
274			rotMat3(1, 1) = w2 - x2 + y2 - z2;
275			rotMat3(1, 2) = 2.0 * ( y() * z() + w() * x() );
276	tim	93
277	gezelter	507	rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
278			rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
279			rotMat3(2, 2) = w2 - x2 -y2 +z2;
280	tim	110
281	gezelter	507	return rotMat3;
282			}
283	tim	93
284	gezelter	507	};//end Quaternion
285	tim	93
286	tim	110
287	tim	93	/**
288	tim	110	* Returns the vaule of scalar multiplication of this quaterion q (q * s).
289			* @return the vaule of scalar multiplication of this vector
290			* @param q the source quaternion
291			* @param s the scalar value
292			*/
293	gezelter	507	template<typename Real, unsigned int Dim>
294			Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {
295			Quaternion<Real> result(q);
296			result.mul(s);
297			return result;
298			}
299	tim	110
300	gezelter	507	/**
301			* Returns the vaule of scalar multiplication of this quaterion q (q * s).
302			* @return the vaule of scalar multiplication of this vector
303			* @param s the scalar value
304			* @param q the source quaternion
305			*/
306			template<typename Real, unsigned int Dim>
307			Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
308			Quaternion<Real> result(q);
309			result.mul(s);
310			return result;
311			}
312	tim	110
313	gezelter	507	/**
314			* Returns the multiplication of two quaternion
315			* @return the multiplication of two quaternion
316			* @param q1 the first quaternion
317			* @param q2 the second quaternion
318			*/
319			template<typename Real>
320			inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
321			Quaternion<Real> result(q1);
322			result *= q2;
323			return result;
324			}
325	tim	93
326	gezelter	507	/**
327			* Returns the division of two quaternion
328			* @param q1 divisor
329			* @param q2 dividen
330			*/
331	tim	93
332	gezelter	507	template<typename Real>
333			inline Quaternion<Real> operator /( Quaternion<Real>& q1, Quaternion<Real>& q2) {
334			return q1 * q2.inverse();
335			}
336	tim	93
337	gezelter	507	/**
338			* Returns the value of the division of a scalar by a quaternion
339			* @return the value of the division of a scalar by a quaternion
340			* @param s scalar
341			* @param q quaternion
342			* @note for a quaternion q, 1/q = q.inverse()
343			*/
344			template<typename Real>
345			Quaternion<Real> operator /(const Real& s, Quaternion<Real>& q) {
346	tim	93
347	gezelter	507	Quaternion<Real> x;
348			x = q.inverse();
349			x *= s;
350			return x;
351			}
352	tim	110
353	gezelter	507	template <class T>
354			inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
355			return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
356			}
357	tim	110
358	gezelter	507	typedef Quaternion<double> Quat4d;
359	tim	92	}
360			#endif //MATH_QUATERNION_HPP