diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index 384367a80..30630f452 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/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; //============================================================================== template InterpMotion::InterpMotion() - : MotionBase(), angular_axis(Vector3::UnitX()) -{ - // Default angular velocity is zero - angular_vel = 0; - - // Default reference point is local zero point - - // Default linear velocity is zero -} + : InterpMotion(Transform3::Identity(), Transform3::Identity(), + Vector3::Zero()) {} //============================================================================== template InterpMotion::InterpMotion( const Matrix3& R1, const Vector3& T1, const Matrix3& R2, const Vector3& T2) - : MotionBase(), - tf1(Transform3::Identity()), - tf2(Transform3::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::Zero()) {} //============================================================================== template InterpMotion::InterpMotion( const Transform3& tf1_, const Transform3& tf2_) - : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1) -{ - // Compute the velocities for the motion - computeVelocity(); -} + : InterpMotion(tf1_, tf2_, Vector3::Zero()) {} //============================================================================== template @@ -117,17 +91,26 @@ InterpMotion::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 -InterpMotion::InterpMotion( - const Transform3& tf1_, const Transform3& tf2_, const Vector3& O) - : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O) -{ - // Do nothing -} +InterpMotion::InterpMotion(const Transform3& tf1_, + const Transform3& tf2_, const Vector3& O) + : InterpMotion(tf1_.linear(), tf1_.translation(), tf2_.linear(), + tf2_.translation(), O) {} //============================================================================== template -bool InterpMotion::integrate(double dt) const +bool InterpMotion::integrate(S dt) const { if(dt > 1) dt = 1; diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index a7f58eb74..12586e539 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -76,7 +76,7 @@ class FCL_EXPORT InterpMotion : public MotionBase /// @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& mb_visitor) const; @@ -90,7 +90,6 @@ class FCL_EXPORT InterpMotion : public MotionBase void getTaylorModel(TMatrix3& tm, TVector3& tv) const; protected: - void computeVelocity(); Quaternion deltaRotation(S dt) const; diff --git a/include/fcl/math/motion/screw_motion-inl.h b/include/fcl/math/motion/screw_motion-inl.h index 53c5c5bfd..ae72d6c70 100644 --- a/include/fcl/math/motion/screw_motion-inl.h +++ b/include/fcl/math/motion/screw_motion-inl.h @@ -92,7 +92,7 @@ ScrewMotion::ScrewMotion( //============================================================================== template -bool ScrewMotion::integrate(double dt) const +bool ScrewMotion::integrate(S dt) const { if(dt > 1) dt = 1; diff --git a/include/fcl/math/motion/screw_motion.h b/include/fcl/math/motion/screw_motion.h index fce33874e..1099ec385 100644 --- a/include/fcl/math/motion/screw_motion.h +++ b/include/fcl/math/motion/screw_motion.h @@ -65,7 +65,7 @@ class FCL_EXPORT ScrewMotion : public MotionBase /// @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& mb_visitor) const; diff --git a/include/fcl/narrowphase/continuous_collision-inl.h b/include/fcl/narrowphase/continuous_collision-inl.h index 1adfb6ec2..f8248e6bd 100644 --- a/include/fcl/narrowphase/continuous_collision-inl.h +++ b/include/fcl/narrowphase/continuous_collision-inl.h @@ -94,6 +94,7 @@ double collide( const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); + //============================================================================== template detail::ConservativeAdvancementFunctionMatrix& diff --git a/src/math/motion/interp_motion.cpp b/src/math/motion/interp_motion.cpp index aa5e592c9..88ee3a09d 100644 --- a/src/math/motion/interp_motion.cpp +++ b/src/math/motion/interp_motion.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class InterpMotion; + template class InterpMotion; diff --git a/src/math/motion/screw_motion.cpp b/src/math/motion/screw_motion.cpp index 65d1066ea..9b2fc9e3a 100644 --- a/src/math/motion/screw_motion.cpp +++ b/src/math/motion/screw_motion.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class ScrewMotion; + template class ScrewMotion; diff --git a/src/narrowphase/collision.cpp b/src/narrowphase/collision.cpp index ad5186158..74522e8c7 100644 --- a/src/narrowphase/collision.cpp +++ b/src/narrowphase/collision.cpp @@ -41,6 +41,13 @@ namespace fcl { //============================================================================== +template +std::size_t collide( + const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); + template std::size_t collide( const CollisionObject* o1, @@ -49,6 +56,15 @@ std::size_t collide( CollisionResult& result); //============================================================================== +template +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + template std::size_t collide( const CollisionGeometry* o1, diff --git a/src/narrowphase/collision_object.cpp b/src/narrowphase/collision_object.cpp index 6362db1f6..e8f336315 100644 --- a/src/narrowphase/collision_object.cpp +++ b/src/narrowphase/collision_object.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class CollisionObject; + template class CollisionObject; diff --git a/src/narrowphase/continuous_collision.cpp b/src/narrowphase/continuous_collision.cpp index bbbce19a9..f08156712 100644 --- a/src/narrowphase/continuous_collision.cpp +++ b/src/narrowphase/continuous_collision.cpp @@ -41,6 +41,15 @@ namespace fcl { //============================================================================== +template +float continuousCollide( + const CollisionGeometry* o1, + const MotionBase* motion1, + const CollisionGeometry* o2, + const MotionBase* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionGeometry* o1, @@ -51,6 +60,17 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float continuousCollide( + const CollisionGeometry* o1, + const Transform3& tf1_beg, + const Transform3& tf1_end, + const CollisionGeometry* o2, + const Transform3& tf2_beg, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionGeometry* o1, @@ -63,6 +83,15 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float continuousCollide( + const CollisionObject* o1, + const Transform3& tf1_end, + const CollisionObject* o2, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double continuousCollide( const CollisionObject* o1, @@ -73,6 +102,13 @@ double continuousCollide( ContinuousCollisionResult& result); //============================================================================== +template +float collide( + const ContinuousCollisionObject* o1, + const ContinuousCollisionObject* o2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + template double collide( const ContinuousCollisionObject* o1, diff --git a/src/narrowphase/continuous_collision_object.cpp b/src/narrowphase/continuous_collision_object.cpp index 93b767204..1f000385f 100644 --- a/src/narrowphase/continuous_collision_object.cpp +++ b/src/narrowphase/continuous_collision_object.cpp @@ -40,6 +40,9 @@ namespace fcl { +template +class ContinuousCollisionObject; + template class ContinuousCollisionObject; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 50e1c1c7b..c7d4af92e 100644 --- a/test/CMakeLists.txt +++ b/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) diff --git a/test/math/CMakeLists.txt b/test/math/CMakeLists.txt new file mode 100644 index 000000000..95692c7f1 --- /dev/null +++ b/test/math/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(motion) diff --git a/test/math/motion/CMakeLists.txt b/test/math/motion/CMakeLists.txt new file mode 100644 index 000000000..8d2686e02 --- /dev/null +++ b/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) diff --git a/test/math/motion/test_interp_motion.cpp b/test/math/motion/test_interp_motion.cpp new file mode 100644 index 000000000..12ac2be91 --- /dev/null +++ b/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 (sean@tri.global) (2020) */ + +#include "fcl/math/motion/interp_motion.h" + +#include + +#include "eigen_matrix_compare.h" + +namespace fcl { +namespace { + +// TODO(SeanCurtis-TRI): Convert this to a parameterized test harness and use +// TEST_P. + +template +class InterpMotionTest : public ::testing::Test {}; + +typedef testing::Types MyTypes; +TYPED_TEST_CASE(InterpMotionTest, MyTypes); + +TYPED_TEST(InterpMotionTest, Construction) { + using S = TypeParam; + const Transform3 I = Transform3::Identity(); + + { + // Default constructor; everything should be identity. + InterpMotion motion{}; + + Transform3 X_FA; + motion.getCurrentTransform(X_FA); + EXPECT_TRUE(CompareMatrices(X_FA.matrix(), I.matrix())); + EXPECT_TRUE( + CompareMatrices(motion.getLinearVelocity(), Vector3::Zero())); + EXPECT_EQ(motion.getAngularVelocity(), S(0)); + EXPECT_TRUE(CompareMatrices(motion.getAngularAxis(), Vector3::UnitX())); + EXPECT_TRUE( + CompareMatrices(motion.getReferencePoint(), Vector3::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(); +}