Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Change shape-shape intersection API to return multiple contacts #52

Merged
merged 15 commits into from
Nov 22, 2015
Merged

Change shape-shape intersection API to return multiple contacts #52

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
66d678b
Change shape-shape intersection API to return multiple contacts
jslee02 Mar 6, 2015
33ff703
Fix bug in adding node-node contact result to the total contact result
jslee02 Mar 17, 2015
c4fe4d9
Merge remote-tracking branch 'upstream/master' into shape_shape_colli…
jslee02 Apr 5, 2015
3cd9219
Merge remote-tracking branch 'upstream/master' into shape_shape_colli…
jslee02 May 4, 2015
4f5b857
Assign default values of query information for shape[~][Intersect/Dis…
jslee02 Jun 2, 2015
3c4954f
Change contact point comparison function to follow the convention: re…
jslee02 Jun 2, 2015
e9d9c44
Merge remote-tracking branch 'origin/master' into shape_shape_collisi…
jslee02 Oct 16, 2015
2eb2a49
Update shape collision test suit for multiple contacts -- only sphere…
jslee02 Oct 20, 2015
6247ed0
Fix normal direction of boxbox collision to point to second object
jslee02 Oct 20, 2015
60f380d
Update boxbox intersection tests
jslee02 Oct 20, 2015
17f72c8
Update other shape-shape intersection tests
jslee02 Oct 20, 2015
a14dd80
Update plane-[shape] intersection tests
jslee02 Oct 21, 2015
10abbd2
Update all the rest intersection tests
jslee02 Oct 21, 2015
d083df2
Give more tolerance for clang build
jslee02 Oct 21, 2015
75b59e6
Remove a legacy function in test_fcl_geometric_shapes.cpp
jslee02 Oct 21, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 22 additions & 0 deletions include/fcl/collision_data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,28 @@ namespace fcl
/// @brief Type of narrow phase GJK solver
enum GJKSolverType {GST_LIBCCD, GST_INDEP};

/// @brief Minimal contact information returned by collision
struct ContactPoint
{
/// @brief Contact normal, pointing from o1 to o2
Vec3f normal;

/// @brief Contact position, in world space
Vec3f pos;

/// @brief Penetration depth
FCL_REAL penetration_depth;

/// @brief Constructor
ContactPoint(const Vec3f& n_, const Vec3f& p_, FCL_REAL d_) : normal(n_),
pos(p_),
penetration_depth(d_)
{}
};

/// @brief Return true if _cp1's penentration depth is greater than _cp2's.
bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The comparison functions used by std::min_element and std::max_element return true if the first element is less than the second. The logic of this function appears to be the opposite of that. It would be nice to match the existing convention.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's intended. This comparison function is used to partially sort contact points in descending order using partial_sort(). I couldn't find a way to do it without passing this customized greater-like function.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Now I learned a way using boost::bind. The comparison function was fixed to follow the existing convention in 3c4954f.


/// @brief Contact information returned by collision
struct Contact
{
Expand Down
257 changes: 181 additions & 76 deletions include/fcl/narrowphase/narrowphase.h
View file
Open in desktop

Large diffs are not rendered by default.

63 changes: 46 additions & 17 deletions include/fcl/traversal/traversal_node_octree.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -298,14 +298,14 @@ class OcTreeSolver
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);

if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL))
{
AABB overlap_part;
AABB aabb1, aabb2;
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources);
cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources);
}
}

Expand All @@ -326,7 +326,7 @@ class OcTreeSolver
bool is_intersect = false;
if(!crequest->enable_contact)
{
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL))
{
is_intersect = true;
if(cresult->numContacts() < crequest->num_max_contacts)
Expand All @@ -335,15 +335,29 @@ class OcTreeSolver
}
else
{
Vec3f contact;
FCL_REAL depth;
Vec3f normal;

if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
std::vector<ContactPoint> contacts;
if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts))
{
is_intersect = true;
if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth));
if(crequest->num_max_contacts > cresult->numContacts())
{
const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
size_t num_adding_contacts;

// If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
if (free_space < contacts.size())
{
std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), comparePenDepth);
num_adding_contacts = free_space;
}
else
{
num_adding_contacts = contacts.size();
}

for(size_t i = 0; i < num_adding_contacts; ++i)
cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
}
}
}

Expand All @@ -370,7 +384,7 @@ class OcTreeSolver
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);

if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL))
{
AABB overlap_part;
AABB aabb1, aabb2;
Expand Down Expand Up @@ -893,14 +907,29 @@ class OcTreeSolver
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);

Vec3f contact;
FCL_REAL depth;
Vec3f normal;
if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
std::vector<ContactPoint> contacts;
if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts))
{
is_intersect = true;
if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
if(crequest->num_max_contacts > cresult->numContacts())
{
const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
size_t num_adding_contacts;

// If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
if (free_space < contacts.size())
{
std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), comparePenDepth);
num_adding_contacts = free_space;
}
else
{
num_adding_contacts = contacts.size();
}

for(size_t i = 0; i < num_adding_contacts; ++i)
cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
}
}
}

Expand Down
31 changes: 24 additions & 7 deletions include/fcl/traversal/traversal_node_shapes.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@
#ifndef FCL_TRAVERSAL_NODE_SHAPES_H
#define FCL_TRAVERSAL_NODE_SHAPES_H

#include <algorithm>

#include "fcl/collision_data.h"
#include "fcl/traversal/traversal_node_base.h"
#include "fcl/narrowphase/narrowphase.h"
Expand All @@ -50,7 +52,6 @@
namespace fcl
{


/// @brief Traversal node for collision between two shapes
template<typename S1, typename S2, typename NarrowPhaseSolver>
class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
Expand Down Expand Up @@ -78,18 +79,34 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
bool is_collision = false;
if(request.enable_contact)
{
Vec3f contact_point, normal;
FCL_REAL penetration_depth;
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
std::vector<ContactPoint> contacts;
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contacts))
{
is_collision = true;
if(request.num_max_contacts > result->numContacts())
result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
{
const size_t free_space = request.num_max_contacts - result->numContacts();
size_t num_adding_contacts;

// If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
if (free_space < contacts.size())
{
std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), comparePenDepth);
num_adding_contacts = free_space;
}
else
{
num_adding_contacts = contacts.size();
}

for(size_t i = 0; i < num_adding_contacts; ++i)
result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
}
}
}
else
{
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL))
{
is_collision = true;
if(request.num_max_contacts > result->numContacts())
Expand All @@ -109,7 +126,7 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
}
else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
{
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL))
{
AABB aabb1, aabb2;
computeBV<AABB, S1>(*model1, tf1, aabb1);
Expand Down
4 changes: 2 additions & 2 deletions src/broadphase/broadphase_dynamic_AABB_tree.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -749,7 +749,7 @@ void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata,
{
if(!octree_as_geometry_collide)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
const OcTree* octree = static_cast<const OcTree*>(obj->collisionGeometry().get());
details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
}
else
Expand All @@ -773,7 +773,7 @@ void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata
{
if(!octree_as_geometry_distance)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
const OcTree* octree = static_cast<const OcTree*>(obj->collisionGeometry().get());
details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
}
else
Expand Down
4 changes: 2 additions & 2 deletions src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -774,7 +774,7 @@ void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void*
{
if(!octree_as_geometry_collide)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
const OcTree* octree = static_cast<const OcTree*>(obj->collisionGeometry().get());
details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
}
else
Expand All @@ -798,7 +798,7 @@ void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void*
{
if(!octree_as_geometry_distance)
{
const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
const OcTree* octree = static_cast<const OcTree*>(obj->collisionGeometry().get());
details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
}
else
Expand Down
5 changes: 5 additions & 0 deletions src/collision_data.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,11 @@
namespace fcl
{

bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2)
{
return (_cp1.penetration_depth > _cp2.penetration_depth);
}

bool CollisionRequest::isSatisfied(const CollisionResult& result) const
{
return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts());
Expand Down
Loading