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 1603 by tim, Tue Oct 19 21:28:55 2004 UTC

# Line 34 | Line 34
34   #define MATH_QUATERNION_HPP
35  
36   #include "math/Vector.hpp"
37 + #include "math/SquareMatrix.hpp"
38  
39   namespace oopse{
40  
# Line 48 | Line 49 | namespace oopse{
49      template<typename Real>
50      class Quaternion : public Vector<Real, 4> {
51          public:
52 <            Quaternion();
52 >            Quaternion() : Vector<Real, 4>() {}
53  
54              /** Constructs and initializes a Quaternion from w, x, y, z values */    
55              Quaternion(Real w, Real x, Real y, Real z) {
# Line 58 | Line 59 | namespace oopse{
59                  data_[3] = z;                
60              }
61              
62 <            /**
62 <             *  
63 <             */
62 >            /** Constructs and initializes a Quaternion from a  Vector<Real,4> */    
63              Quaternion(const Vector<Real,4>& v)
64                  : Vector<Real, 4>(v){
65              }
66  
67 <            /** */
67 >            /** copy assignment */
68              Quaternion& operator =(const Vector<Real, 4>& v){
69                  if (this == & v)
70                      return *this;
# Line 139 | Line 138 | namespace oopse{
138              }
139  
140              /**
141 +             * Tests if this quaternion is equal to other quaternion
142 +             * @return true if equal, otherwise return false
143 +             * @param q quaternion to be compared
144 +             */
145 +             inline bool operator ==(const Quaternion<Real>& q) {
146 +
147 +                for (unsigned int i = 0; i < 4; i ++) {
148 +                    if (!equal(data_[i], q[i])) {
149 +                        return false;
150 +                    }
151 +                }
152 +                
153 +                return true;
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(){
162 >            Quaternion<Real> inverse() {
163                  Quaternion<Real> q;
164 <                Real d = this->lengthSquared();
164 >                Real d = this->lengthSquare();
165                  
166                  q.w() = w() / d;
167                  q.x() = -x() / d;
# Line 161 | Line 176 | namespace oopse{
176               * @param q the other quaternion
177               */
178              void mul(const Quaternion<Real>& q) {
179 +                Quaternion<Real> tmp(*this);
180  
181 <                Real a0( (z() - y()) * (q.y() - q.z()) );
182 <                Real a1( (w() + x()) * (q.w() + q.x()) );
183 <                Real a2( (w() - x()) * (q.y() + q.z()) );
184 <                Real a3( (y() + z()) * (q.w() - q.x()) );
185 <                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()) );
181 >                data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
182 >                data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
183 >                data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
184 >                data_[3] = (tmp[0]*q[3]) + (tmp[3]*q[0]) + (tmp[1]*q[2]) - (tmp[2]*q[1]);                
185 >            }
186  
187 <                data_[0] = a0 + 0.5*(b0 + b1 + b2 + b3),;
188 <                data_[1] = a1 + 0.5*(b0 + b1 - b2 - b3);
189 <                data_[2] = a2 + 0.5*(b0 - b1 + b2 - b3),
190 <                data_[3] = a3 + 0.5*(b0 - b1 - b2 + b3) );
187 >            void mul(const Real& s) {
188 >                data_[0] *= s;
189 >                data_[1] *= s;
190 >                data_[2] *= s;
191 >                data_[3] *= s;
192              }
193  
180 –
194              /** Set the value of this quaternion to the division of itself by another quaternion */
195 <            void div(const Quaternion<Real>& q) {
195 >            void div(Quaternion<Real>& q) {
196                  mul(q.inverse());
197              }
198 +
199 +            void div(const Real& s) {
200 +                data_[0] /= s;
201 +                data_[1] /= s;
202 +                data_[2] /= s;
203 +                data_[3] /= s;
204 +            }
205              
206              Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
207                  mul(q);
208                  return *this;
209              }
210 <                        
211 <            Quaternion<Real>& operator /=(const Quaternion<Real>& q) {
212 <                mul(q.inverse());
210 >
211 >            Quaternion<Real>& operator *=(const Real& s) {
212 >                mul(s);
213                  return *this;
214              }
215              
216 +            Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
217 +                *this *= q.inverse();
218 +                return *this;
219 +            }
220 +
221 +            Quaternion<Real>& operator /=(const Real& s) {
222 +                div(s);
223 +                return *this;
224 +            }            
225              /**
226               * Returns the conjugate quaternion of this quaternion
227               * @return the conjugate quaternion of this quaternion
# Line 232 | Line 261 | namespace oopse{
261                  rotMat3(2, 0) = 2.0 * ( x() * z() + w() * y() );
262                  rotMat3(2, 1) = 2.0 * ( y() * z() - w() * x() );
263                  rotMat3(2, 2) = w2 - x2 -y2 +z2;
264 +
265 +                return rotMat3;
266              }
267  
268      };//end Quaternion
269  
270 +
271      /**
272 +     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
273 +     * @return  the vaule of scalar multiplication of this vector
274 +     * @param q the source quaternion
275 +     * @param s the scalar value
276 +     */
277 +    template<typename Real, unsigned int Dim>                
278 +    Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {      
279 +        Quaternion<Real> result(q);
280 +        result.mul(s);
281 +        return result;          
282 +    }
283 +    
284 +    /**
285 +     * Returns the vaule of scalar multiplication of this quaterion q (q * s).
286 +     * @return  the vaule of scalar multiplication of this vector
287 +     * @param s the scalar value
288 +     * @param q the source quaternion
289 +     */  
290 +    template<typename Real, unsigned int Dim>
291 +    Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
292 +        Quaternion<Real> result(q);
293 +        result.mul(s);
294 +        return result;          
295 +    }    
296 +
297 +    /**
298       * Returns the multiplication of two quaternion
299       * @return the multiplication of two quaternion
300       * @param q1 the first quaternion
# Line 256 | Line 314 | namespace oopse{
314       */
315  
316      template<typename Real>
317 <    inline Quaternion<Real> operator /(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
317 >    inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
318          return q1 * q2.inverse();
319      }
320  
# Line 268 | Line 326 | namespace oopse{
326       * @note for a quaternion q, 1/q = q.inverse()
327       */
328      template<typename Real>
329 <    Quaternion<Real> operator /(const Real& s, const Quaternion<Real>& q) {
329 >    Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
330  
331 <        Quaternion<Real> x = q.inv();
332 <        return x * s;
331 >        Quaternion<Real> x;
332 >        x = q.inverse();
333 >        x *= s;
334 >        return x;
335      }
336 <
336 >    
337 >    template <class T>
338 >    inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
339 >        return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
340 >    }
341 >    
342      typedef Quaternion<double> Quat4d;
343   }
344   #endif //MATH_QUATERNION_HPP

Diff Legend

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