Skip to content

GitLab

Commit e2bdabcb authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Update API of GJKSolver::shapeIntersect

parent c90803a0
Hide whitespace changes
Inline Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @ e2bdabcb
......@@ -56,7 +56,9 @@ namespace fcl
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance_lower_bound,
bool enable_penetration,
Vec3f* contact_points, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
......@@ -71,9 +73,11 @@ namespace fcl
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
if (!enable_penetration && contact_points == NULL && normal == NULL)
return true;
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = gjk.distance;
distance_lower_bound = gjk.distance;
if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
return true;
......@@ -86,16 +90,19 @@ namespace fcl
)
{
epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
distance_lower_bound = -epa.depth;
if(normal) *normal = tf1.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
if(penetration_depth) *penetration_depth = -std::numeric_limits<FCL_REAL>::max();
distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
// EPA failed but we know there is a collision so we should
return true;
}
break;
case details::GJK::Valid:
distance_lower_bound = gjk.distance;
break;
default:
;
}
......@@ -315,7 +322,8 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \
const Shape2& s2, const Transform3f& tf2, \
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance_lower_bound, bool enable_penetration, \
Vec3f* contact_points, Vec3f* normal) const
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
......
src/narrowphase/narrowphase.cpp
View file @ e2bdabcb
......@@ -78,69 +78,77 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \
const Shape2& s2, const Transform3f& tf2, \
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const \
FCL_REAL& distance_lower_bound, bool enable_penetration, \
Vec3f* contact_points, Vec3f* normal) const \
{ \
bool res = shapeIntersect (s2, tf2, s1, tf1, contact_points, \
penetration_depth, normal); \
bool res = shapeIntersect (s2, tf2, s1, tf1, distance_lower_bound, \
enable_penetration, contact_points, normal); \
(*normal) *= -1.0; \
return res; \
}
template<>
bool GJKSolver::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
const Capsule &s2, const Transform3f& tf2,
FCL_REAL& distance_lower_bound,
bool,
Vec3f* contact_points, Vec3f* normal) const
{
return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, distance_lower_bound,
contact_points, normal);
}
SHAPE_INTERSECT_INVERTED(Capsule, Sphere)
template<>
bool GJKSolver::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
const Sphere& s2, const Transform3f& tf2,
FCL_REAL& distance_lower_bound,
bool,
Vec3f* contact_points, Vec3f* normal) const
{
return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
return details::sphereSphereIntersect(s1, tf1, s2, tf2, distance_lower_bound,
contact_points, normal);
}
template<>
bool GJKSolver::shapeIntersect<Box, Sphere>(const Box & s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
bool GJKSolver::shapeIntersect<Box, Sphere>
(const Box & s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL dist;
Vec3f ps, pb, n;
bool intersect = details::boxSphereDistance (s1, tf1, s2, tf2, dist, ps, pb, n);
if (!intersect) return false;
if (penetration_depth) *penetration_depth = dist;
bool res = details::boxSphereDistance (s1, tf1, s2, tf2, distance, ps, pb, n);
if (normal) *normal = n;
if (contact_points) *contact_points = pb;
return true;
return res;
}
SHAPE_INTERSECT_INVERTED(Sphere, Box)
template<>
bool GJKSolver::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
const Box& s2, const Transform3f& tf2,
FCL_REAL& distance_lower_bound,
bool,
Vec3f* contact_points, Vec3f* normal) const
{
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, &distance_lower_bound, normal);
}
template<>
bool GJKSolver::shapeIntersect<Sphere, Halfspace>
(const Sphere& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -150,14 +158,14 @@ template<>
bool GJKSolver::shapeIntersect<Box, Halfspace>
(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -167,14 +175,14 @@ template<>
bool GJKSolver::shapeIntersect<Capsule, Halfspace>
(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::capsuleHalfspaceIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -184,14 +192,14 @@ template<>
bool GJKSolver::shapeIntersect<Cylinder, Halfspace>
(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::cylinderHalfspaceIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -201,41 +209,49 @@ template<>
bool GJKSolver::shapeIntersect<Cone, Halfspace>
(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::coneHalfspaceIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
SHAPE_INTERSECT_INVERTED(Halfspace, Cone)
template<>
bool GJKSolver::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
bool GJKSolver::shapeIntersect<Halfspace, Halfspace>
(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
FCL_REAL& distance, bool,
Vec3f* /*contact_points*/, Vec3f* /*normal*/) const
{
Halfspace s;
Vec3f p, d;
FCL_REAL depth;
int ret;
return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
bool res = details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
distance = - depth;
return res;
}
template<>
bool GJKSolver::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
bool GJKSolver::shapeIntersect<Plane, Halfspace>
(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
FCL_REAL& distance, bool,
Vec3f* /*contact_points*/, Vec3f* /*normal*/) const
{
Plane pl;
Vec3f p, d;
FCL_REAL depth;
int ret;
return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
bool res = details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
distance = - depth;
return res;
}
SHAPE_INTERSECT_INVERTED(Halfspace, Plane)
......@@ -244,14 +260,14 @@ template<>
bool GJKSolver::shapeIntersect<Sphere, Plane>
(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1,
p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -261,14 +277,14 @@ template<>
bool GJKSolver::shapeIntersect<Box, Plane>
(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::boxPlaneIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -278,14 +294,14 @@ template<>
bool GJKSolver::shapeIntersect<Capsule, Plane>
(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1,
p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -295,14 +311,14 @@ template<>
bool GJKSolver::shapeIntersect<Cylinder, Plane>
(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::cylinderPlaneIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
......@@ -312,25 +328,27 @@ template<>
bool GJKSolver::shapeIntersect<Cone, Plane>
(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
FCL_REAL distance;
Vec3f p1, p2;
Vec3f p1, p2, n;
bool res = details::conePlaneIntersect
(s1, tf1, s2, tf2, distance, p1, p2, *normal);
*contact_points = p1;
*penetration_depth = -distance;
(s1, tf1, s2, tf2, distance, p1, p2, n);
if (contact_points) *contact_points = p1;
if (normal) *normal = n;
return res;
}
SHAPE_INTERSECT_INVERTED(Plane, Cone)
template<>
bool GJKSolver::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
bool GJKSolver::shapeIntersect<Plane, Plane>
(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
FCL_REAL& distance, bool,
Vec3f* contact_points, Vec3f* normal) const
{
return details::planeIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance, normal);
}
......
Markdown is supported
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