Templatize tests

Some failing tests with float scalar type are disabled for now. They will be addressed in the future commits.

include/fcl/BV/bv_utility.h

@@ -77,7 +77,7 @@ template <typename Scalar>
 void getExtentAndCenter_pointcloud(
     Vector3<Scalar>* ps, Vector3<Scalar>* ps2,
     Triangle* ts, unsigned int* indices, int n,
-    const Matrix3<Scalar>& axis, Vector3d& center, Vector3<Scalar>& extent);
+    const Matrix3<Scalar>& axis, Vector3<Scalar>& center, Vector3<Scalar>& extent);
 
 /// \brief Compute the bounding volume extent and center for a set or subset of
 /// points. The bounding volume axes are known.

include/fcl/articulated_model/joint.h

@@ -290,7 +290,7 @@ std::size_t PrismaticJoint<Scalar>::getNumDofs() const
 template <typename Scalar>
 Transform3<Scalar> PrismaticJoint<Scalar>::getLocalTransform() const
 {
-  const Quaternion3d quat(transform_to_parent_.linear());
+  const Quaternion3<Scalar> quat(transform_to_parent_.linear());
   const Vector3<Scalar>& transl = transform_to_parent_.translation();
 
   Transform3<Scalar> tf = Transform3<Scalar>::Identity();
@@ -331,7 +331,7 @@ template <typename Scalar>
 Transform3<Scalar> RevoluteJoint<Scalar>::getLocalTransform() const
 {
   Transform3<Scalar> tf = Transform3<Scalar>::Identity();
-  tf.linear() = transform_to_parent_.linear() * Eigen::AngleAxisd((*joint_cfg_)[0], axis_);
+  tf.linear() = transform_to_parent_.linear() * AngleAxis<Scalar>((*joint_cfg_)[0], axis_);
   tf.translation() = transform_to_parent_.translation();
 
   return tf;
@@ -356,10 +356,10 @@ std::size_t BallEulerJoint<Scalar>::getNumDofs() const
 template <typename Scalar>
 Transform3<Scalar> BallEulerJoint<Scalar>::getLocalTransform() const
 {
-  Matrix3d rot(
-      Eigen::AngleAxisd((*joint_cfg_)[0], Eigen::Vector3<Scalar>::UnitX())
-        * Eigen::AngleAxisd((*joint_cfg_)[1], Eigen::Vector3<Scalar>::UnitY())
-        * Eigen::AngleAxisd((*joint_cfg_)[2], Eigen::Vector3<Scalar>::UnitZ()));
+  Matrix3<Scalar> rot(
+      AngleAxis<Scalar>((*joint_cfg_)[0], Eigen::Vector3<Scalar>::UnitX())
+        * AngleAxis<Scalar>((*joint_cfg_)[1], Eigen::Vector3<Scalar>::UnitY())
+        * AngleAxis<Scalar>((*joint_cfg_)[2], Eigen::Vector3<Scalar>::UnitZ()));
 
   Transform3<Scalar> tf = Transform3<Scalar>::Identity();
   tf.linear() = rot;

include/fcl/articulated_model/link.h

@@ -64,7 +64,7 @@ class Link
 
   void setParentJoint(const std::shared_ptr<Joint>& joint);
 
-  void addObject(const std::shared_ptr<CollisionObjectd>& object);
+  void addObject(const std::shared_ptr<CollisionObject<Scalar>>& object);
 
   std::size_t getNumChildJoints() const;
 
@@ -73,7 +73,7 @@ class Link
 protected:
   std::string name_;
 
-  std::vector<std::shared_ptr<CollisionObjectd> > objects_;
+  std::vector<std::shared_ptr<CollisionObject<Scalar>> > objects_;
 
   std::vector<std::shared_ptr<Joint> > children_joints_;
 
@@ -121,7 +121,7 @@ void Link<Scalar>::setParentJoint(const std::shared_ptr<Joint>& joint)
 
 //==============================================================================
 template <typename Scalar>
-void Link<Scalar>::addObject(const std::shared_ptr<CollisionObjectd>& object)
+void Link<Scalar>::addObject(const std::shared_ptr<CollisionObject<Scalar>>& object)
 {
   objects_.push_back(object);
 }

include/fcl/broadphase/broadphase_SSaP.h

@@ -421,10 +421,10 @@ template <typename Scalar>
 bool SSaPCollisionManager<Scalar>::distance_(CollisionObject<Scalar>* obj, void* cdata, DistanceCallBack<Scalar> callback, Scalar& min_dist) const
 {
   static const unsigned int CUTOFF = 100;
-  Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
-  Vector3d dummy_vector = obj->getAABB().max_;
+  Vector3<Scalar> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  Vector3<Scalar> dummy_vector = obj->getAABB().max_;
   if(min_dist < std::numeric_limits<Scalar>::max())
-    dummy_vector += Vector3d(min_dist, min_dist, min_dist);
+    dummy_vector += Vector3<Scalar>(min_dist, min_dist, min_dist);
 
   typename std::vector<CollisionObject<Scalar>*>::const_iterator pos_start1 = objs_x.begin();
   typename std::vector<CollisionObject<Scalar>*>::const_iterator pos_start2 = objs_y.begin();
@@ -491,12 +491,12 @@ bool SSaPCollisionManager<Scalar>::distance_(CollisionObject<Scalar>* obj, void*
         // to check the possible missed ones to the right of the objs array
         if(min_dist < old_min_distance)
         {
-          dummy_vector = obj->getAABB().max_ + Vector3d(min_dist, min_dist, min_dist);
+          dummy_vector = obj->getAABB().max_ + Vector3<Scalar>(min_dist, min_dist, min_dist);
           status = 0;
         }
         else // need more loop
         {
-          if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits<Vector3d::Scalar>::epsilon() * 100))
+          if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits<Scalar>::epsilon() * 100))
             dummy_vector = dummy_vector + delta;
           else
             dummy_vector = dummy_vector * 2 - obj->getAABB().max_;

include/fcl/broadphase/broadphase_bruteforce.h

@@ -198,10 +198,10 @@ void NaiveCollisionManager<Scalar>::collide(void* cdata, CollisionCallBack<Scala
 {
   if(size() == 0) return;
 
-  for(typename std::list<CollisionObjectd*>::const_iterator it1 = objs.begin(), end = objs.end();
+  for(typename std::list<CollisionObject<Scalar>*>::const_iterator it1 = objs.begin(), end = objs.end();
       it1 != end; ++it1)
   {
-    typename std::list<CollisionObjectd*>::const_iterator it2 = it1; it2++;
+    typename std::list<CollisionObject<Scalar>*>::const_iterator it2 = it1; it2++;
     for(; it2 != end; ++it2)
     {
       if((*it1)->getAABB().overlap((*it2)->getAABB()))
@@ -220,9 +220,9 @@ void NaiveCollisionManager<Scalar>::distance(void* cdata, DistanceCallBack<Scala
   if(size() == 0) return;
 
   Scalar min_dist = std::numeric_limits<Scalar>::max();
-  for(typename std::list<CollisionObjectd*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
+  for(typename std::list<CollisionObject<Scalar>*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1)
   {
-    typename std::list<CollisionObjectd*>::const_iterator it2 = it1; it2++;
+    typename std::list<CollisionObject<Scalar>*>::const_iterator it2 = it1; it2++;
     for(; it2 != end; ++it2)
     {
       if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)

include/fcl/broadphase/broadphase_interval_tree.h

@@ -385,8 +385,8 @@ void IntervalTreeCollisionManager<Scalar>::update()
 template <typename Scalar>
 void IntervalTreeCollisionManager<Scalar>::update(CollisionObject<Scalar>* updated_obj)
 {
-  AABBd old_aabb;
-  const AABBd& new_aabb = updated_obj->getAABB();
+  AABB<Scalar> old_aabb;
+  const AABB<Scalar>& new_aabb = updated_obj->getAABB();
   for(int i = 0; i < 3; ++i)
   {
     const auto it = obj_interval_maps[i].find(updated_obj);
@@ -539,11 +539,11 @@ bool IntervalTreeCollisionManager<Scalar>::distance_(CollisionObject<Scalar>* ob
 {
   static const unsigned int CUTOFF = 100;
 
-  Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
-  AABBd aabb = obj->getAABB();
+  Vector3<Scalar> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+  AABB<Scalar> aabb = obj->getAABB();
   if(min_dist < std::numeric_limits<Scalar>::max())
   {
-    Vector3d min_dist_delta(min_dist, min_dist, min_dist);
+    Vector3<Scalar> min_dist_delta(min_dist, min_dist, min_dist);
     aabb.expand(min_dist_delta);
   }
 
@@ -601,8 +601,8 @@ bool IntervalTreeCollisionManager<Scalar>::distance_(CollisionObject<Scalar>* ob
       {
         if(min_dist < old_min_distance)
         {
-          Vector3d min_dist_delta(min_dist, min_dist, min_dist);
-          aabb = AABBd(obj->getAABB(), min_dist_delta);
+          Vector3<Scalar> min_dist_delta(min_dist, min_dist, min_dist);
+          aabb = AABB<Scalar>(obj->getAABB(), min_dist_delta);
           status = 0;
         }
         else
@@ -651,8 +651,8 @@ void IntervalTreeCollisionManager<Scalar>::collide(void* cdata, CollisionCallBac
       for(; iter != end; ++iter)
       {
         CollisionObject<Scalar>* active_index = *iter;
-        const AABBd& b0 = active_index->getAABB();
-        const AABBd& b1 = index->getAABB();
+        const AABB<Scalar>& b0 = active_index->getAABB();
+        const AABB<Scalar>& b1 = index->getAABB();
 
         int axis2 = (axis + 1) % 3;
         int axis3 = (axis + 2) % 3;

include/fcl/ccd/motion.h

@@ -72,7 +72,7 @@ class TranslationMotion : public MotionBase<Scalar>
     tf.translation() = trans_start;
   }
 
-  bool integrate(Scalar dt) const
+  bool integrate(Scalar dt) const override
   {
     if(dt > 1)
       dt = 1;
@@ -142,27 +142,27 @@ class SplineMotion : public MotionBase<Scalar>
 
   /// @brief Integrate the motion from 0 to dt
   /// We compute the current transformation from zero point instead of from last integrate time, for precision.
-  bool integrate(double dt) const;
+  bool integrate(Scalar dt) const;
 
   /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
-  Scalar computeMotionBound(const BVMotionBoundVisitor<Scalar>& mb_visitor) const
+  Scalar computeMotionBound(const BVMotionBoundVisitor<Scalar>& mb_visitor) const override
   {
     return mb_visitor.visit(*this);
   }
 
   /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor
-  Scalar computeMotionBound(const TriangleMotionBoundVisitor<Scalar>& mb_visitor) const
+  Scalar computeMotionBound(const TriangleMotionBoundVisitor<Scalar>& mb_visitor) const override
   {
     return mb_visitor.visit(*this);
   }
 
   /// @brief Get the rotation and translation in current step
-  void getCurrentTransform(Transform3<Scalar>& tf_) const
+  void getCurrentTransform(Transform3<Scalar>& tf_) const override
   {
     tf_ = tf;
   }
 
-  void getTaylorModel(TMatrix3<Scalar>& tm, TVector3<Scalar>& tv) const
+  void getTaylorModel(TMatrix3<Scalar>& tm, TVector3<Scalar>& tv) const override
   {
     // set tv
     Vector3<Scalar> c[4];
@@ -379,7 +379,7 @@ class ScrewMotion : public MotionBase<Scalar>
 protected:
   void computeScrewParameter()
   {
-    const Eigen::AngleAxisd aa(tf2.linear() * tf1.linear().transpose());
+    const AngleAxis<Scalar> aa(tf2.linear() * tf1.linear().transpose());
 
     axis = aa.axis();
     angular_vel = aa.angle();
@@ -407,7 +407,7 @@ class ScrewMotion : public MotionBase<Scalar>
 
   Quaternion3<Scalar> deltaRotation(Scalar dt) const
   {
-    return Quaternion3<Scalar>(Eigen::AngleAxisd((Scalar)(dt * angular_vel), axis));
+    return Quaternion3<Scalar>(AngleAxis<Scalar>((Scalar)(dt * angular_vel), axis));
   }
 
   Quaternion3<Scalar> absoluteRotation(Scalar dt) const
@@ -633,7 +633,7 @@ SplineMotion<Scalar>::SplineMotion(const Vector3<Scalar>& Td0, const Vector3<Sca
 
 //==============================================================================
 template <typename Scalar>
-bool SplineMotion<Scalar>::integrate(double dt) const
+bool SplineMotion<Scalar>::integrate(Scalar dt) const
 {
   if(dt > 1) dt = 1;
 
@@ -642,7 +642,7 @@ bool SplineMotion<Scalar>::integrate(double dt) const
   Scalar cur_angle = cur_w.norm();
   cur_w.normalize();
 
-  tf.linear() = Eigen::AngleAxisd(cur_angle, cur_w).toRotationMatrix();
+  tf.linear() = AngleAxis<Scalar>(cur_angle, cur_w).toRotationMatrix();
   tf.translation() = cur_T;
 
   tf_t = dt;
@@ -892,7 +892,7 @@ void InterpMotion<Scalar>::computeVelocity()
 {
   linear_vel = tf2 * reference_p - tf1 * reference_p;
 
-  const Eigen::AngleAxisd aa(tf2.linear() * tf1.linear().transpose());
+  const AngleAxis<Scalar> aa(tf2.linear() * tf1.linear().transpose());
   angular_axis = aa.axis();
   angular_vel = aa.angle();
 
@@ -907,7 +907,7 @@ void InterpMotion<Scalar>::computeVelocity()
 template <typename Scalar>
 Quaternion3<Scalar> InterpMotion<Scalar>::deltaRotation(Scalar dt) const
 {
-  return Quaternion3<Scalar>(Eigen::AngleAxisd((Scalar)(dt * angular_vel), angular_axis));
+  return Quaternion3<Scalar>(AngleAxis<Scalar>((Scalar)(dt * angular_vel), angular_axis));
 }
 
 //==============================================================================

include/fcl/ccd/motion_base.h

@@ -127,7 +127,7 @@ class MotionBase
   virtual ~MotionBase() {}
 
   /** \brief Integrate the motion from 0 to dt */
-  virtual bool integrate(double dt) const = 0;
+  virtual bool integrate(Scalar dt) const = 0;
 
   /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
   virtual Scalar computeMotionBound(const BVMotionBoundVisitor<Scalar>& mb_visitor) const = 0;

include/fcl/ccd/taylor_matrix.h

@@ -62,7 +62,7 @@ class TMatrix3
   const TaylorModel<Scalar>& operator () (size_t i, size_t j) const;
   TaylorModel<Scalar>& operator () (size_t i, size_t j);
 
-  TVector3<Scalar> operator * (const Vector3d& v) const;
+  TVector3<Scalar> operator * (const Vector3<Scalar>& v) const;
   TVector3<Scalar> operator * (const TVector3<Scalar>& v) const;
   TMatrix3 operator * (const Matrix3<Scalar>& m) const;
   TMatrix3 operator * (const TMatrix3& m) const;
@@ -224,9 +224,9 @@ TaylorModel<Scalar>& TMatrix3<Scalar>::operator () (size_t i, size_t j)
 template <typename Scalar>
 TMatrix3<Scalar> TMatrix3<Scalar>::operator * (const Matrix3<Scalar>& m) const
 {
-  const Vector3d& mc0 = m.col(0);
-  const Vector3d& mc1 = m.col(1);
-  const Vector3d& mc2 = m.col(2);
+  const Vector3<Scalar>& mc0 = m.col(0);
+  const Vector3<Scalar>& mc1 = m.col(1);
+  const Vector3<Scalar>& mc2 = m.col(2);
 
   return TMatrix3(TVector3<Scalar>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
                   TVector3<Scalar>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
@@ -235,7 +235,7 @@ TMatrix3<Scalar> TMatrix3<Scalar>::operator * (const Matrix3<Scalar>& m) const
 
 //==============================================================================
 template <typename Scalar>
-TVector3<Scalar> TMatrix3<Scalar>::operator * (const Vector3d& v) const
+TVector3<Scalar> TMatrix3<Scalar>::operator * (const Vector3<Scalar>& v) const
 {
   return TVector3<Scalar>(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
 }
@@ -278,9 +278,9 @@ TMatrix3<Scalar> TMatrix3<Scalar>::operator * (Scalar d) const
 template <typename Scalar>
 TMatrix3<Scalar>& TMatrix3<Scalar>::operator *= (const Matrix3<Scalar>& m)
 {
-  const Vector3d& mc0 = m.col(0);
-  const Vector3d& mc1 = m.col(1);
-  const Vector3d& mc2 = m.col(2);
+  const Vector3<Scalar>& mc0 = m.col(0);
+  const Vector3<Scalar>& mc1 = m.col(1);
+  const Vector3<Scalar>& mc2 = m.col(2);
 
   v_[0] = TVector3<Scalar>(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
   v_[1] = TVector3<Scalar>(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));

include/fcl/collision.h

@@ -103,7 +103,7 @@ std::size_t collide(
   if(!nsolver_)
     nsolver = new NarrowPhaseSolver();
 
-  const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
+  const auto& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
 
   std::size_t res;
   if(request.num_max_contacts == 0)

include/fcl/collision_data.h

@@ -136,7 +136,7 @@ struct Contact
 using Contactf = Contact<float>;
 using Contactd = Contact<double>;
 
-/// @brief Cost source describes an area with a cost. The area is described by an AABBd region.
+/// @brief Cost source describes an area with a cost. The area is described by an AABB<Scalar> region.
 template <typename Scalar>
 struct CostSource
 {
@@ -146,14 +146,14 @@ struct CostSource
   /// @brief aabb upper bound
   Vector3<Scalar> aabb_max;
 
-  /// @brief cost density in the AABBd region
+  /// @brief cost density in the AABB<Scalar> region
   Scalar cost_density;
 
   Scalar total_cost;
 
   CostSource(const Vector3<Scalar>& aabb_min_, const Vector3<Scalar>& aabb_max_, Scalar cost_density_);
 
-  CostSource(const AABBd& aabb, Scalar cost_density_);
+  CostSource(const AABB<Scalar>& aabb, Scalar cost_density_);
 
   CostSource();
 
@@ -501,7 +501,7 @@ CostSource<Scalar>::CostSource(const Vector3<Scalar>& aabb_min_, const Vector3<S
 
 //==============================================================================
 template <typename Scalar>
-CostSource<Scalar>::CostSource(const AABBd& aabb, Scalar cost_density_) : aabb_min(aabb.min_),
+CostSource<Scalar>::CostSource(const AABB<Scalar>& aabb, Scalar cost_density_) : aabb_min(aabb.min_),
   aabb_max(aabb.max_),
   cost_density(cost_density_)
 {

include/fcl/collision_node.h

@@ -99,8 +99,8 @@ void collide2(MeshCollisionTraversalNodeOBB<Scalar>* node, BVHFrontList* front_l
   }
   else
   {
-    Matrix3d Rtemp, R;
-    Vector3d Ttemp, T;
+    Matrix3<Scalar> Rtemp, R;
+    Vector3<Scalar> Ttemp, T;
     Rtemp = node->R * node->model2->getBV(0).getOrientation();
     R = node->model1->getBV(0).getOrientation().transpose() * Rtemp;
     Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T;