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

Comparing trunk/OOPSE-4/src/math/Quaternion.hpp (file contents):
Revision 2203 by tim, Tue Mar 1 20:10:14 2005 UTC vs.
Revision 2204 by gezelter, Fri Apr 15 22:04:00 2005 UTC

# Line 1 | Line 1
1 < /*
1 > /*
2   * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3   *
4   * The University of Notre Dame grants you ("Licensee") a
# Line 54 | Line 54 | namespace oopse{
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() : Vector<Real, 4>() {}
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 <                this->data_[0] = w;
73 <                this->data_[1] = x;
74 <                this->data_[2] = y;
75 <                this->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 <            /** Constructs and initializes a Quaternion from a  Vector<Real,4> */    
79 <            Quaternion(const Vector<Real,4>& v)
80 <                : Vector<Real, 4>(v){
81 <            }
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 <            /** copy assignment */
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 this->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 this->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 this->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 this->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 this->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 this->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 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 <            }
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 <             * 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) {
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 <                }
163 >      for (unsigned int i = 0; i < 4; i ++) {
164 >        if (!equal(this->data_[i], q[i])) {
165 >          return false;
166 >        }
167 >      }
168                  
169 <                return true;
170 <            }
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();
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 <                q.w() = w() / d;
183 <                q.x() = -x() / d;
184 <                q.y() = -y() / d;
185 <                q.z() = -z() / d;
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 <            }
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) {
195 <                Quaternion<Real> tmp(*this);
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 <                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 <            }
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 <            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 <            }
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 <            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 <            }
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 >    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 <            }
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 <            }
227 >    Quaternion<Real>& operator *=(const Real& s) {
228 >      mul(s);
229 >      return *this;
230 >    }
231              
232 <            Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
233 <                *this *= q.inverse();
234 <                return *this;
235 <            }
232 >    Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
233 >      *this *= q.inverse();
234 >      return *this;
235 >    }
236  
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 <            }
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 <            /**
250 <             * Returns the corresponding rotation matrix (3x3)
251 <             * @return a 3x3 rotation matrix
252 <             */
253 <            SquareMatrix<Real, 3> toRotationMatrix3() {
254 <                SquareMatrix<Real, 3> rotMat3;
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 <                Real w2;
257 <                Real x2;
258 <                Real y2;
259 <                Real z2;
256 >      Real w2;
257 >      Real x2;
258 >      Real y2;
259 >      Real z2;
260  
261 <                if (!this->isNormalized())
262 <                    this->normalize();
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;
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 <                return rotMat3;
282 <            }
281 >      return rotMat3;
282 >    }
283  
284 <    };//end Quaternion
284 >  };//end Quaternion
285  
286  
287      /**
# Line 290 | Line 290 | namespace oopse{
290       * @param q the source quaternion
291       * @param s the scalar value
292       */
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 <    }
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 <    }    
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 <    /**
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 <    }
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 <    /**
327 <     * Returns the division of two quaternion
328 <     * @param q1 divisor
329 <     * @param q2 dividen
330 <     */
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 <    }
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) {
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 <    }
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 <    }
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;
358 >  typedef Quaternion<double> Quat4d;
359   }
360   #endif //MATH_QUATERNION_HPP

Diff Legend

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