Fixed issue with creating instances of type InterpMotion<float>

include/fcl/math/motion/interp_motion-inl.h

@@ -40,8 +40,7 @@
 
 #include "fcl/math/motion/interp_motion.h"
 
-namespace fcl
-{
+namespace fcl {
 
 //==============================================================================
 extern template
@@ -50,46 +49,21 @@ class FCL_EXPORT InterpMotion<double>;
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion()
-  : MotionBase<S>(), angular_axis(Vector3<S>::UnitX())
-{
-  // Default angular velocity is zero
-  angular_vel = 0;
-
-  // Default reference point is local zero point
-
-  // Default linear velocity is zero
-}
+    : InterpMotion(Transform3<S>::Identity(), Transform3<S>::Identity(),
+                   Vector3<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion(
     const Matrix3<S>& R1, const Vector3<S>& T1,
     const Matrix3<S>& R2, const Vector3<S>& T2)
-  : MotionBase<S>(),
-    tf1(Transform3<S>::Identity()),
-    tf2(Transform3<S>::Identity())
-{
-  tf1.linear() = R1;
-  tf1.translation() = T1;
-
-  tf2.linear() = R2;
-  tf2.translation() = T2;
-
-  tf = tf1;
-
-  // Compute the velocities for the motion
-  computeVelocity();
-}
+    : InterpMotion(R1, T1, R2, T2, Vector3<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
 InterpMotion<S>::InterpMotion(
     const Transform3<S>& tf1_, const Transform3<S>& tf2_)
-  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1)
-{
-  // Compute the velocities for the motion
-  computeVelocity();
-}
+  : InterpMotion<S>(tf1_, tf2_, Vector3<S>::Zero()) {}
 
 //==============================================================================
 template <typename S>
@@ -117,17 +91,26 @@ InterpMotion<S>::InterpMotion(
 }
 
 //==============================================================================
+
+// Note: In the name of using delegating constructors, we introduce the
+// silliness below. We *unpack* the transforms tf1_ and tf2_ just so that we
+// can pack them *back* into the member fields. Eigen forces us to do this
+// because we *can't* go the other way; there is no constructor of a Transform3
+// based on a Matrix3 and a Vector3. It might be worth creating a utility that
+// does that just so this could become more compact. The constructor that this
+// delegates to would, instead, do something like:
+//  InterpMotion(MakeTransform(R1, T1), MakeTransform(R2, T2), O) {}
+// And then the core functionality for initializing the class would go into this
+// constructor.
 template <typename S>
-InterpMotion<S>::InterpMotion(
-    const Transform3<S>& tf1_, const Transform3<S>& tf2_, const Vector3<S>& O)
-  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O)
-{
-  // Do nothing
-}
+InterpMotion<S>::InterpMotion(const Transform3<S>& tf1_,
+                              const Transform3<S>& tf2_, const Vector3<S>& O)
+    : InterpMotion(tf1_.linear(), tf1_.translation(), tf2_.linear(),
+                   tf2_.translation(), O) {}
 
 //==============================================================================
 template <typename S>
-bool InterpMotion<S>::integrate(double dt) const
+bool InterpMotion<S>::integrate(S dt) const
 {
   if(dt > 1) dt = 1;
 

include/fcl/math/motion/interp_motion.h

@@ -76,7 +76,7 @@ class FCL_EXPORT InterpMotion : public MotionBase<S>
 
   /// @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(S dt) const;
 
   /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
   S computeMotionBound(const BVMotionBoundVisitor<S>& mb_visitor) const;
@@ -90,7 +90,6 @@ class FCL_EXPORT InterpMotion : public MotionBase<S>
   void getTaylorModel(TMatrix3<S>& tm, TVector3<S>& tv) const;
 
 protected:
-
   void computeVelocity();
 
   Quaternion<S> deltaRotation(S dt) const;

include/fcl/math/motion/screw_motion-inl.h

@@ -92,7 +92,7 @@ ScrewMotion<S>::ScrewMotion(
 
 //==============================================================================
 template <typename S>
-bool ScrewMotion<S>::integrate(double dt) const
+bool ScrewMotion<S>::integrate(S dt) const
 {
   if(dt > 1) dt = 1;
 

include/fcl/math/motion/screw_motion.h

@@ -65,7 +65,7 @@ class FCL_EXPORT ScrewMotion : public MotionBase<S>
 
   /// @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(S dt) const;
 
   /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
   S computeMotionBound(const BVMotionBoundVisitor<S>& mb_visitor) const;

include/fcl/narrowphase/continuous_collision-inl.h

@@ -94,6 +94,7 @@ double collide(
     const ContinuousCollisionRequest<double>& request,
     ContinuousCollisionResult<double>& result);
 
+
 //==============================================================================
 template<typename GJKSolver>
 detail::ConservativeAdvancementFunctionMatrix<GJKSolver>&

src/math/motion/interp_motion.cpp

@@ -40,6 +40,9 @@
 namespace fcl
 {
 
+template
+class InterpMotion<float>;
+
 template
 class InterpMotion<double>;
 

src/math/motion/screw_motion.cpp

@@ -40,6 +40,9 @@
 namespace fcl
 {
 
+template
+class ScrewMotion<float>;
+
 template
 class ScrewMotion<double>;
 

src/narrowphase/collision.cpp

@@ -41,6 +41,13 @@ namespace fcl
 {
 
 //==============================================================================
+template
+std::size_t collide(
+    const CollisionObject<float>* o1,
+    const CollisionObject<float>* o2,
+    const CollisionRequest<float>& request,
+    CollisionResult<float>& result);
+
 template
 std::size_t collide(
     const CollisionObject<double>* o1,
@@ -49,6 +56,15 @@ std::size_t collide(
     CollisionResult<double>& result);
 
 //==============================================================================
+template
+std::size_t collide(
+    const CollisionGeometry<float>* o1,
+    const Transform3<float>& tf1,
+    const CollisionGeometry<float>* o2,
+    const Transform3<float>& tf2,
+    const CollisionRequest<float>& request,
+    CollisionResult<float>& result);
+
 template
 std::size_t collide(
     const CollisionGeometry<double>* o1,

src/narrowphase/collision_object.cpp

@@ -40,6 +40,9 @@
 namespace fcl
 {
 
+template
+class CollisionObject<float>;
+
 template
 class CollisionObject<double>;
 

src/narrowphase/continuous_collision.cpp

@@ -41,6 +41,15 @@ namespace fcl
 {
 
 //==============================================================================
+template
+float continuousCollide(
+    const CollisionGeometry<float>* o1,
+    const MotionBase<float>* motion1,
+    const CollisionGeometry<float>* o2,
+    const MotionBase<float>* motion2,
+    const ContinuousCollisionRequest<float>& request,
+    ContinuousCollisionResult<float>& result);
+
 template
 double continuousCollide(
     const CollisionGeometry<double>* o1,
@@ -51,6 +60,17 @@ double continuousCollide(
     ContinuousCollisionResult<double>& result);
 
 //==============================================================================
+template
+float continuousCollide(
+    const CollisionGeometry<float>* o1,
+    const Transform3<float>& tf1_beg,
+    const Transform3<float>& tf1_end,
+    const CollisionGeometry<float>* o2,
+    const Transform3<float>& tf2_beg,
+    const Transform3<float>& tf2_end,
+    const ContinuousCollisionRequest<float>& request,
+    ContinuousCollisionResult<float>& result);
+
 template
 double continuousCollide(
     const CollisionGeometry<double>* o1,
@@ -63,6 +83,15 @@ double continuousCollide(
     ContinuousCollisionResult<double>& result);
 
 //==============================================================================
+template
+float continuousCollide(
+    const CollisionObject<float>* o1,
+    const Transform3<float>& tf1_end,
+    const CollisionObject<float>* o2,
+    const Transform3<float>& tf2_end,
+    const ContinuousCollisionRequest<float>& request,
+    ContinuousCollisionResult<float>& result);
+
 template
 double continuousCollide(
     const CollisionObject<double>* o1,
@@ -73,6 +102,13 @@ double continuousCollide(
     ContinuousCollisionResult<double>& result);
 
 //==============================================================================
+template
+float collide(
+    const ContinuousCollisionObject<float>* o1,
+    const ContinuousCollisionObject<float>* o2,
+    const ContinuousCollisionRequest<float>& request,
+    ContinuousCollisionResult<float>& result);
+
 template
 double collide(
     const ContinuousCollisionObject<double>* o1,

src/narrowphase/continuous_collision_object.cpp

@@ -40,6 +40,9 @@
 namespace fcl
 {
 
+template
+class ContinuousCollisionObject<float>;
+
 template
 class ContinuousCollisionObject<double>;
 

test/CMakeLists.txt

@@ -97,6 +97,7 @@ foreach(test ${tests})
   add_fcl_test(${test})
 endforeach(test)
 
+add_subdirectory(broadphase)
 add_subdirectory(geometry)
+add_subdirectory(math)
 add_subdirectory(narrowphase)
-add_subdirectory(broadphase)

test/math/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(motion)

test/math/motion/CMakeLists.txt

@@ -0,0 +1,8 @@
+set(tests
+        test_interp_motion.cpp
+        )
+
+# Build all the tests
+foreach(test ${tests})
+    add_fcl_test(${test})
+endforeach(test)

test/math/motion/test_interp_motion.cpp

@@ -0,0 +1,99 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020. Toyota Research Institute
+ *  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 CNRS-LAAS and AIST 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 Sean Curtis ([email protected]) (2020) */
+
+#include "fcl/math/motion/interp_motion.h"
+
+#include <gtest/gtest.h>
+
+#include "eigen_matrix_compare.h"
+
+namespace fcl {
+namespace {
+
+// TODO(SeanCurtis-TRI): Convert this to a parameterized test harness and use
+// TEST_P.
+
+template <typename S>
+class InterpMotionTest : public ::testing::Test {};
+
+typedef testing::Types<double, float> MyTypes;
+TYPED_TEST_CASE(InterpMotionTest, MyTypes);
+
+TYPED_TEST(InterpMotionTest, Construction) {
+  using S = TypeParam;
+  const Transform3<S> I = Transform3<S>::Identity();
+
+  {
+    // Default constructor; everything should be identity.
+    InterpMotion<S> motion{};
+
+    Transform3<S> X_FA;
+    motion.getCurrentTransform(X_FA);
+    EXPECT_TRUE(CompareMatrices(X_FA.matrix(), I.matrix()));
+    EXPECT_TRUE(
+        CompareMatrices(motion.getLinearVelocity(), Vector3<S>::Zero()));
+    EXPECT_EQ(motion.getAngularVelocity(), S(0));
+    EXPECT_TRUE(CompareMatrices(motion.getAngularAxis(), Vector3<S>::UnitX()));
+    EXPECT_TRUE(
+        CompareMatrices(motion.getReferencePoint(), Vector3<S>::Zero()));
+  }
+
+  {
+    // Construct from start (R_FS, p_FSo) and goal (R_FG, p_FGo).
+  }
+
+  {
+    // Construct from start X_FS and goal X_FG.
+  }
+
+  {
+    // Construct from start (R_FS, p_FSo), goal (R_FG, p_FGo), and an origin of
+    //   rotation (p_FO).
+  }
+
+  {
+    // Construct from start X_FS, goal X_FG, and rotation origin p_FO.
+  }
+}
+
+}  // namespace
+}  // namespace fcl
+
+//==============================================================================
+int main(int argc, char *argv[]) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}