--- trunk/src/math/Quaternion.hpp	2004/10/18 17:07:27	99
+++ trunk/src/math/Quaternion.hpp	2004/10/19 21:28:55	110
@@ -34,6 +34,7 @@
 #define MATH_QUATERNION_HPP
 
 #include "math/Vector.hpp"
+#include "math/SquareMatrix.hpp"
 
 namespace oopse{
 
@@ -48,7 +49,7 @@ namespace oopse{
     template<typename Real>
     class Quaternion : public Vector<Real, 4> {
         public:
-            Quaternion();
+            Quaternion() : Vector<Real, 4>() {}
 
             /** Constructs and initializes a Quaternion from w, x, y, z values */     
             Quaternion(Real w, Real x, Real y, Real z) {
@@ -58,14 +59,12 @@ namespace oopse{
                 data_[3] = z;                
             }
             
-            /**
-             *  
-             */
+            /** Constructs and initializes a Quaternion from a  Vector<Real,4> */     
             Quaternion(const Vector<Real,4>& v) 
                 : Vector<Real, 4>(v){
             }
 
-            /** */
+            /** copy assignment */
             Quaternion& operator =(const Vector<Real, 4>& v){
                 if (this == & v)
                     return *this;
@@ -139,14 +138,30 @@ namespace oopse{
             }
 
             /**
+             * 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<Real>& q) {
+
+                for (unsigned int i = 0; i < 4; i ++) {
+                    if (!equal(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<Real> inverse(){
+            Quaternion<Real> inverse() {
                 Quaternion<Real> q;
-                Real d = this->lengthSquared();
+                Real d = this->lengthSquare();
                 
                 q.w() = w() / d;
                 q.x() = -x() / d;
@@ -161,38 +176,52 @@ namespace oopse{
              * @param q the other quaternion
              */
             void mul(const Quaternion<Real>& q) {
+                Quaternion<Real> 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()) );
+                data_[0] = (tmp[0]*q[0]) -(tmp[1]*q[1]) - (tmp[2]*q[2]) - (tmp[3]*q[3]);
+                data_[1] = (tmp[0]*q[1]) + (tmp[1]*q[0]) + (tmp[2]*q[3]) - (tmp[3]*q[2]);
+                data_[2] = (tmp[0]*q[2]) + (tmp[2]*q[0]) + (tmp[3]*q[1]) - (tmp[1]*q[3]);
+                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) {
+                data_[0] *= s;
+                data_[1] *= s;
+                data_[2] *= s;
+                data_[3] *= s;
             }
 
-
             /** Set the value of this quaternion to the division of itself by another quaternion */
-            void div(const Quaternion<Real>& q) {
+            void div(Quaternion<Real>& q) {
                 mul(q.inverse());
             }
+
+            void div(const Real& s) {
+                data_[0] /= s;
+                data_[1] /= s;
+                data_[2] /= s;
+                data_[3] /= s;
+            }
             
             Quaternion<Real>& operator *=(const Quaternion<Real>& q) {
                 mul(q);
                 return *this;
             }
-                        
-            Quaternion<Real>& operator /=(const Quaternion<Real>& q) {
-                mul(q.inverse());
+
+            Quaternion<Real>& operator *=(const Real& s) {
+                mul(s);
                 return *this;
             }
             
+            Quaternion<Real>& operator /=(Quaternion<Real>& q) {                
+                *this *= q.inverse();
+                return *this;
+            }
+
+            Quaternion<Real>& operator /=(const Real& s) {
+                div(s);
+                return *this;
+            }            
             /**
              * Returns the conjugate quaternion of this quaternion
              * @return the conjugate quaternion of this quaternion
@@ -232,11 +261,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<typename Real, unsigned int Dim>                 
+    Quaternion<Real> operator * ( const Quaternion<Real>& q, Real s) {       
+        Quaternion<Real> 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<typename Real, unsigned int Dim>
+    Quaternion<Real> operator * ( const Real& s, const Quaternion<Real>& q ) {
+        Quaternion<Real> 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 +314,7 @@ namespace oopse{
      */
 
     template<typename Real>
-    inline Quaternion<Real> operator /(const Quaternion<Real>& q1, const Quaternion<Real>& q2) {
+    inline Quaternion<Real> operator /( Quaternion<Real>& q1,  Quaternion<Real>& q2) {
         return q1 * q2.inverse();
     }
 
@@ -268,12 +326,19 @@ namespace oopse{
      * @note for a quaternion q, 1/q = q.inverse()
      */
     template<typename Real>
-    Quaternion<Real> operator /(const Real& s, const Quaternion<Real>& q) {
+    Quaternion<Real> operator /(const Real& s,  Quaternion<Real>& q) {
 
-        Quaternion<Real> x = q.inv();
-        return x * s;
+        Quaternion<Real> x;
+        x = q.inverse();
+        x *= s;
+        return x;
     }
-
+    
+    template <class T>
+    inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs) {
+        return equal(lhs[0] ,rhs[0]) && equal(lhs[1] , rhs[1]) && equal(lhs[2], rhs[2]) && equal(lhs[3], rhs[3]);
+    }
+    
     typedef Quaternion<double> Quat4d;
 }
 #endif //MATH_QUATERNION_HPP