git-svn-id: https://kforge.ros.org/fcl/fcl_ros@84 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/include/fcl/transform.h

@@ -88,18 +88,22 @@ public:
 
   /** \brief addition */
   SimpleQuaternion operator + (const SimpleQuaternion& other) const;
+  const SimpleQuaternion& operator += (const SimpleQuaternion& other);
 
   /** \brief minus */
   SimpleQuaternion operator - (const SimpleQuaternion& other) const;
+  const SimpleQuaternion& operator -= (const SimpleQuaternion& other);
 
   /** \brief multiplication */
   SimpleQuaternion operator * (const SimpleQuaternion& other) const;
+  const SimpleQuaternion& operator *= (const SimpleQuaternion& other);
 
   /** \brief division */
   SimpleQuaternion operator - () const;
 
   /** \brief scalar multiplication */
   SimpleQuaternion operator * (BVH_REAL t) const;
+  const SimpleQuaternion& operator *= (BVH_REAL t);
 
   /** \brief conjugate */
   SimpleQuaternion conj() const;
@@ -217,6 +221,25 @@ public:
     return q.transform(v) + T;
   }
 
+  const SimpleTransform& operator *= (const SimpleTransform& other)
+  {
+    T = q.transform(other.T) + T;
+    q *= other.q;
+    q.toRotation(R);
+
+    return *this;
+  }
+
+  SimpleTransform operator * (const SimpleTransform& other) const
+  {
+    SimpleQuaternion q_new = q * other.q;
+    SimpleTransform t;
+    t.q = q_new;
+    q_new.toRotation(t.R);
+    t.T = q.transform(other.T) + T;
+    return t;
+  }
+
   bool isIdentity() const
   {
     return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 0) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)

trunk/fcl/src/transform.cpp

@@ -210,12 +210,32 @@ SimpleQuaternion SimpleQuaternion::operator + (const SimpleQuaternion& other) co
                           data[2] + other.data[2], data[3] + other.data[3]);
 }
 
+const SimpleQuaternion& SimpleQuaternion::operator += (const SimpleQuaternion& other)
+{
+  data[0] += other.data[0];
+  data[1] += other.data[1];
+  data[2] += other.data[2];
+  data[3] += other.data[3];
+
+  return *this;
+}
+
 SimpleQuaternion SimpleQuaternion::operator - (const SimpleQuaternion& other) const
 {
   return SimpleQuaternion(data[0] - other.data[0], data[1] - other.data[1],
                           data[2] - other.data[2], data[3] - other.data[3]);
 }
 
+const SimpleQuaternion& SimpleQuaternion::operator -= (const SimpleQuaternion& other)
+{
+  data[0] -= other.data[0];
+  data[1] -= other.data[1];
+  data[2] -= other.data[2];
+  data[3] -= other.data[3];
+
+  return *this;
+}
+
 SimpleQuaternion SimpleQuaternion::operator * (const SimpleQuaternion& other) const
 {
   return SimpleQuaternion(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
@@ -224,6 +244,21 @@ SimpleQuaternion SimpleQuaternion::operator * (const SimpleQuaternion& other) co
                           data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
 }
 
+
+const SimpleQuaternion& SimpleQuaternion::operator *= (const SimpleQuaternion& other)
+{
+  BVH_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
+  BVH_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
+  BVH_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1];
+  BVH_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0];
+
+  data[0] = a;
+  data[1] = b;
+  data[2] = c;
+  data[3] = d;
+  return *this;
+}
+
 SimpleQuaternion SimpleQuaternion::operator - () const
 {
   return SimpleQuaternion(-data[0], -data[1], -data[2], -data[3]);
@@ -234,6 +269,17 @@ SimpleQuaternion SimpleQuaternion::operator * (BVH_REAL t) const
   return SimpleQuaternion(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
 }
 
+const SimpleQuaternion& SimpleQuaternion::operator *= (BVH_REAL t)
+{
+  data[0] *= t;
+  data[1] *= t;
+  data[2] *= t;
+  data[3] *= t;
+
+  return *this;
+}
+
+
 SimpleQuaternion SimpleQuaternion::conj() const
 {
   return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);