Commit 5952532e authored by Martin Felis's avatar Martin Felis
Browse files

added sphereCapsuleDistance()

parent d97dbed0
......@@ -302,6 +302,12 @@ template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-capsule distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
......
......@@ -100,16 +100,32 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
*contact_points = tf2.transform(segment_point + normal * distance);
}
/*
std::cout << "tf2 rot = " << tf2.getRotation() << std::endl;
std::cout << "distance = " << distance << std::endl;
std::cout << "segment_point = " << segment_point << std::endl;
std::cout << "diff = " << diff << std::endl;
std::cout << "normal(l)= " << (*normal_) << std::endl;
std::cout << "normal(w)= " << normal << std::endl;
std::cout << "contact = " << (*contact_points) << std::endl;
std::cout << "origin = " << tf2.transform(Vec3f()) << std::endl;
*/
return true;
}
bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist)
{
Transform3f tf2_inv (tf2);
tf2_inv.inverse();
Vec3f pos1 (0., 0., 0.5 * s2.lz);
Vec3f pos2 (0., 0., -0.5 * s2.lz);
Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
Vec3f segment_point;
lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
Vec3f diff = s_c - segment_point;
FCL_REAL distance = diff.length() - s1.radius - s2.radius;
if (distance <= 0)
return false;
if (dist)
*dist = distance;
return true;
}
......@@ -2540,6 +2556,14 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
}
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
......@@ -2747,6 +2771,14 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
}
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
}
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
......
......@@ -43,7 +43,7 @@
using namespace fcl;
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z)
{
GJKSolver_libccd solver;
......@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z_negative)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative)
{
GJKSolver_libccd solver;
......@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_z_negative)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_x)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x)
{
GJKSolver_libccd solver;
......@@ -85,7 +85,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_x)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_capsule_rotated)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated)
{
GJKSolver_libccd solver;
......@@ -101,7 +101,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_separated_capsule_rotated)
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z)
{
GJKSolver_libccd solver;
......@@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z)
BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z_rotated)
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
{
GJKSolver_libccd solver;
......@@ -148,3 +148,36 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_test_penetration_z_rotated)
BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
BOOST_CHECK (Vec3f (0., 0., 50.).equal(contact_point, solver.collision_tolerance));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 100));
BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 175));
FCL_REAL distance = 0.;
bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance);
BOOST_CHECK (is_separated);
BOOST_CHECK (distance == 25.);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment