--- trunk/src/math/Quaternion.hpp 2004/10/18 17:07:27 99 +++ trunk/src/math/Quaternion.hpp 2006/05/17 21:51:42 963 @@ -1,28 +1,44 @@ /* - * Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project - * - * Contact: oopse@oopse.org - * - * This program is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public License - * as published by the Free Software Foundation; either version 2.1 - * of the License, or (at your option) any later version. - * All we ask is that proper credit is given for our work, which includes - * - but is not limited to - adding the above copyright notice to the beginning - * of your source code files, and to any copyright notice that you may distribute - * with programs based on this work. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * 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 @@ -34,246 +50,311 @@ #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 - class Quaternion : public Vector { - public: - Quaternion(); + /** + * @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. RealType), and + * i*i = -1; j*j = -1; k*k = -1; + * i*j = k; j*k = i; k*i = j; + */ + template + class Quaternion : public Vector { + public: + Quaternion() : Vector() {} - /** Constructs and initializes a Quaternion from w, x, y, z values */ - Quaternion(Real w, Real x, Real y, Real z) { - data_[0] = w; - data_[1] = x; - data_[2] = y; - data_[3] = z; - } + /** 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; + } - /** - * - */ - Quaternion(const Vector& v) - : Vector(v){ - } + /** Constructs and initializes a Quaternion from a Vector */ + Quaternion(const Vector& v) + : Vector(v){ + } - /** */ - Quaternion& operator =(const Vector& v){ - if (this == & v) - return *this; + /** copy assignment */ + Quaternion& operator =(const Vector& v){ + if (this == & v) + return *this; - Vector::operator=(v); + Vector::operator=(v); - return *this; - } + 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 data_[0]; - } + /** + * 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 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 data_[1]; - } + /** + * 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 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 data_[2]; - } + /** + * 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 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 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 data_[3]; - } + /** + * 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]; + } - /** - * 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 inverse(){ - Quaternion q; - Real d = this->lengthSquared(); + /** + * 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& q) { + + for (unsigned int i = 0; i < 4; i ++) { + if (!equal(this->data_[i], q[i])) { + return false; + } + } - q.w() = w() / d; - q.x() = -x() / d; - q.y() = -y() / d; - q.z() = -z() / d; + 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 inverse() { + Quaternion q; + Real d = this->lengthSquare(); - return q; - } + 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& q) { + /** + * Sets the value to the multiplication of itself and another quaternion + * @param q the other quaternion + */ + void mul(const Quaternion& q) { + Quaternion tmp(*this); - Real a0( (z() - y()) * (q.y() - q.z()) ); - Real a1( (w() + x()) * (q.w() + q.x()) ); - Real a2( (w() - x()) * (q.y() + q.z()) ); - Real a3( (y() + z()) * (q.w() - q.x()) ); - Real b0( -(x() - z()) * (q.x() - q.y()) ); - Real b1( -(x() + z()) * (q.x() + q.y()) ); - Real b2( (w() + y()) * (q.w() - q.z()) ); - Real b3( (w() - y()) * (q.w() + q.z()) ); + 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]); + } - data_[0] = a0 + 0.5*(b0 + b1 + b2 + b3),; - data_[1] = a1 + 0.5*(b0 + b1 - b2 - b3); - data_[2] = a2 + 0.5*(b0 - b1 + b2 - b3), - data_[3] = a3 + 0.5*(b0 - b1 - b2 + b3) ); - } + 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& q) { + mul(q.inverse()); + } - /** Set the value of this quaternion to the division of itself by another quaternion */ - void div(const Quaternion& 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& operator *=(const Quaternion& q) { - mul(q); - return *this; - } - - Quaternion& operator /=(const Quaternion& q) { - mul(q.inverse()); - return *this; - } + Quaternion& operator *=(const Quaternion& q) { + mul(q); + return *this; + } + + Quaternion& operator *=(const Real& s) { + mul(s); + return *this; + } - /** - * Returns the conjugate quaternion of this quaternion - * @return the conjugate quaternion of this quaternion - */ - Quaternion conjugate() { - return Quaternion(w(), -x(), -y(), -z()); - } + Quaternion& operator /=(Quaternion& q) { + *this *= q.inverse(); + return *this; + } - /** - * Returns the corresponding rotation matrix (3x3) - * @return a 3x3 rotation matrix - */ - SquareMatrix toRotationMatrix3() { - SquareMatrix rotMat3; + Quaternion& operator /=(const Real& s) { + div(s); + return *this; + } + /** + * Returns the conjugate quaternion of this quaternion + * @return the conjugate quaternion of this quaternion + */ + Quaternion conjugate() { + return Quaternion(w(), -x(), -y(), -z()); + } - Real w2; - Real x2; - Real y2; - Real z2; + /** + * Returns the corresponding rotation matrix (3x3) + * @return a 3x3 rotation matrix + */ + SquareMatrix toRotationMatrix3() { + SquareMatrix rotMat3; - if (!isNormalized()) - normalize(); + 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(); + 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(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(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; - } + 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; - };//end Quaternion - - /** - * Returns the multiplication of two quaternion - * @return the multiplication of two quaternion - * @param q1 the first quaternion - * @param q2 the second quaternion - */ - template - inline Quaternion operator *(const Quaternion& q1, const Quaternion& q2) { - Quaternion result(q1); - result *= q2; - return result; + return rotMat3; } - /** - * Returns the division of two quaternion - * @param q1 divisor - * @param q2 dividen - */ + };//end Quaternion - template - inline Quaternion operator /(const Quaternion& q1, const Quaternion& 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() + * 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 - Quaternion operator /(const Real& s, const Quaternion& q) { + template + Quaternion operator * ( const Quaternion& q, Real s) { + Quaternion 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 + Quaternion operator * ( const Real& s, const Quaternion& q ) { + Quaternion result(q); + result.mul(s); + return result; + } - Quaternion x = q.inv(); - return x * s; - } + /** + * Returns the multiplication of two quaternion + * @return the multiplication of two quaternion + * @param q1 the first quaternion + * @param q2 the second quaternion + */ + template + inline Quaternion operator *(const Quaternion& q1, const Quaternion& q2) { + Quaternion result(q1); + result *= q2; + return result; + } - typedef Quaternion Quat4d; + /** + * Returns the division of two quaternion + * @param q1 divisor + * @param q2 dividen + */ + + template + inline Quaternion operator /( Quaternion& q1, Quaternion& 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 + Quaternion operator /(const Real& s, Quaternion& q) { + + Quaternion x; + x = q.inverse(); + x *= s; + return x; + } + + template + inline bool operator==(const Quaternion& lhs, const Quaternion& rhs) { + return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]); + } + + typedef Quaternion Quat4d; } #endif //MATH_QUATERNION_HPP