Add a few methods in Matrix3fX and Vec3fX:

  Matrix3fX::isIdentity,
  Vec3fX::norm, Vec3fX::squaredNorm,
  Vec3fX::setZero,
  operator * (double, Vec3fX).x

include/fcl/math/matrix_3f.h

@@ -171,6 +171,14 @@ public:
     data.setIdentity();
   }
 
+  inline bool isIdentity () const
+  {
+    return
+      data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
+      data (0,0) == 0 && data (0,1) == 1 && data (0,2) == 0 &&
+      data (0,0) == 0 && data (0,1) == 0 && data (0,2) == 1;
+  }
+
   inline void setZero()
   {
     data.setZero();

include/fcl/math/vec_3f.h

@@ -125,9 +125,12 @@ public:
   }
 
   inline U length() const { return sqrt(details::dot_prod3(data, data)); }
+  inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
   inline U sqrLength() const { return details::dot_prod3(data, data); }
+  inline U squaredNorm() const { return details::dot_prod3(data, data); }
   inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
   inline void setValue(U x) { data.setValue(x); }
+  inline void setZero () {data.setValue (0); }
   inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
   inline Vec3fX<T>& negate() { data.negate(); return *this; }
 
@@ -241,6 +244,13 @@ static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
   return o;
 }
 
+ template <typename T>
+   inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+				 const Vec3fX <T>& v)
+   {
+     return Vec3fX <T> (v.data * t);
+   }
+
 
 }