ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE-2.0/src/math/Quaternion.hpp
(Generate patch)

Comparing trunk/OOPSE-2.0/src/math/Quaternion.hpp (file contents):
Revision 1592 by tim, Mon Oct 18 17:07:27 2004 UTC vs.
Revision 2204 by gezelter, Fri Apr 15 22:04:00 2005 UTC

# Line 1 | Line 1
1   /*
2 < * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
3 < *
4 < * Contact: oopse@oopse.org
5 < *
6 < * This program is free software; you can redistribute it and/or
7 < * modify it under the terms of the GNU Lesser General Public License
8 < * as published by the Free Software Foundation; either version 2.1
9 < * of the License, or (at your option) any later version.
10 < * All we ask is that proper credit is given for our work, which includes
11 < * - but is not limited to - adding the above copyright notice to the beginning
12 < * of your source code files, and to any copyright notice that you may distribute
13 < * with programs based on this work.
14 < *
15 < * This program is distributed in the hope that it will be useful,
16 < * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 < * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18 < * GNU Lesser General Public License for more details.
19 < *
20 < * You should have received a copy of the GNU Lesser General Public License
21 < * along with this program; if not, write to the Free Software
22 < * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
2 > * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3   *
4 + * 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   */
41 <
41 >
42   /**
43   * @file Quaternion.hpp
44   * @author Teng Lin
# Line 34 | Line 50
50   #define MATH_QUATERNION_HPP
51  
52   #include "math/Vector.hpp"
53 + #include "math/SquareMatrix.hpp"
54  
55   namespace oopse{
56  
57 <    /**
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 + x*i + y*j + z*k,
61 <     * where w, x, y, and z are numbers of type T (e.g. double), and
62 <     * i*i = -1; j*j = -1; k*k = -1;
63 <     * i*j = k; j*k = i; k*i = j;
64 <     */
65 <    template<typename Real>
66 <    class Quaternion : public Vector<Real, 4> {
67 <        public:
68 <            Quaternion();
57 >  /**
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 + x*i + y*j + z*k,
61 >   * where w, x, y, and z are numbers of type T (e.g. double), and
62 >   * i*i = -1; j*j = -1; k*k = -1;
63 >   * i*j = k; j*k = i; k*i = j;
64 >   */
65 >  template<typename Real>
66 >  class Quaternion : public Vector<Real, 4> {
67 >  public:
68 >    Quaternion() : Vector<Real, 4>() {}
69  
70 <            /** Constructs and initializes a Quaternion from w, x, y, z values */    
71 <            Quaternion(Real w, Real x, Real y, Real z) {
72 <                data_[0] = w;
73 <                data_[1] = x;
74 <                data_[2] = y;
75 <                data_[3] = z;                
76 <            }
70 >    /** 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              
78 <            /**
79 <             *  
80 <             */
81 <            Quaternion(const Vector<Real,4>& v)
65 <                : Vector<Real, 4>(v){
66 <            }
78 >    /** Constructs and initializes a Quaternion from a  Vector<Real,4> */    
79 >    Quaternion(const Vector<Real,4>& v)
80 >      : Vector<Real, 4>(v){
81 >      }
82  
83 <            /** */
84 <            Quaternion& operator =(const Vector<Real, 4>& v){
85 <                if (this == & v)
86 <                    return *this;
83 >    /** copy assignment */
84 >    Quaternion& operator =(const Vector<Real, 4>& v){
85 >      if (this == & v)
86 >        return *this;
87  
88 <                Vector<Real, 4>::operator=(v);
88 >      Vector<Real, 4>::operator=(v);
89                  
90 <                return *this;
91 <            }
90 >      return *this;
91 >    }
92  
93 <            /**
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 data_[0];
99 <            }
93 >    /**
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  
101 <            /**
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 data_[0];    
107 <            }
101 >    /**
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  
109 <            /**
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 data_[1];
115 <            }
109 >    /**
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  
117 <            /**
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 data_[1];    
123 <            }
117 >    /**
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  
125 <            /**
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 data_[2];
131 <            }
125 >    /**
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  
133 <            /**
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 data_[2];    
139 <            }
133 >    /**
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  
141 <            /**
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 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 data_[3];    
154 <            }
141 >    /**
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  
156 <            /**
157 <             * Returns the inverse of this quaternion
158 <             * @return inverse
159 <             * @note since quaternion is a complex number, the inverse of quaternion
160 <             * q = w + xi + yj+ zk is inv_q = (w -xi - yj - zk)/(|q|^2)
161 <             */
162 <            Quaternion<Real> inverse(){
163 <                Quaternion<Real> q;
164 <                Real d = this->lengthSquared();
156 >    /**
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 >
163 >      for (unsigned int i = 0; i < 4; i ++) {
164 >        if (!equal(this->data_[i], q[i])) {
165 >          return false;
166 >        }
167 >      }
168                  
169 <                q.w() = w() / d;
170 <                q.x() = -x() / d;
171 <                q.y() = -y() / d;
172 <                q.z() = -z() / d;
169 >      return true;
170 >    }
171 >            
172 >    /**
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                  
182 <                return q;
183 <            }
182 >      q.w() = w() / d;
183 >      q.x() = -x() / d;
184 >      q.y() = -y() / d;
185 >      q.z() = -z() / d;
186 >                
187 >      return q;
188 >    }
189  
190 <            /**
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) {
190 >    /**
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  
197 <                Real a0( (z() - y()) * (q.y() - q.z()) );
198 <                Real a1( (w() + x()) * (q.w() + q.x()) );
199 <                Real a2( (w() - x()) * (q.y() + q.z()) );
200 <                Real a3( (y() + z()) * (q.w() - q.x()) );
201 <                Real b0( -(x() - z()) * (q.x() - q.y()) );
170 <                Real b1( -(x() + z()) * (q.x() + q.y()) );
171 <                Real b2( (w() + y()) * (q.w() - q.z()) );
172 <                Real b3( (w() - y()) * (q.w() + q.z()) );
197 >      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  
203 <                data_[0] = a0 + 0.5*(b0 + b1 + b2 + b3),;
204 <                data_[1] = a1 + 0.5*(b0 + b1 - b2 - b3);
205 <                data_[2] = a2 + 0.5*(b0 - b1 + b2 - b3),
206 <                data_[3] = a3 + 0.5*(b0 - b1 - b2 + b3) );
207 <            }
203 >    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  
210 +    /** 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  
215 <            /** Set the value of this quaternion to the division of itself by another quaternion */
216 <            void div(const Quaternion<Real>& q) {
217 <                mul(q.inverse());
218 <            }
215 >    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              
222 <            Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
223 <                mul(q);
224 <                return *this;
225 <            }
226 <                        
227 <            Quaternion<Real>& operator /=(const Quaternion<Real>& q) {
228 <                mul(q.inverse());
229 <                return *this;
230 <            }
222 >    Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
223 >      mul(q);
224 >      return *this;
225 >    }
226 >
227 >    Quaternion<Real>& operator *=(const Real& s) {
228 >      mul(s);
229 >      return *this;
230 >    }
231              
232 <            /**
233 <             * Returns the conjugate quaternion of this quaternion
234 <             * @return the conjugate quaternion of this quaternion
235 <             */
200 <            Quaternion<Real> conjugate() {
201 <                return Quaternion<Real>(w(), -x(), -y(), -z());            
202 <            }
232 >    Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
233 >      *this *= q.inverse();
234 >      return *this;
235 >    }
236  
237 <            /**
238 <             * Returns the corresponding rotation matrix (3x3)
239 <             * @return a 3x3 rotation matrix
240 <             */
241 <            SquareMatrix<Real, 3> toRotationMatrix3() {
242 <                SquareMatrix<Real, 3> rotMat3;
237 >    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  
249 <                Real w2;
250 <                Real x2;
251 <                Real y2;
252 <                Real z2;
249 >    /**
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  
256 <                if (!isNormalized())
257 <                    normalize();
256 >      Real w2;
257 >      Real x2;
258 >      Real y2;
259 >      Real z2;
260 >
261 >      if (!this->isNormalized())
262 >        this->normalize();
263                  
264 <                w2 = w() * w();
265 <                x2 = x() * x();
266 <                y2 = y() * y();
267 <                z2 = z() * z();
264 >      w2 = w() * w();
265 >      x2 = x() * x();
266 >      y2 = y() * y();
267 >      z2 = z() * z();
268  
269 <                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() );
269 >      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  
273 <                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() );
273 >      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  
277 <                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;
235 <            }
277 >      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  
281 <    };//end Quaternion
238 <
239 <    /**
240 <     * Returns the multiplication of two quaternion
241 <     * @return the multiplication of two quaternion
242 <     * @param q1 the first quaternion
243 <     * @param q2 the second quaternion
244 <     */
245 <    template<typename Real>
246 <    inline Quaternion<Real> operator *(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
247 <        Quaternion<Real> result(q1);
248 <        result *= q2;
249 <        return result;
281 >      return rotMat3;
282      }
283  
284 <    /**
253 <     * Returns the division of two quaternion
254 <     * @param q1 divisor
255 <     * @param q2 dividen
256 <     */
284 >  };//end Quaternion
285  
258 –    template<typename Real>
259 –    inline Quaternion<Real> operator /(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
260 –        return q1 * q2.inverse();
261 –    }
286  
287      /**
288 <     * Returns the value of the division of a scalar by a quaternion
289 <     * @return the value of the division of a scalar by a quaternion
290 <     * @param s scalar
291 <     * @param q quaternion
268 <     * @note for a quaternion q, 1/q = q.inverse()
288 >     * 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 <    template<typename Real>
294 <    Quaternion<Real> operator /(const Real& s, const Quaternion<Real>& q) {
293 >  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 >    
300 >  /**
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  
313 <        Quaternion<Real> x = q.inv();
314 <        return x * s;
315 <    }
313 >  /**
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  
326 <    typedef Quaternion<double> Quat4d;
326 >  /**
327 >   * Returns the division of two quaternion
328 >   * @param q1 divisor
329 >   * @param q2 dividen
330 >   */
331 >
332 >  template<typename Real>
333 >  inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
334 >    return q1 * q2.inverse();
335 >  }
336 >
337 >  /**
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 >
347 >    Quaternion<Real> x;
348 >    x = q.inverse();
349 >    x *= s;
350 >    return x;
351 >  }
352 >    
353 >  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 >    
358 >  typedef Quaternion<double> Quat4d;
359   }
360   #endif //MATH_QUATERNION_HPP

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines