--- trunk/src/math/Quaternion.hpp 2004/10/18 17:07:27 99 +++ trunk/src/math/Quaternion.hpp 2005/03/01 20:10:14 385 @@ -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,6 +50,7 @@ #define MATH_QUATERNION_HPP #include "math/Vector.hpp" +#include "math/SquareMatrix.hpp" namespace oopse{ @@ -48,24 +65,22 @@ namespace oopse{ template class Quaternion : public Vector { public: - Quaternion(); + 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; + this->data_[0] = w; + this->data_[1] = x; + this->data_[2] = y; + this->data_[3] = z; } - /** - * - */ + /** Constructs and initializes a Quaternion from a Vector */ Quaternion(const Vector& v) : Vector(v){ } - /** */ + /** copy assignment */ Quaternion& operator =(const Vector& v){ if (this == & v) return *this; @@ -80,7 +95,7 @@ namespace oopse{ * @return the value of the first element of this quaternion */ Real w() const { - return data_[0]; + return this->data_[0]; } /** @@ -88,7 +103,7 @@ namespace oopse{ * @return the reference of the first element of this quaternion */ Real& w() { - return data_[0]; + return this->data_[0]; } /** @@ -96,7 +111,7 @@ namespace oopse{ * @return the value of the first element of this quaternion */ Real x() const { - return data_[1]; + return this->data_[1]; } /** @@ -104,7 +119,7 @@ namespace oopse{ * @return the reference of the second element of this quaternion */ Real& x() { - return data_[1]; + return this->data_[1]; } /** @@ -112,7 +127,7 @@ namespace oopse{ * @return the value of the third element of this quaternion */ Real y() const { - return data_[2]; + return this->data_[2]; } /** @@ -120,7 +135,7 @@ namespace oopse{ * @return the reference of the third element of this quaternion */ Real& y() { - return data_[2]; + return this->data_[2]; } /** @@ -128,25 +143,41 @@ namespace oopse{ * @return the value of the fourth element of this quaternion */ Real z() const { - return data_[3]; + 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 data_[3]; + 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& 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 inverse(){ + Quaternion inverse() { Quaternion q; - Real d = this->lengthSquared(); + Real d = this->lengthSquare(); q.w() = w() / d; q.x() = -x() / d; @@ -161,38 +192,52 @@ namespace oopse{ * @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(const Quaternion& q) { + void div(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()); + + Quaternion& operator *=(const Real& s) { + mul(s); return *this; } + Quaternion& operator /=(Quaternion& q) { + *this *= q.inverse(); + return *this; + } + + Quaternion& operator /=(const Real& s) { + div(s); + return *this; + } /** * Returns the conjugate quaternion of this quaternion * @return the conjugate quaternion of this quaternion @@ -213,8 +258,8 @@ namespace oopse{ Real y2; Real z2; - if (!isNormalized()) - normalize(); + if (!this->isNormalized()) + this->normalize(); w2 = w() * w(); x2 = x() * x(); @@ -232,11 +277,40 @@ namespace oopse{ 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 + 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; + } + + /** * Returns the multiplication of two quaternion * @return the multiplication of two quaternion * @param q1 the first quaternion @@ -256,7 +330,7 @@ namespace oopse{ */ template - inline Quaternion operator /(const Quaternion& q1, const Quaternion& q2) { + inline Quaternion operator /( Quaternion& q1, Quaternion& q2) { return q1 * q2.inverse(); } @@ -268,12 +342,19 @@ namespace oopse{ * @note for a quaternion q, 1/q = q.inverse() */ template - Quaternion operator /(const Real& s, const Quaternion& q) { + Quaternion operator /(const Real& s, Quaternion& q) { - Quaternion x = q.inv(); - return x * s; + 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