diff --git a/trunk/fcl/include/fcl/articulated_model/joint.h b/trunk/fcl/include/fcl/articulated_model/joint.h
new file mode 100644
index 0000000000000000000000000000000000000000..76db3d7b7e990f728d0bb00a950e3384c645c9c1
--- /dev/null
+++ b/trunk/fcl/include/fcl/articulated_model/joint.h
@@ -0,0 +1,125 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_H
+#define FCL_ARTICULATED_MODEL_JOINT_H
+
+#include "fcl/math/transform.h"
+#include "fcl/data_types.h"
+
+#include <string>
+#include <vector>
+#include <map>
+#include <limits>
+#include <boost/shared_ptr.hpp>
+
+namespace fcl
+{
+
+class JointConfig;
+
+/// @brief Base Joint
+class Joint
+{
+public:
+  Joint(const std::string& name, std::size_t dofs_num);
+
+  virtual ~Joint() {}
+
+  /// @brief Get joint's name
+  std::string getName() const;
+
+  /// @brief Get Joint's degrees of freedom
+  std::size_t getDOFs() const;
+
+  /// @brief How joint works
+  virtual Transform3f append(const JointConfig& q, const Transform3f& t) const = 0;
+
+  bool isCompatible(const JointConfig& q) const;
+  
+private:
+  std::string name_;	
+  std::size_t dofs_num_;
+};
+
+
+class PrismaticJoint : public Joint
+{
+public:
+  PrismaticJoint(const std::string& name, const Vec3f& axis);
+
+  virtual ~PrismaticJoint() {}
+
+  Vec3f getAxis() const;
+
+  Transform3f append(const JointConfig& q, const Transform3f& t) const;
+
+private:
+  Vec3f axis_; // prismatic axis
+};
+
+
+class RevoluteJoint : public Joint
+{
+public:
+  RevoluteJoint(const std::string& name, const Vec3f& axis);
+
+  virtual ~RevoluteJoint() {}
+
+  Vec3f getAxis() const;
+
+  Transform3f append(const JointConfig& q, const Transform3f& t) const;
+  
+private:
+  Vec3f axis_; // revolute axis
+};
+
+class SphericJoint : public Joint
+{
+public:
+  SphericJoint(const std::string& name);
+
+  virtual ~SphericJoint() {}
+
+  Transform3f append(const JointConfig& q, const Transform3f& t) const;
+ 
+private:
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/articulated_model/joint_config.h b/trunk/fcl/include/fcl/articulated_model/joint_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..ecbdb95980e234e74fa3e44da412e6124af2c602
--- /dev/null
+++ b/trunk/fcl/include/fcl/articulated_model/joint_config.h
@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+
+#include "fcl/data_types.h"
+#include <boost/shared_ptr.hpp>
+#include <vector>
+
+namespace fcl
+{
+
+class Joint;
+
+class JointConfig
+{
+public:
+  JointConfig();
+
+  JointConfig(const JointConfig& joint_cfg);
+
+  JointConfig(boost::shared_ptr<Joint> joint, FCL_REAL default_value = 0);
+
+  std::size_t size() const;
+
+  inline FCL_REAL operator [] (std::size_t i) const
+  {
+    return values_[i];
+  }
+
+  inline FCL_REAL& operator [] (std::size_t i)
+  {
+    return values_[i];
+  }
+
+  bool operator == (const JointConfig& joint_cfg) const;
+
+  boost::shared_ptr<Joint> getJoint() const;
+
+private:
+  boost::shared_ptr<Joint> joint_;
+
+  std::vector<FCL_REAL> values_;
+};
+
+}
+
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/articulated_model/model.h b/trunk/fcl/include/fcl/articulated_model/model.h
new file mode 100644
index 0000000000000000000000000000000000000000..9aa8bbddd379a1c7b5363d3e0860f2b707d6526d
--- /dev/null
+++ b/trunk/fcl/include/fcl/articulated_model/model.h
@@ -0,0 +1,255 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_H
+#define FCL_ARTICULATED_MODEL_MODEL_H
+
+#include "fcl/data_types.h"
+#include "fcl/articulated_model/joint.h"
+
+#include <map>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/enable_shared_from_this.hpp>
+
+
+namespace fcl
+{
+
+
+template<typename Key, typename Value>
+class GeneralTreeNode : public boost::enable_shared_from_this<GeneralTreeNode<Key, Value> >
+{
+public:
+  GeneralTreeNode(Key key, Value value):
+    key_(key),
+    value_(value)
+  {}
+
+  void setParent(boost::weak_ptr<GeneralTreeNode<Key, Value> > parent)
+  {
+    this->parent_ = parent;
+
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent_shared;
+
+    if(parent_shared = parent.lock())
+      parent_shared->addChild(this->shared_from_this());
+  }
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > getParent() const
+  {
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent_shared;
+    parent_shared = this->parent_.lock();
+    return parent_shared;
+  }
+
+  bool hasParent() const
+  {
+    bool has_parent = false;
+
+    if(this->parent_.lock())
+      has_parent = true;
+
+    return has_parent;
+  }
+
+  void addChild(boost::shared_ptr<GeneralTreeNode<Key, Value> > child)
+  {
+    this->children_map_[child->getKey()] = child;
+  }
+
+  std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > > getChilds() const
+  {
+    return this->children_map_;
+  }
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > getChildByKey(Key key) const
+  {
+    typename std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it = this->children_map_.find(key);
+
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > child = it->second; 
+
+    BOOST_ASSERT_MSG(child.use_count() != 0, "Child wasn't found! Maybe bad key.");
+
+    return child;
+  }
+
+  Key getKey() const
+  {
+    return this->key_;
+  }
+
+  Value getValue() const
+  {
+    return this->value_;
+  }
+
+private:
+  Key key_;
+  Value value_;
+
+  boost::weak_ptr<GeneralTreeNode<Key, Value> > parent_; // empty means that this Joint is root
+  std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > > children_map_;
+};
+
+
+template<typename Key, typename Value>
+class GeneralTree
+{
+public:
+  GeneralTree()
+  {
+  }	
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > createRoot(Key key, Value value)
+  {
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > node(new GeneralTreeNode<Key, Value>(key,value));
+
+    if(this->root.use_count() != 0)
+    {
+      this->root->setParent(node);
+    }
+
+    this->root = node;
+    this->nodes_map[key] = node;
+
+    return node;
+  }
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > createNode(Key key, Value value, Key parentKey)
+  {
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent(getNodeByKey(parentKey) );
+
+    if (parent.use_count() == 0)
+    {
+      return boost::shared_ptr<GeneralTreeNode<Key, Value> >();
+    }
+
+    boost::shared_ptr<GeneralTreeNode<Key, Value> > node(new GeneralTreeNode<Key, Value>(key,value) );
+
+    node->setParent(parent);
+    this->nodes_map[key] = node;
+
+    return node;
+  }	
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > getNodeByKey(Key key) const
+  {
+    typename std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it = nodes_map.find(key);
+    boost::weak_ptr<GeneralTreeNode<Key, Value> > node = it->second;
+    return node.lock();
+  }
+
+  bool existsKey(Key key) const
+  {
+    return nodes_map.find(key) != nodes_map.end();
+  }
+
+  FCL_INT32 getNodesNum() const
+  {
+    return this->nodes_map.size();
+  }
+
+  std::map<Key, Value> getValuesMap() const
+  {
+    std::map<Key, Value> values_map;
+
+    typename std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it;
+
+    for(it = this->nodes_map.begin(); it != this->nodes_map.end(); ++it)
+    {
+      Key first = it->first;
+      if(boost::shared_ptr<GeneralTreeNode<Key, Value> > second_shared = it->second.lock())
+      {
+        values_map[first] = second_shared->getValue();
+      }
+    }
+
+    return values_map;
+  }
+
+private:
+
+  boost::shared_ptr<GeneralTreeNode<Key, Value> > root;
+  std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > > nodes_map; // holds node under its key
+};
+
+class ModelConfig;
+
+class Model : public boost::enable_shared_from_this<Model>
+{
+public:
+  Model(const std::string& name);
+
+  boost::shared_ptr<Model> getSharedPtr();
+
+  std::string getName() const;
+
+  void addJoint(boost::shared_ptr<Joint> joint, const std::string& parent_name = "");
+
+  boost::shared_ptr<Joint> getJointByName(const std::string& joint_name) const;
+  boost::shared_ptr<Joint> getJointParentByName(const std::string& joint_name) const;
+  boost::shared_ptr<Joint> getJointParent(boost::shared_ptr<Joint> joint) const;
+
+  bool hasJointParentByName(const std::string& joint_name) const;
+  bool hasJointParent(boost::shared_ptr<Joint> joint) const;
+
+  std::size_t getJointNum() const;
+
+  std::map<std::string, boost::shared_ptr<Joint> > getJointsMap() const;
+
+  ///FCL_REAL getMotionBoundFromParentToChildJoint(const std::string& child_name) const;
+  ///FCL_REAL getMotionBoundFromParentToChildJoint(boost::shared_ptr<Joint> child_joint) const;
+
+  ///Vec3f getVectorFromParentToChildJoint(const std::string& child_name, FCL_REAL time) const;
+  ///Vec3f getVectorFromParentToChildJoint(boost::shared_ptr<Joint> child_joint, FCL_REAL time) const;
+
+  std::vector<boost::shared_ptr<Joint> > getJointsChain(const std::string& last_joint_name) const;
+  std::vector<boost::shared_ptr<Joint> > getJointsChain(boost::shared_ptr<Joint> last_joint) const;
+
+  bool isCompatible(const ModelConfig& model_config) const;
+
+private:
+  std::string name_;
+  GeneralTree<std::string, boost::shared_ptr<Joint> > joints_tree_;
+};
+
+
+}
+
+#endif
+
diff --git a/trunk/fcl/include/fcl/articulated_model/model_config.h b/trunk/fcl/include/fcl/articulated_model/model_config.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e7c99bca71a5e7da0f52ebe3d08e22fd0e6a0c0
--- /dev/null
+++ b/trunk/fcl/include/fcl/articulated_model/model_config.h
@@ -0,0 +1,77 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+#define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+
+#include "fcl/data_types.h"
+#include "fcl/articulated_model/joint_config.h"
+#include <string>
+#include <map>
+#include <boost/weak_ptr.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace fcl
+{
+
+class ModelConfig
+{
+public:
+  ModelConfig();
+
+  ModelConfig(const ModelConfig& model_cfg);
+
+  ModelConfig(std::map<std::string, boost::shared_ptr<Joint> > joints_map);
+
+  JointConfig getJointConfigByJointName(const std::string& joint_name) const;
+  JointConfig& getJointConfigByJointName(const std::string& joint_name);
+
+  JointConfig getJointConfigByJoint(boost::shared_ptr<Joint> joint) const;
+  JointConfig& getJointConfigByJoint(boost::shared_ptr<Joint> joint);
+
+  bool operator == (const ModelConfig& model_cfg) const;
+
+  std::map<std::string, JointConfig> getJointCfgsMap() const
+  { return joint_cfgs_map_; }
+
+private:
+  std::map<std::string, JointConfig> joint_cfgs_map_;
+};
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/math/matrix_3f.h b/trunk/fcl/include/fcl/math/matrix_3f.h
index 8ee76388ec9e0c54926f1a7146852351b2750256..674cf93011b697936929c4b231fc5a1c6e8039aa 100644
--- a/trunk/fcl/include/fcl/math/matrix_3f.h
+++ b/trunk/fcl/include/fcl/math/matrix_3f.h
@@ -176,6 +176,42 @@ public:
     data.setZero();
   }
 
+  /// @brief Set the matrix from euler angles YPR around ZYX axes
+  /// @param eulerX Roll about X axis
+  /// @param eulerY Pitch around Y axis
+  /// @param eulerZ Yaw aboud Z axis
+  ///  
+  /// These angles are used to produce a rotation matrix. The euler
+  /// angles are applied in ZYX order. I.e a vector is first rotated 
+  /// about X then Y and then Z
+  inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
+  {
+    FCL_REAL ci(cos(eulerX));
+    FCL_REAL cj(cos(eulerY));
+    FCL_REAL ch(cos(eulerZ));
+    FCL_REAL si(sin(eulerX));
+    FCL_REAL sj(sin(eulerY));
+    FCL_REAL sh(sin(eulerZ));
+    FCL_REAL cc = ci * ch;
+    FCL_REAL cs = ci * sh;
+    FCL_REAL sc = si * ch;
+    FCL_REAL ss = si * sh;
+
+    setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+             cj * sh, sj * ss + cc, sj * cs - sc, 
+             -sj,     cj * si,      cj * ci);
+
+  }
+
+  /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+  /// @param yaw Yaw about Y axis
+  /// @param pitch Pitch about X axis
+  /// @param roll Roll about Z axis 
+  void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
+  {
+    setEulerZYX(roll, pitch, yaw);
+  }
+
   inline U determinant() const
   {
     return data.determinant();
@@ -408,9 +444,9 @@ typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename
 
 
 #if FCL_HAVE_SSE
-  typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
 #else
-  typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
+typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
 #endif
 
 static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)