From 66d678b2a455298328350512cdc9cb7169435bb3 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 6 Mar 2015 17:06:43 -0500 Subject: [PATCH 01/12] Change shape-shape intersection API to return multiple contacts --- include/fcl/collision_data.h | 22 + include/fcl/narrowphase/narrowphase.h | 207 ++- include/fcl/traversal/traversal_node_octree.h | 57 +- include/fcl/traversal/traversal_node_shapes.h | 30 +- .../broadphase_dynamic_AABB_tree.cpp | 4 +- .../broadphase_dynamic_AABB_tree_array.cpp | 4 +- src/collision_data.cpp | 5 + src/narrowphase/narrowphase.cpp | 619 +++++---- test/test_fcl_collision.cpp | 10 +- test/test_fcl_geometric_shapes.cpp | 1121 +++++++++++------ test/test_fcl_sphere_capsule.cpp | 48 +- 11 files changed, 1349 insertions(+), 778 deletions(-) diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h index 5ea511434..792e58da6 100644 --- a/include/fcl/collision_data.h +++ b/include/fcl/collision_data.h @@ -55,6 +55,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); + /// @brief Contact information returned by collision struct Contact { diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h index 5abf8acf6..a2db7e824 100644 --- a/include/fcl/narrowphase/narrowphase.h +++ b/include/fcl/narrowphase/narrowphase.h @@ -37,32 +37,53 @@ #ifndef FCL_NARROWPHASE_H #define FCL_NARROWPHASE_H +#include + +#include "fcl/collision_data.h" #include "fcl/narrowphase/gjk.h" #include "fcl/narrowphase/gjk_libccd.h" - - namespace fcl { - - - /// @brief collision and distance solver based on libccd library. struct GJKSolver_libccd { /// @brief intersection checking between two shapes + /// @deprecated use shapeIntersect(const S1&, const Transform3f&, const S2&, const Transform3f&, std::vector*) const template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const FCL_DEPRECATED; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + std::vector* contacts) const { void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); - bool res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - max_collision_iterations, collision_tolerance, - contact_points, penetration_depth, normal); + bool res; + + if(contacts) + { + Vec3f normal; + Vec3f point; + FCL_REAL depth; + res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), + o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), + max_collision_iterations, collision_tolerance, + &point, &depth, &normal); + contacts->push_back(ContactPoint(normal, point, depth)); + } + else + { + res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), + o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), + max_collision_iterations, collision_tolerance, + NULL, NULL, NULL); + } details::GJKInitializer::deleteGJKObject(o1); details::GJKInitializer::deleteGJKObject(o2); @@ -242,94 +263,129 @@ struct GJKSolver_libccd }; +template +bool GJKSolver_libccd::shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const +{ + bool res; + + if (contact_points || penetration_depth || normal) + { + std::vector contacts; + + res = shapeIntersect(s1, tf1, s2, tf2, &contacts); + + if (!contacts.empty()) + { + // Get the deepest contact point + const ContactPoint& maxDepthContact = (*std::min_element(contacts.begin(), contacts.end(), &comparePenDepth)); + + if (contact_points) + *contact_points = maxDepthContact.pos; + + if (penetration_depth) + *penetration_depth = maxDepthContact.penetration_depth; + + if (normal) + *normal = maxDepthContact.normal; + } + } + else + { + res = shapeIntersect(s1, tf1, s2, tf2, NULL); + } + + return res; +} /// @brief Fast implementation for sphere-capsule collision template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; /// @brief Fast implementation for sphere-sphere collision template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; -/// @brief Fast implementation for box-box collision +/// @brief Fast implementation for box-box collision template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; /// @brief Fast implementation for sphere-triangle collision template<> @@ -384,24 +440,31 @@ bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const struct GJKSolver_indep { /// @brief intersection checking between two shapes + /// @deprecated use shapeIntersect(const S1&, const Transform3f&, const S2&, const Transform3f&, std::vector*) const template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const FCL_DEPRECATED; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + std::vector* contacts) const { Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; - + details::MinkowskiDiff shape; shape.shapes[0] = &s1; shape.shapes[1] = &s2; shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation()); shape.toshape0 = tf1.inverseTimes(tf2); - + details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - + switch(gjk_status) { case details::GJK::Inside: @@ -415,9 +478,13 @@ struct GJKSolver_indep { w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; } - if(penetration_depth) *penetration_depth = -epa.depth; - if(normal) *normal = -epa.normal; - if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); + if(contacts) + { + Vec3f normal = -epa.normal; + Vec3f point = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); + FCL_REAL depth = -epa.depth; + contacts->push_back(ContactPoint(normal, point, depth)); + } return true; } else return false; @@ -732,92 +799,128 @@ struct GJKSolver_indep mutable Vec3f cached_guess; }; +template +bool GJKSolver_indep::shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const +{ + bool res; + + if (contact_points || penetration_depth || normal) + { + std::vector contacts; + + res = shapeIntersect(s1, tf1, s2, tf2, &contacts); + + if (!contacts.empty()) + { + // Get the deepest contact point + const ContactPoint& maxDepthContact = (*std::min_element(contacts.begin(), contacts.end(), &comparePenDepth)); + + if (contact_points) + *contact_points = maxDepthContact.pos; + + if (penetration_depth) + *penetration_depth = maxDepthContact.penetration_depth; + + if (normal) + *normal = maxDepthContact.normal; + } + } + else + { + res = shapeIntersect(s1, tf1, s2, tf2, NULL); + } + + return res; +} + template<> bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; /// @brief Fast implementation for sphere-sphere collision template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; /// @brief Fast implementation for box-box collision template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; + std::vector* contacts) const; /// @brief Fast implementation for sphere-triangle collision template<> diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index 3ef5da228..e2b2a7e7a 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -297,14 +297,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(box, box_tf, aabb1); computeBV(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); } } @@ -325,7 +325,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) @@ -334,15 +334,26 @@ class OcTreeSolver } else { - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - - if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal)) + std::vector 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)); + 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, then contacts of greater penetration are added first. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.end() + 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)); } } @@ -369,7 +380,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; @@ -892,14 +903,26 @@ 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 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)); + 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, then contacts of greater penetration are added first. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.end() + 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)); } } diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h index fb2c40622..9c81dcf74 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -38,6 +38,8 @@ #ifndef FCL_TRAVERSAL_NODE_SHAPES_H #define FCL_TRAVERSAL_NODE_SHAPES_H +#include + #include "fcl/collision_data.h" #include "fcl/traversal/traversal_node_base.h" #include "fcl/narrowphase/narrowphase.h" @@ -49,7 +51,6 @@ namespace fcl { - /// @brief Traversal node for collision between two shapes template class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase @@ -77,18 +78,31 @@ 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 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, then contacts of greater penetration are added first. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.end() + 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()) @@ -108,7 +122,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(*model1, tf1, aabb1); diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 69dc7d798..16dea44e2 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -746,7 +746,7 @@ void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, { if(!octree_as_geometry_collide) { - const OcTree* octree = static_cast(obj->getCollisionGeometry()); + const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); } else @@ -770,7 +770,7 @@ void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata { if(!octree_as_geometry_distance) { - const OcTree* octree = static_cast(obj->getCollisionGeometry()); + const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); } else diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index c83645170..b2a00fcbd 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -771,7 +771,7 @@ void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* { if(!octree_as_geometry_collide) { - const OcTree* octree = static_cast(obj->getCollisionGeometry()); + const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); } else @@ -795,7 +795,7 @@ void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* { if(!octree_as_geometry_distance) { - const OcTree* octree = static_cast(obj->getCollisionGeometry()); + const OcTree* octree = static_cast(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 diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 1d7a0bc1c..6840c9c60 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -39,6 +39,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()); diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index b701a7ea9..c012d3a29 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -196,35 +196,34 @@ static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f & bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_) + std::vector* contacts) { - Transform3f tf2_inv (tf2); + 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())); + const Vec3f pos1(0., 0., 0.5 * s2.lz); + const Vec3f pos2(0., 0., -0.5 * s2.lz); + const 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; + const FCL_REAL distance = diff.length() - s1.radius - s2.radius; if (distance > 0) return false; - Vec3f normal = diff.normalize() * - FCL_REAL(1); + const Vec3f local_normal = diff.normalize() * - FCL_REAL(1); - if (distance < 0 && penetration_depth) - *penetration_depth = -distance; - - if (normal_) - *normal_ = tf2.getQuatRotation().transform(normal); + if (contacts) + { + const Vec3f normal = tf2.getQuatRotation().transform(local_normal); + const Vec3f point = tf2.transform(segment_point + local_normal * distance); + const FCL_REAL penetration_depth = -distance; - if (contact_points) { - *contact_points = tf2.transform(segment_point + normal * distance); + contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; @@ -265,32 +264,26 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, return true; } -bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, +bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f()); FCL_REAL len = diff.length(); if(len > s1.radius + s2.radius) return false; - if(penetration_depth) - *penetration_depth = s1.radius + s2.radius - len; - if(normal) + if(contacts) { - if(len > 0) - *normal = diff / len; - else - *normal = diff; + const Vec3f normal = len > 0 ? diff / len : diff; + const Vec3f point = tf1.transform(Vec3f()) - diff * s1.radius / (s1.radius + s2.radius); + const FCL_REAL penetration_depth = s1.radius + s2.radius - len; + contacts->push_back(ContactPoint(normal, point, penetration_depth)); } - if(contact_points) - *contact_points = tf1.transform(Vec3f()) - diff * s1.radius / (s1.radius + s2.radius); - return true; } - bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) @@ -731,17 +724,6 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1, return res; } - - -struct ContactPoint -{ - Vec3f normal; - Vec3f point; - FCL_REAL depth; - ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {} -}; - - static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua, const Vec3f& pb, const Vec3f& ub, FCL_REAL* alpha, FCL_REAL* beta) @@ -957,20 +939,20 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, tmp = pp[0]; s2 = std::abs(tmp) - (Q.dotX(B) + A[0]); if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) + if(s2 > s) { - s = s2; + s = s2; best_col_id = 0; invert_normal = (tmp < 0); code = 1; } - tmp = pp[1]; + tmp = pp[1]; s2 = std::abs(tmp) - (Q.dotY(B) + A[1]); if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) + if(s2 > s) { - s = s2; + s = s2; best_col_id = 1; invert_normal = (tmp < 0); code = 2; @@ -1017,7 +999,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, best_col_id = 2; code = 6; } - + FCL_REAL fudge2(1.0e-6); Q += fudge2; @@ -1031,7 +1013,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 0), R(1, 0)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1049,7 +1031,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 1), R(1, 1)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1061,13 +1043,13 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, code = 8; } } - + tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 2), R(1, 2)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1086,7 +1068,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(R(2, 0), 0, -R(0, 0)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1104,7 +1086,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(R(2, 1), 0, -R(0, 1)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1116,13 +1098,13 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, code = 11; } } - + tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(R(2, 2), 0, -R(0, 2)); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1141,7 +1123,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(-R(1, 0), R(0, 0), 0); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1159,7 +1141,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(-R(1, 1), R(0, 1), 0); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1171,13 +1153,13 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, code = 14; } } - + tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); if(s2 > eps) { *return_code = 0; return 0; } n = Vec3f(-R(1, 2), R(0, 2), 0); l = n.length(); - if(l > eps) + if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) @@ -1191,36 +1173,36 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, } - + if (!code) { *return_code = code; return 0; } // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. - if(best_col_id != -1) + if(best_col_id != -1) normal = R.getColumn(best_col_id); - else + else normal = R1 * normalC; - - if(invert_normal) + + if(invert_normal) normal.negate(); *depth = -s; // compute contact point(s) - if(code > 6) + if(code > 6) { // an edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 Vec3f pa(T1); FCL_REAL sign; - + for(int j = 0; j < 3; ++j) { sign = (R1.transposeDot(j, normal) > 0) ? 1 : -1; pa += R1.getColumn(j) * (A[j] * sign); } - + // find a point pb on the intersecting edge of box 2 Vec3f pb; pb = T2; @@ -1233,16 +1215,16 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, FCL_REAL alpha, beta; Vec3f ua(R1.getColumn((code-7)/3)); Vec3f ub(R2.getColumn((code-7)%3)); - + lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); pa += ua * alpha; pb += ub * beta; - - + + Vec3f pointInWorld((pa + pb) * 0.5); contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); *return_code = code; - + return 1; } @@ -1250,7 +1232,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). - + const Matrix3f *Ra, *Rb; const Vec3f *pa, *pb, *Sa, *Sb; @@ -1276,9 +1258,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. Vec3f normal2, nr, anr; - if(code <= 3) + if(code <= 3) normal2 = normal; - else + else normal2 = -normal; nr = Rb->transposeTimes(normal2); @@ -1288,30 +1270,30 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, // for the indident face. the other axis numbers of the indicent face // are stored in a1,a2. int lanr, a1, a2; - if(anr[1] > anr[0]) + if(anr[1] > anr[0]) { - if(anr[1] > anr[2]) + if(anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } - else + else { a1 = 0; a2 = 1; lanr = 2; } } - else + else { - if(anr[0] > anr[2]) + if(anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } - else + else { a1 = 0; a2 = 1; @@ -1321,28 +1303,28 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, // compute center point of incident face, in reference-face coordinates Vec3f center; - if(nr[lanr] < 0) + if(nr[lanr] < 0) center = (*pb) - (*pa) + Rb->getColumn(lanr) * ((*Sb)[lanr]); else center = (*pb) - (*pa) - Rb->getColumn(lanr) * ((*Sb)[lanr]); // find the normal and non-normal axis numbers of the reference box int codeN, code1, code2; - if(code <= 3) - codeN = code-1; + if(code <= 3) + codeN = code-1; else codeN = code-4; - - if(codeN == 0) + + if(codeN == 0) { code1 = 1; code2 = 2; } - else if(codeN == 1) + else if(codeN == 1) { code1 = 0; code2 = 2; } - else + else { code1 = 0; code2 = 1; @@ -1362,7 +1344,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, tempRac = Ra->getColumn(code2); m21 = Rb->transposeDot(a1, tempRac); m22 = Rb->transposeDot(a2, tempRac); - + FCL_REAL k1 = m11 * (*Sb)[a1]; FCL_REAL k2 = m21 * (*Sb)[a1]; FCL_REAL k3 = m12 * (*Sb)[a2]; @@ -1398,13 +1380,13 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, m21 *= det1; m22 *= det1; int cnum = 0; // number of penetrating contact points found - for(int j = 0; j < n_intersect; ++j) + for(int j = 0; j < n_intersect; ++j) { FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2; dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); - if(dep[cnum] >= 0) + if(dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; @@ -1417,36 +1399,36 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(maxc > cnum) maxc = cnum; if(maxc < 1) maxc = 1; - if(cnum <= maxc) + if(cnum <= maxc) { - if(code<4) + if(code<4) { // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) + for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa); contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); } - } + } else { // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) + for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); } } } - else + else { // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; FCL_REAL maxdepth = dep[0]; - for(int i = 1; i < cnum; ++i) + for(int i = 1; i < cnum; ++i) { - if(dep[i] > maxdepth) + if(dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; @@ -1456,7 +1438,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, int iret[8]; cullPoints2(cnum, ret, maxc, i1, iret); - for(int j = 0; j < maxc; ++j) + for(int j = 0; j < maxc; ++j) { Vec3f posInWorld = points[iret[j] * 3] + (*pa); if(code < 4) @@ -1471,14 +1453,12 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, return cnum; } - - bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_) + std::vector* contacts_) { std::vector contacts; - int return_code; + int return_code; Vec3f normal; FCL_REAL depth; /* int cnum = */ boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(), @@ -1486,21 +1466,8 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, normal, &depth, &return_code, 4, contacts); - if(normal_) *normal_ = normal; - if(penetration_depth_) *penetration_depth_ = depth; - - if(contact_points) - { - Vec3f contact_point; - for(size_t i = 0; i < contacts.size(); ++i) - { - contact_point += contacts[i].point; - } - - contact_point = contact_point / (FCL_REAL)contacts.size(); - - *contact_points = contact_point; - } + if(contacts_) + *contacts_ = contacts; return return_code != 0; } @@ -1525,20 +1492,29 @@ double halfspaceIntersectTolerance() bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { - Halfspace new_s2 = transform(s2, tf2); + const Halfspace new_s2 = transform(s2, tf2); const Vec3f& center = tf1.getTranslation(); - FCL_REAL depth = s1.radius - new_s2.signedDistance(center); - if(depth >= 0) + const FCL_REAL depth = s1.radius - new_s2.signedDistance(center); + + if (depth >= 0) { - if(normal) *normal = -new_s2.n; // pointing from s1 to s2 - if(penetration_depth) *penetration_depth = depth; - if(contact_points) *contact_points = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5); + if (contacts) + { + const Vec3f normal = -new_s2.n; // pointing from s1 to s2 + const Vec3f point = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } + return true; } else + { return false; + } } /// @brief box half space, a, b, c = +/- edge size @@ -1565,13 +1541,15 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { - if(!contact_points && !penetration_depth && !normal) + if(!contacts) + { return boxHalfspaceIntersect(s1, tf1, s2, tf2); + } else { - Halfspace new_s2 = transform(s2, tf2); + const Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); @@ -1617,9 +1595,14 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, } /// compute the contact point from the deepest point - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = p + new_s2.n * (depth * 0.5); + if (contacts) + { + const Vec3f normal = -new_s2.n; + const Vec3f point = p + new_s2.n * (depth * 0.5); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } @@ -1627,7 +1610,7 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); @@ -1643,9 +1626,14 @@ bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - s1.radius); + if (contacts) + { + const Vec3f normal = -new_s2.n; + const Vec3f point = T + new_s2.n * (0.5 * depth - s1.radius); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } @@ -1658,13 +1646,13 @@ bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) + if (contacts) { - // deepest point - Vec3f c = p - new_s2.n * s1.radius; - *contact_points = c + new_s2.n * (0.5 * depth); + const Vec3f normal = -new_s2.n; + const Vec3f point = p - new_s2.n * s1.radius + new_s2.n * (0.5 * depth); // deepest point + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; @@ -1674,7 +1662,7 @@ bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); @@ -1690,9 +1678,14 @@ bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1, FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - s1.radius); + if (contacts) + { + const Vec3f normal = -new_s2.n; + const Vec3f point = T + new_s2.n * (0.5 * depth - s1.radius); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } @@ -1715,9 +1708,15 @@ bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1, if(depth < 0) return false; else { - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = p + new_s2.n * (0.5 * depth); + if (contacts) + { + const Vec3f normal = -new_s2.n; + const Vec3f point = p + new_s2.n * (0.5 * depth); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } + return true; } } @@ -1726,7 +1725,7 @@ bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1, bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); @@ -1743,9 +1742,15 @@ bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, if(depth < 0) return false; else { - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius); + if (contacts) + { + const Vec3f normal = -new_s2.n; + const Vec3f point = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } + return true; } } @@ -1770,10 +1775,15 @@ bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, if(d1 > 0 && d2 > 0) return false; else { - FCL_REAL depth = -std::min(d1, d2); - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * depth); + if (contacts) + { + const FCL_REAL penetration_depth = -std::min(d1, d2); + const Vec3f normal = -new_s2.n; + const Vec3f point = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * penetration_depth); + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } + return true; } } @@ -1994,23 +2004,31 @@ float planeIntersectTolerance() bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { - Plane new_s2 = transform(s2, tf2); + const Plane new_s2 = transform(s2, tf2); const Vec3f& center = tf1.getTranslation(); - FCL_REAL signed_dist = new_s2.signedDistance(center); - FCL_REAL depth = - std::abs(signed_dist) + s1.radius; + const FCL_REAL signed_dist = new_s2.signedDistance(center); + const FCL_REAL depth = - std::abs(signed_dist) + s1.radius; + if(depth >= 0) { - if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; - if(penetration_depth) *penetration_depth = depth; - if(contact_points) *contact_points = center - new_s2.n * signed_dist; + if (contacts) + { + const Vec3f normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; + const Vec3f point = center - new_s2.n * signed_dist; + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } else + { return false; + } } /// @brief box half space, a, b, c = +/- edge size @@ -2022,7 +2040,7 @@ bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1, /// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Plane new_s2 = transform(s2, tf2); @@ -2078,9 +2096,14 @@ bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, } // compute the contact point by project the deepest point onto the plane - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; - if(contact_points) *contact_points = p - new_s2.n * new_s2.signedDistance(p); + if (contacts) + { + const Vec3f normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; + const Vec3f point = p - new_s2.n * new_s2.signedDistance(p); + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } @@ -2111,12 +2134,12 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { - Plane new_s2 = transform(s2, tf2); - - if(!contact_points && !penetration_depth && !normal) + if(!contacts) + { return capsulePlaneIntersect(s1, tf1, s2, tf2); + } else { Plane new_s2 = transform(s2, tf2); @@ -2144,46 +2167,60 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, { if(abs_d1 < abs_d2) { - if(penetration_depth) *penetration_depth = abs_d1 + s1.radius; - if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); - if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n; + if (contacts) + { + const Vec3f normal = (d1 < 0) ? -new_s2.n : new_s2.n; + const Vec3f point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); + const FCL_REAL penetration_depth = abs_d1 + s1.radius; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } } else { - if(penetration_depth) *penetration_depth = abs_d2 + s1.radius; - if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); - if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n; + if (contacts) + { + const Vec3f normal = (d2 < 0) ? -new_s2.n : new_s2.n; + const Vec3f point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); + const FCL_REAL penetration_depth = abs_d2 + s1.radius; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } } return true; } if(abs_d1 > s1.radius && abs_d2 > s1.radius) + { return false; + } else { - if(penetration_depth) *penetration_depth = s1.radius - std::min(abs_d1, abs_d2); - - if(contact_points) + if (contacts) { + const Vec3f normal = (d1 < 0) ? new_s2.n : -new_s2.n; + const FCL_REAL penetration_depth = s1.radius - std::min(abs_d1, abs_d2); + Vec3f point; if(abs_d1 <= s1.radius && abs_d2 <= s1.radius) { - Vec3f c1 = p1 - new_s2.n * d2; - Vec3f c2 = p2 - new_s2.n * d1; - *contact_points = (c1 + c2) * 0.5; + const Vec3f c1 = p1 - new_s2.n * d2; + const Vec3f c2 = p2 - new_s2.n * d1; + point = (c1 + c2) * 0.5; } else if(abs_d1 <= s1.radius) { - Vec3f c = p1 - new_s2.n * d1; - *contact_points = c; + const Vec3f c = p1 - new_s2.n * d1; + point = c; } else if(abs_d2 <= s1.radius) { - Vec3f c = p2 - new_s2.n * d2; - *contact_points = c; + const Vec3f c = p2 - new_s2.n * d2; + point = c; } + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); } - - if(normal) *normal = (d1 < 0) ? new_s2.n : -new_s2.n; + return true; } } @@ -2216,10 +2253,12 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { - if(!contact_points && !penetration_depth && !normal) + if(!contacts) + { return cylinderPlaneIntersect(s1, tf1, s2, tf2); + } else { Plane new_s2 = transform(s2, tf2); @@ -2237,9 +2276,14 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, if(depth < 0) return false; else { - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n; - if(contact_points) *contact_points = T - new_s2.n * d; + if (contacts) + { + const Vec3f normal = (d < 0) ? new_s2.n : -new_s2.n; + const Vec3f point = T - new_s2.n * d; + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } return true; } } @@ -2280,27 +2324,39 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, if(abs_d1 > abs_d2) { - if(penetration_depth) *penetration_depth = abs_d2; - if(contact_points) *contact_points = c2 - new_s2.n * d2; - if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n; + if (contacts) + { + const Vec3f normal = (d2 < 0) ? -new_s2.n : new_s2.n; + const Vec3f point = c2 - new_s2.n * d2; + const FCL_REAL penetration_depth = abs_d2; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } } else { - if(penetration_depth) *penetration_depth = abs_d1; - if(contact_points) *contact_points = c1 - new_s2.n * d1; - if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n; + if (contacts) + { + const Vec3f normal = (d1 < 0) ? -new_s2.n : new_s2.n; + const Vec3f point = c1 - new_s2.n * d1; + const FCL_REAL penetration_depth = abs_d1; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } } return true; } else + { return false; + } } } } bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* contacts) { Plane new_s2 = transform(s2, tf2); @@ -2317,9 +2373,15 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, if(depth < 0) return false; else { - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n; - if(contact_points) *contact_points = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d; + if (contacts) + { + const Vec3f normal = (d < 0) ? new_s2.n : -new_s2.n; + const Vec3f point = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d; + const FCL_REAL penetration_depth = depth; + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); + } + return true; } } @@ -2368,41 +2430,46 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, } } - if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative); - if(normal) *normal = (d_positive > d_negative) ? -new_s2.n : new_s2.n; - if(contact_points) + if (contacts) { + const Vec3f normal = (d_positive > d_negative) ? -new_s2.n : new_s2.n; + const FCL_REAL penetration_depth = std::min(d_positive, d_negative); + + Vec3f point; Vec3f p[2]; Vec3f q; - + FCL_REAL p_d[2]; FCL_REAL q_d(0); if(n_positive == 2) - { + { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } - Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); - Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); - *contact_points = (t1 + t2) * 0.5; + const Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); + const Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); + point = (t1 + t2) * 0.5; } else - { + { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } - Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); - Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); - *contact_points = (t1 + t2) * 0.5; + const Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); + const Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); + point = (t1 + t2) * 0.5; } + + contacts->push_back(ContactPoint(normal, point, penetration_depth)); } + return true; } } @@ -2537,7 +2604,7 @@ bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1, bool planeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) + std::vector* /*contacts*/) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); @@ -2558,71 +2625,71 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1, template<> bool GJKSolver_libccd::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Halfspace s; Vec3f p, d; @@ -2634,7 +2701,7 @@ bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Plane pl; Vec3f p, d; @@ -2646,47 +2713,47 @@ bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const T template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::conePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Plane pl; Vec3f p, d; @@ -2698,9 +2765,9 @@ bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, con template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::planeIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::planeIntersect(s1, tf1, s2, tf2, contacts); } @@ -2775,71 +2842,71 @@ bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Tran template<> bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Halfspace s; Vec3f p, d; @@ -2852,7 +2919,7 @@ bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Plane pl; Vec3f p, d; @@ -2864,47 +2931,47 @@ bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Tr template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::conePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { Plane pl; Vec3f p, d; @@ -2916,9 +2983,9 @@ bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, cons template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - return details::planeIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal); + return details::planeIntersect(s1, tf1, s2, tf2, contacts); } diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index d69b93ea7..25fcf24e7 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -115,7 +115,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) GJKSolver_libccd solver; bool overlap_obb = obb1.overlap(obb2); - bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL); + bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL); BOOST_CHECK(overlap_obb == overlap_box); } @@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) computeBV(sphere, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); - bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL, NULL, NULL); + bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_sphere); } @@ -163,7 +163,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) computeBV(capsule, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); - bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL, NULL, NULL); + bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_capsule); } @@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) computeBV(cone, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); - bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL, NULL, NULL); + bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_cone); } @@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) computeBV(cylinder, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); - bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL, NULL, NULL); + bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_cylinder); } } diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 624fff11c..0fbcc059d 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -51,6 +51,16 @@ FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10}; GJKSolver_libccd solver1; GJKSolver_indep solver2; +void getContactInfo(const std::vector& contacts, size_t index, + Vec3f& contact, Vec3f& normal, FCL_REAL& depth) +{ + assert(index < contacts.size()); + + contact = contacts[index].pos; + normal = contacts[index].normal; + depth = contacts[index].penetration_depth; +} + #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) BOOST_AUTO_TEST_CASE(gjkcache) @@ -126,73 +136,73 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) CollisionResult result; bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0); @@ -211,27 +221,33 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) CollisionRequest request; CollisionResult result; + Vec3f contact; + FCL_REAL depth; + Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + const double tolerance = 1e-6; + + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), request, result) > 0); @@ -239,17 +255,47 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(q), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0); BOOST_CHECK(res); + + + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 30)), &contacts); + BOOST_CHECK(res); + BOOST_CHECK(contacts.size() == 4); + + Vec3f p1(-5, -5, 25); + Vec3f p2(-5, +5, 25); + Vec3f p3(+5, +5, 25); + Vec3f p4(+5, -5, 25); + + getContactInfo(contacts, 0, contact, normal, depth); + BOOST_CHECK(std::abs(depth) < tolerance); + BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); + BOOST_CHECK(contact.equal(Vec3f(-5, -5, 25))); + + getContactInfo(contacts, 1, contact, normal, depth); + BOOST_CHECK(std::abs(depth) < tolerance); + BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); + BOOST_CHECK(contact.equal(Vec3f(-5, 5, 25))); + + getContactInfo(contacts, 2, contact, normal, depth); + BOOST_CHECK(std::abs(depth) < tolerance); + BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); + BOOST_CHECK(contact.equal(Vec3f(5, 5, 25))); + + getContactInfo(contacts, 3, contact, normal, depth); + BOOST_CHECK(std::abs(depth) < tolerance); + BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); + BOOST_CHECK(contact.equal(Vec3f(5, -5, 25))); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) @@ -266,38 +312,38 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0); @@ -318,37 +364,37 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0); @@ -369,49 +415,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0); @@ -432,61 +478,61 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) bool res; - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0); BOOST_CHECK(res); - res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), request, result) > 0); @@ -535,57 +581,73 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 15) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 15) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-7.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-7.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 20.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 20.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); @@ -603,55 +665,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); } @@ -666,63 +740,79 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 3.75) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-0.625, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 3.75) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-0.625, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.875, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.875, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5.01) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0.005, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5.01) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.005, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contacts); BOOST_CHECK(res); } @@ -737,57 +827,69 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(1.25, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(1.25, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 1.25) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contacts); BOOST_CHECK(res); } @@ -802,60 +904,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -863,58 +981,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) hs = Halfspace(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -922,58 +1057,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) hs = Halfspace(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 12.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 12.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -6.25))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -6.25)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 20.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 20.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -988,54 +1139,66 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1043,52 +1206,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) hs = Plane(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1096,52 +1271,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) hs = Plane(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -1156,60 +1343,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0.05, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1217,58 +1420,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) hs = Halfspace(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -1.25, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -3.75, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0.05, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1276,58 +1495,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) hs = Halfspace(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -1342,54 +1577,66 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1397,52 +1644,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) hs = Plane(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1450,52 +1709,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) hs = Plane(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -1511,60 +1782,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-1.25, 0, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-1.25, 0, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-3.75, 0, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-3.75, 0, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0.05, 0, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0.05, 0, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1572,58 +1859,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) hs = Halfspace(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -1.25, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -1.25, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -3.75, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -3.75, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0.05, -5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0.05, -5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1631,58 +1934,74 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) hs = Halfspace(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -1.25))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 7.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -1.25)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -3.75))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -3.75)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0.05))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 10.1) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0.05)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -1697,54 +2016,66 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) Vec3f contact; FCL_REAL depth; Vec3f normal; + std::vector contacts; bool res; - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(2.5, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(-1, 0, 0))); BOOST_CHECK(contact.equal(Vec3f(-2.5, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(-2.5, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1752,52 +2083,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) hs = Plane(Vec3f(0, 1, 0), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, 2.5, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 2.5, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, -1, 0))); BOOST_CHECK(contact.equal(Vec3f(0, -2.5, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, -2.5, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contacts); BOOST_CHECK_FALSE(res); @@ -1805,52 +2148,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) hs = Plane(Vec3f(0, 0, 1), 0); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 0))); - res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform, &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 0)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, 1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, 2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, 2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(Vec3f(0, 0, -1))); BOOST_CHECK(contact.equal(Vec3f(0, 0, -2.5))); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal); + contacts.clear(); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contacts); + getContactInfo(contacts, 0, contact, normal, depth); BOOST_CHECK(res); BOOST_CHECK(std::abs(depth - 2.5) < 0.001); BOOST_CHECK(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1)))); BOOST_CHECK(contact.equal(transform.transform(Vec3f(0, 0, -2.5)))); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); - res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal); + res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contacts); BOOST_CHECK_FALSE(res); } @@ -2143,114 +2498,102 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) CollisionRequest request; CollisionResult result; - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; request.gjk_solver_type = GST_INDEP; // use indep GJK solver - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0); BOOST_CHECK_FALSE(res); - - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform, request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - - - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0); BOOST_CHECK(res); - - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contacts); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0); @@ -2265,41 +2608,39 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contacts); BOOST_CHECK(res); } @@ -2311,39 +2652,37 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contacts); BOOST_CHECK(res); } @@ -2355,39 +2694,37 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); } @@ -2399,49 +2736,47 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contacts); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contacts); BOOST_CHECK(res); } @@ -2453,59 +2788,57 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL penetration_depth; - Vec3f normal; + std::vector contacts; bool res; - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform, NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform, &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contacts); BOOST_CHECK(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL); BOOST_CHECK_FALSE(res); - res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &penetration_depth, &normal); + res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contacts); BOOST_CHECK_FALSE(res); } diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp index 6d8cf82c6..db9b0688d 100644 --- a/test/test_fcl_sphere_capsule.cpp +++ b/test/test_fcl_sphere_capsule.cpp @@ -58,7 +58,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z) Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., 200)); - BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL)); + BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative) @@ -72,7 +72,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative) Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., -200)); - BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL)); + BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x) @@ -86,7 +86,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x) Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (150., 0., 0.)); - BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL)); + BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated) @@ -102,30 +102,32 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated) rotation.setEulerZYX (M_PI * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.)); - BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL)); + BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z) { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3f sphere1_transform; - sphere1_transform.setTranslation (Vec3f (0., 0., -50)); + Sphere sphere1 (50); + Transform3f sphere1_transform; + sphere1_transform.setTranslation (Vec3f (0., 0., -50)); - Capsule capsule (50, 200.); - Transform3f capsule_transform (Vec3f (0., 0., 125)); + Capsule capsule (50, 200.); + Transform3f capsule_transform (Vec3f (0., 0., 125)); - FCL_REAL penetration = 0.; - Vec3f contact_point; - Vec3f normal; + std::vector contacts; - bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal); + bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); - BOOST_CHECK (is_intersecting); - BOOST_CHECK (penetration == 25.); - BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal)); - BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point)); + FCL_REAL penetration = contacts[0].penetration_depth; + Vec3f contact_point = contacts[0].pos; + Vec3f normal = contacts[0].normal; + + BOOST_CHECK (is_intersecting); + BOOST_CHECK (penetration == 25.); + BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal)); + BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated) @@ -141,11 +143,13 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated) rotation.setEulerZYX (M_PI * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (0., 50., 75)); - FCL_REAL penetration = 0.; - Vec3f contact_point; - Vec3f normal; + std::vector contacts; + + bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); - bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal); + FCL_REAL penetration = contacts[0].penetration_depth; + Vec3f contact_point = contacts[0].pos; + Vec3f normal = contacts[0].normal; BOOST_CHECK (is_intersecting); BOOST_CHECK_CLOSE (25, penetration, solver.collision_tolerance); From 33ff703da4c9b32ce55f6f46896158482e42a447 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 17 Mar 2015 15:34:21 -0400 Subject: [PATCH 02/12] Fix bug in adding node-node contact result to the total contact result - Fix use of std::partial_sort - Add safety check in case that the total contact result was initially non-empty --- include/fcl/traversal/traversal_node_octree.h | 62 ++++++++++--------- include/fcl/traversal/traversal_node_shapes.h | 31 +++++----- 2 files changed, 51 insertions(+), 42 deletions(-) diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index e2b2a7e7a..be48658da 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -338,22 +338,25 @@ class OcTreeSolver if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts)) { is_intersect = true; - 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, then contacts of greater penetration are added first. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.end() + free_space, contacts.end(), comparePenDepth); - num_adding_contacts = free_space; - } - else + if(crequest->num_max_contacts > cresult->numContacts()) { - num_adding_contacts = contacts.size(); + 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)); } - - 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)); } } @@ -907,22 +910,25 @@ class OcTreeSolver if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts)) { is_intersect = true; - 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, then contacts of greater penetration are added first. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.end() + free_space, contacts.end(), comparePenDepth); - num_adding_contacts = free_space; - } - else + if(crequest->num_max_contacts > cresult->numContacts()) { - num_adding_contacts = contacts.size(); - } + 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)); + 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)); + } } } diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h index 9c81dcf74..f58cfcdfe 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -82,22 +82,25 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contacts)) { is_collision = true; - 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, then contacts of greater penetration are added first. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.end() + free_space, contacts.end(), comparePenDepth); - num_adding_contacts = free_space; - } - else + if(request.num_max_contacts > result->numContacts()) { - num_adding_contacts = contacts.size(); + 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)); } - - 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 From 4f5b857fa26ba49668ad2c75c5d144daccfacc2d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 2 Jun 2015 00:50:29 -0400 Subject: [PATCH 03/12] Assign default values of query information for shape[~][Intersect/Distance](~) --- include/fcl/narrowphase/narrowphase.h | 72 +++++---------------------- 1 file changed, 12 insertions(+), 60 deletions(-) diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h index 0aa578afd..163702ed5 100644 --- a/include/fcl/narrowphase/narrowphase.h +++ b/include/fcl/narrowphase/narrowphase.h @@ -61,7 +61,7 @@ struct GJKSolver_libccd template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - std::vector* contacts) const + std::vector* contacts = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); @@ -96,7 +96,7 @@ struct GJKSolver_libccd /// @brief intersection checking between one shape and a triangle template bool shapeTriangleIntersect(const S& s, const Transform3f& tf, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf); void* o2 = details::triCreateGJKObject(P1, P2, P3); @@ -116,7 +116,7 @@ struct GJKSolver_libccd template bool shapeTriangleIntersect(const S& 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 + Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf1); void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); @@ -137,7 +137,7 @@ struct GJKSolver_libccd template bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const + FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); @@ -156,20 +156,12 @@ struct GJKSolver_libccd return res; } - template - bool shapeDistance(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - FCL_REAL* dist) const - { - return shapeDistance(s1, tf1, s2, tf2, dist, NULL, NULL); - } - /// @brief distance computation between one shape and a triangle template bool shapeTriangleDistance(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const + FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf); void* o2 = details::triCreateGJKObject(P1, P2, P3); @@ -185,20 +177,12 @@ struct GJKSolver_libccd return res; } - - template - bool shapeTriangleDistance(const S& s, const Transform3f& tf, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - FCL_REAL* dist) - { - return shapeTriangleDistance(s, tf, P1, P2, P3, dist, NULL, NULL); - } /// @brief distance computation between one shape and a triangle with transformation template bool shapeTriangleDistance(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const + FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf1); void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); @@ -216,14 +200,6 @@ struct GJKSolver_libccd return res; } - template - bool shapeTriangleDistance(const S& s, const Transform3f& tf1, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL* dist) const - { - return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, NULL, NULL); - } - /// @brief default setting for GJK algorithm GJKSolver_libccd() { @@ -513,7 +489,7 @@ struct GJKSolver_indep template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - std::vector* contacts) const + std::vector* contacts = NULL) const { Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; @@ -564,7 +540,7 @@ struct GJKSolver_indep template bool shapeTriangleIntersect(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { TriangleP tri(P1, P2, P3); @@ -613,7 +589,7 @@ struct GJKSolver_indep template bool shapeTriangleIntersect(const S& 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 + Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { TriangleP tri(P1, P2, P3); @@ -662,7 +638,7 @@ struct GJKSolver_indep template bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const + FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; @@ -701,19 +677,11 @@ struct GJKSolver_indep } } - template - bool shapeDistance(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - FCL_REAL* distance) const - { - return shapeDistance(s1, tf1, s2, tf2, distance, NULL, NULL); - } - /// @brief distance computation between one shape and a triangle template bool shapeTriangleDistance(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const + FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); @@ -750,20 +718,12 @@ struct GJKSolver_indep return false; } } - - template - bool shapeTriangleDistance(const S& s, const Transform3f& tf, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, - FCL_REAL* distance) const - { - return shapeTriangleDistance(s, tf, P1, P2, P3, distance, NULL, NULL); - } /// @brief distance computation between one shape and a triangle with transformation template bool shapeTriangleDistance(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const + FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); @@ -800,14 +760,6 @@ struct GJKSolver_indep return false; } } - - template - bool shapeTriangleDistance(const S& s, const Transform3f& tf1, - const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, - FCL_REAL* distance) const - { - return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, distance, NULL, NULL); - } /// @brief default setting for GJK algorithm GJKSolver_indep() From 3c4954f24465a9712af925ed310030ad89322f2d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 2 Jun 2015 04:34:03 -0400 Subject: [PATCH 04/12] Change contact point comparison function to follow the convention: return true if the first element is less than the second. --- include/fcl/collision_data.h | 2 +- include/fcl/narrowphase/narrowphase.h | 4 ++-- include/fcl/traversal/traversal_node_octree.h | 4 ++-- include/fcl/traversal/traversal_node_shapes.h | 2 +- src/collision_data.cpp | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h index 79b73797e..ad161bf59 100644 --- a/include/fcl/collision_data.h +++ b/include/fcl/collision_data.h @@ -75,7 +75,7 @@ struct ContactPoint {} }; -/// @brief Return true if _cp1's penentration depth is greater than _cp2's. +/// @brief Return true if _cp1's penentration depth is less than _cp2's. bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2); /// @brief Contact information returned by collision diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h index 163702ed5..062906368 100644 --- a/include/fcl/narrowphase/narrowphase.h +++ b/include/fcl/narrowphase/narrowphase.h @@ -257,7 +257,7 @@ bool GJKSolver_libccd::shapeIntersect(const S1& s1, const Transform3f& tf1, if (!contacts.empty()) { // Get the deepest contact point - const ContactPoint& maxDepthContact = (*std::min_element(contacts.begin(), contacts.end(), &comparePenDepth)); + const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); if (contact_points) *contact_points = maxDepthContact.pos; @@ -830,7 +830,7 @@ bool GJKSolver_indep::shapeIntersect(const S1& s1, const Transform3f& tf1, if (!contacts.empty()) { // Get the deepest contact point - const ContactPoint& maxDepthContact = (*std::min_element(contacts.begin(), contacts.end(), &comparePenDepth)); + const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); if (contact_points) *contact_points = maxDepthContact.pos; diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index 077281f37..24e056bd3 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -347,7 +347,7 @@ class OcTreeSolver // 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); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); num_adding_contacts = free_space; } else @@ -919,7 +919,7 @@ class OcTreeSolver // 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); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); num_adding_contacts = free_space; } else diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h index 102652372..809b581b5 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -91,7 +91,7 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase // 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); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); num_adding_contacts = free_space; } else diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 54b82be29..1f6ddaa6b 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -42,7 +42,7 @@ namespace fcl bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2) { - return (_cp1.penetration_depth > _cp2.penetration_depth); + return _cp1.penetration_depth < _cp2.penetration_depth; } bool CollisionRequest::isSatisfied(const CollisionResult& result) const From 2eb2a499c1d284be4b9fe7864103a122a1638bfd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 14:45:23 -0400 Subject: [PATCH 05/12] Update shape collision test suit for multiple contacts -- only sphere-sphere test is applied --- include/fcl/collision_data.h | 3 + test/test_fcl_geometric_shapes.cpp | 994 +++++++++++++++++------------ 2 files changed, 589 insertions(+), 408 deletions(-) diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h index ad161bf59..cfe58dac2 100644 --- a/include/fcl/collision_data.h +++ b/include/fcl/collision_data.h @@ -68,6 +68,9 @@ struct ContactPoint /// @brief Penetration depth FCL_REAL penetration_depth; + /// @brief Constructor + ContactPoint() : normal(Vec3f()), pos(Vec3f()), penetration_depth(0.0) {} + /// @brief Constructor ContactPoint(const Vec3f& n_, const Vec3f& p_, FCL_REAL d_) : normal(n_), pos(p_), diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index f3f0d6897..301a817e4 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -119,8 +119,8 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, - const Vec3f& contact_or_normal, const Vec3f& expected_contact_or_normal, + const Vec3f& actual_contact_or_normal, bool check_opposite_normal, FCL_REAL tol) { @@ -133,14 +133,14 @@ void printComparisonError(const std::string& comparison_type, << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation() << std::endl - << comparison_type << ": " << contact_or_normal << std::endl - << "expected_" << comparison_type << ": " << expected_contact_or_normal; + << "expected_" << comparison_type << ": " << expected_contact_or_normal + << "actual_" << comparison_type << " : " << actual_contact_or_normal << std::endl; if (check_opposite_normal) std::cout << " or " << -expected_contact_or_normal; std::cout << std::endl - << "difference: " << (contact_or_normal - expected_contact_or_normal).norm() << std::endl + << "difference: " << (actual_contact_or_normal - expected_contact_or_normal).norm() << std::endl << "tolerance: " << tol << std::endl; } @@ -149,8 +149,8 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, - FCL_REAL depth, FCL_REAL expected_depth, + FCL_REAL actual_depth, FCL_REAL tol) { std::cout << "Disagreement between " << comparison_type @@ -162,77 +162,226 @@ void printComparisonError(const std::string& comparison_type, << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation() << std::endl - << "depth: " << depth << std::endl << "expected_depth: " << expected_depth << std::endl - << "difference: " << std::fabs(depth - expected_depth) << std::endl + << "actual_depth : " << actual_depth << std::endl + << "difference: " << std::fabs(actual_depth - expected_depth) << std::endl << "tolerance: " << tol << std::endl; } template -void compareContact(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - GJKSolverType solver_type, - const Vec3f& contact, Vec3f* expected_point, - FCL_REAL depth, FCL_REAL* expected_depth, - const Vec3f& normal, Vec3f* expected_normal, bool check_opposite_normal, - FCL_REAL tol) +bool checkContactPoints(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + GJKSolverType solver_type, + const ContactPoint& expected, const ContactPoint& actual, + bool check_position = false, + bool check_depth = false, + bool check_normal = false, + bool check_opposite_normal = false, + FCL_REAL tol = 1e-9) { - if (expected_point) + if (check_position) { - bool contact_equal = contact.equal(*expected_point, tol); - BOOST_CHECK(contact_equal); + bool contact_equal = actual.pos.equal(expected.pos, tol); if (!contact_equal) - printComparisonError("contact", s1, tf1, s2, tf2, solver_type, contact, *expected_point, false, tol); + return false; } - if (expected_depth) + if (check_depth) { - bool depth_equal = std::fabs(depth - *expected_depth) < tol; - BOOST_CHECK(depth_equal); + bool depth_equal = std::fabs(actual.penetration_depth - expected.penetration_depth) < tol; if (!depth_equal) - printComparisonError("depth", s1, tf1, s2, tf2, solver_type, depth, *expected_depth, tol); + return false; } - if (expected_normal) + if (check_normal) { - bool normal_equal = normal.equal(*expected_normal, tol); + bool normal_equal = actual.normal.equal(expected.normal, tol); if (!normal_equal && check_opposite_normal) - normal_equal = normal.equal(-(*expected_normal), tol); + normal_equal = actual.normal.equal(-expected.normal, tol); - BOOST_CHECK(normal_equal); if (!normal_equal) - printComparisonError("normal", s1, tf1, s2, tf2, solver_type, normal, *expected_normal, check_opposite_normal, tol); + return false; } + + return true; } template -void testShapeInersection(const S1& s1, const Transform3f& tf1, +bool inspectContactPoints(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, - bool expected_res, - Vec3f* expected_point = NULL, - FCL_REAL* expected_depth = NULL, - Vec3f* expected_normal = NULL, + const std::vector& expected_contacts, + const std::vector& actual_contacts, + bool check_position = false, + bool check_depth = false, + bool check_normal = false, bool check_opposite_normal = false, FCL_REAL tol = 1e-9) +{ + // Check number of contact points + bool sameNumContacts = (actual_contacts.size() == expected_contacts.size()); + BOOST_CHECK(sameNumContacts); + if (!sameNumContacts) + { + std::cout << "[compareContacts] The numbers of expected contacts '" + << expected_contacts.size() + << "' and the number of actual contacts '" + << actual_contacts.size() + << "' are not equal.\n"; + } + + // Check if actual contacts and expected contacts are matched + const size_t numContacts = actual_contacts.size(); + + std::vector index_to_actual_contacts(numContacts, -1); + std::vector index_to_expected_contacts(numContacts, -1); + + bool foundAll = true; + for (size_t i = 0; i < numContacts; ++i) + { + const ContactPoint& expected = expected_contacts[i]; + + // Check if expected contact is in the list of actual contacts + for (size_t j = 0; j < numContacts; ++j) + { + if (index_to_expected_contacts[j] != -1) + continue; + + const ContactPoint& actual = actual_contacts[j]; + + bool found = checkContactPoints( + s1, tf1, s2, tf2, solver_type, + expected, actual, + check_position, + check_depth, + check_normal, check_opposite_normal, + tol); + + if (found) + { + index_to_actual_contacts[i] = j; + index_to_expected_contacts[j] = i; + } + } + + if (index_to_actual_contacts[i] == -1) + foundAll = false; + } + + if (!foundAll) + { + std::cout << "===== [geometric shape collision test fail report] ======\n" + << "\n" + << "Solver type: " << getGJKSolverName(solver_type) << "\n" + << "\n" + << "[ Shape 1 ]\n" + << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" + << "tf1.quaternion : " << tf1.getQuatRotation() << "\n" + << "tf1.translation: " << tf1.getTranslation() << "\n" + << "\n" + << "[ Shape 2 ]\n" + << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" + << "tf2.quaternion : " << tf2.getQuatRotation() << "\n" + << "tf2.translation: " << tf2.getTranslation() << "\n" + << "\n" + << "[ Expected Contacts: " << numContacts << " ]\n"; + for (size_t i = 0; i < numContacts; ++i) + { + const ContactPoint& expected = expected_contacts[i]; + + std::cout << "(" << i << ") pos: " << expected.pos << ", " + << "normal: " << expected.normal << ", " + << "depth: " << expected.penetration_depth + << " ---- "; + if (index_to_actual_contacts[i] != -1) + std::cout << "found, actual (" << index_to_actual_contacts[i] << ")\n"; + else + std::cout << "not found!\n"; + } + std::cout << "\n" + << "[ Actual Contacts: " << numContacts << " ]\n"; + for (size_t i = 0; i < numContacts; ++i) + { + const ContactPoint& actual = actual_contacts[i]; + + std::cout << "(" << i << ") pos: " << actual.pos << ", " + << "normal: " << actual.normal << ", " + << "depth: " << actual.penetration_depth + << " ---- "; + if (index_to_expected_contacts[i] != -1) + std::cout << "found, expected (" << index_to_expected_contacts[i] << ")\n"; + else + std::cout << "not found!\n"; + } + + std::cout << "\n"; + } + + return foundAll; +} + +void getContactPointsFromResult(std::vector& contacts, const CollisionResult& result) +{ + const size_t numContacts = result.numContacts(); + contacts.resize(numContacts); + + for (size_t i = 0; i < numContacts; ++i) + { + const Contact& cnt = result.getContact(i); + + contacts[i].pos = cnt.pos; + contacts[i].normal = cnt.normal; + contacts[i].penetration_depth = cnt.penetration_depth; + } +} + +template +FCL_DEPRECATED +void testShapeIntersection(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + GJKSolverType solver_type, + bool expected_res, + Vec3f* expected_point, + FCL_REAL* expected_depth, + Vec3f* expected_normal, + bool check_opposite_normal = false, + FCL_REAL tol = 1e-9) +{ + // do nothing +} + +template +void testShapeIntersection( + const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + GJKSolverType solver_type, + bool expected_res, + const std::vector& expected_contacts = std::vector(), + bool check_position = true, + bool check_depth = true, + bool check_normal = true, + bool check_opposite_normal = false, + FCL_REAL tol = 1e-9) { CollisionRequest request; request.gjk_solver_type = solver_type; CollisionResult result; - Vec3f contact; - FCL_REAL depth; - Vec3f normal; // normal direction should be from object 1 to object 2 + std::vector actual_contacts; + bool res; + // Part A: Check collisions using shapeIntersect() + + // Check only whether they are colliding or not. if (solver_type == GST_LIBCCD) { - res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL); + res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL); } else if (solver_type == GST_INDEP) { - res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL, NULL, NULL); + res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL); } else { @@ -241,13 +390,14 @@ void testShapeInersection(const S1& s1, const Transform3f& tf1, } BOOST_CHECK_EQUAL(res, expected_res); + // Check contact information as well if (solver_type == GST_LIBCCD) { - res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal); + res = solver1.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else if (solver_type == GST_INDEP) { - res = solver2.shapeIntersect(s1, tf1, s2, tf2, &contact, &depth, &normal); + res = solver2.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else { @@ -256,25 +406,37 @@ void testShapeInersection(const S1& s1, const Transform3f& tf1, } BOOST_CHECK_EQUAL(res, expected_res); if (expected_res) - compareContact(s1, tf1, s2, tf2, solver_type, contact, expected_point, depth, expected_depth, normal, expected_normal, check_opposite_normal, tol); + { + BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, solver_type, + expected_contacts, actual_contacts, + check_position, + check_depth, + check_normal, check_opposite_normal, + tol)); + } + + // Part B: Check collisions using collide() + // Check only whether they are colliding or not. request.enable_contact = false; result.clear(); res = (collide(&s1, tf1, &s2, tf2, request, result) > 0); BOOST_CHECK_EQUAL(res, expected_res); + // Check contact information as well request.enable_contact = true; result.clear(); res = (collide(&s1, tf1, &s2, tf2, request, result) > 0); BOOST_CHECK_EQUAL(res, expected_res); if (expected_res) { - BOOST_CHECK_EQUAL(result.numContacts(), 1); - if (result.numContacts() == 1) - { - Contact contact = result.getContact(0); - compareContact(s1, tf1, s2, tf2, solver_type, contact.pos, expected_point, contact.penetration_depth, expected_depth, contact.normal, expected_normal, check_opposite_normal, tol); - } + getContactPointsFromResult(actual_contacts, result); + BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, solver_type, + expected_contacts, actual_contacts, + check_position, + check_depth, + check_normal, check_opposite_normal, + tol)); } } @@ -289,75 +451,95 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); - normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + contacts[0].pos.setValue(20, 0, 0); + contacts[0].penetration_depth = 0.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + contacts[0].pos.setValue(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[0].pos = transform.transform(Vec3f(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0)); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(); - normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) + contacts[0].pos.setZero(); + contacts[0].penetration_depth = 20.0 + 10.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) + contacts[0].pos = transform.transform(Vec3f()); + contacts[0].penetration_depth = 20.0 + 10.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); - normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(-1, 0, 0); + contacts[0].pos.setValue(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + contacts[0].pos = transform.transform(Vec3f(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0)); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); - normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(-1, 0, 0); + contacts[0].pos.setValue(-20, 0, 0); + contacts[0].penetration_depth = 0.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); - normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } bool compareContactPoints(const Vec3f& c1,const Vec3f& c2) @@ -391,13 +573,7 @@ void testBoxBoxContactPoints(const Matrix3f& R) Transform3f tf1 = Transform3f(Vec3f(0, 0, -50)); Transform3f tf2 = Transform3f(R); - Vec3f normal; - Vec3f point; - double penetration; - - // Make sure the two boxes are colliding - bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &point, &penetration, &normal); - BOOST_CHECK(res); + std::vector contacts; // Compute global vertices for (int i = 0; i < 8; ++i) @@ -407,7 +583,9 @@ void testBoxBoxContactPoints(const Matrix3f& R) std::sort(vertices.begin(), vertices.end(), compareContactPoints); // The lowest vertex along z-axis should be the contact point - BOOST_CHECK(vertices[0].equal(point)); + contacts.resize(1); + contacts[0].pos = vertices[0]; + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, true, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) @@ -432,32 +610,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(q); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(q); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); FCL_UINT32 numTests = 1e+2; for (FCL_UINT32 i = 0; i < numTests; ++i) @@ -487,30 +665,30 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.501, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) @@ -531,42 +709,42 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25.1, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) @@ -587,30 +765,30 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) @@ -631,40 +809,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.001, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.001, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal.setValue(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) @@ -685,48 +863,48 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.061); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.061); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.46); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.46); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal.setValue(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.01)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.01)); - testShapeInersection(s1, tf1, s2, tf2, GST_LIBCCD, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) @@ -870,64 +1048,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) contact.setValue(-5, 0, 0); depth = 10; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-5, 0, 0)); depth = 10; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contact.setValue(-2.5, 0, 0); depth = 15; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 15; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contact.setValue(-7.5, 0, 0); depth = 5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); contact = transform.transform(Vec3f(-7.5, 0, 0)); depth = 5; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); contact.setValue(0.05, 0, 0); depth = 20.1; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); contact = transform.transform(Vec3f(0.05, 0, 0)); depth = 20.1; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) @@ -950,58 +1128,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) contact.setZero(); depth = 10; normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 10; normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contact.setValue(5, 0, 0); depth = 5; normal.setValue(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); contact = transform.transform(Vec3f(5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contact.setValue(-5, 0, 0); depth = 5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); contact = transform.transform(Vec3f(-5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) @@ -1024,68 +1202,68 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) contact.setValue(-1.25, 0, 0); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 2.5; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contact.setValue(-0.625, 0, 0); depth = 3.75; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contact = transform.transform(Vec3f(-0.625, 0, 0)); depth = 3.75; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contact.setValue(-1.875, 0, 0); depth = 1.25; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contact = transform.transform(Vec3f(-1.875, 0, 0)); depth = 1.25; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); contact.setValue(0.005, 0, 0); depth = 5.01; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); contact = transform.transform(Vec3f(0.005, 0, 0)); depth = 5.01; normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true); } BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) @@ -1108,62 +1286,62 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) contact.setValue(0, 0, 0); depth = 2.5; normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contact.setValue(1.25, 0, 0); depth = 1.25; normal.setValue(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contact = transform.transform(Vec3f(1.25, 0, 0)); depth = 1.25; normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contact.setValue(-1.25, 0, 0); depth = 1.25; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 1.25; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) @@ -1186,64 +1364,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact.setValue(-2.5, 0, 0); depth = 5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(-1.25, 0, 0); depth = 7.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-3.75, 0, 0); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact.setValue(0.05, 0, 0); depth = 10.1; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contact = transform.transform(Vec3f(0.05, 0, 0)); depth = 10.1; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1255,64 +1433,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact.setValue(0, -2.5, 0); depth = 5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, -2.5, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, -1.25, 0); depth = 7.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -3.75, 0); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact.setValue(0, 0.05, 0); depth = 10.1; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contact = transform.transform(Vec3f(0, 0.05, 0)); depth = 10.1; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1324,64 +1502,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) contact.setValue(0, 0, -5); depth = 10; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, -5)); depth = 10; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, -3.75); depth = 12.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 12.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -6.25); depth = 7.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -6.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); contact.setValue(0, 0, 0.05); depth = 20.1; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); contact = transform.transform(Vec3f(0, 0, 0.05)); depth = 20.1; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) @@ -1404,58 +1582,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact.setValue(0, 0, 0); depth = 5; normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(2.5, 0, 0); depth = 2.5; normal.setValue(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-2.5, 0, 0); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1467,58 +1645,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact.setValue(0, 0, 0); depth = 5; normal.setValue(0, 1, 0); // (0, 1, 0) or (0, -1, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, 2.5, 0); depth = 2.5; normal.setValue(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, 2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -2.5, 0); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1530,58 +1708,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) contact.setValue(0, 0, 0); depth = 10; normal.setValue(0, 0, 1); // (0, 0, 1) or (0, 0, -1) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 10; normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, 2.5); depth = 7.5; normal.setValue(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, 2.5)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -2.5); depth = 7.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) @@ -1604,64 +1782,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact.setValue(-2.5, 0, 0); depth = 5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(-1.25, 0, 0); depth = 7.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-3.75, 0, 0); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact.setValue(0.05, 0, 0); depth = 10.1; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contact = transform.transform(Vec3f(0.05, 0, 0)); depth = 10.1; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1673,64 +1851,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact.setValue(0, -2.5, 0); depth = 5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, -2.5, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, -1.25, 0); depth = 7.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, 0)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -3.75, 0); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact.setValue(0, 0.05, 0); depth = 10.1; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contact = transform.transform(Vec3f(0, 0.05, 0)); depth = 10.1; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1742,64 +1920,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) contact.setValue(0, 0, -2.5); depth = 5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, -1.25); depth = 7.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -1.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -3.75); depth = 2.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contact.setValue(0, 0, 0.05); depth = 10.1; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); contact = transform.transform(Vec3f(0, 0, 0.05)); depth = 10.1; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) @@ -1822,58 +2000,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact.setValue(0, 0, 0); depth = 5; normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(2.5, 0, 0); depth = 2.5; normal.setValue(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-2.5, 0, 0); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1885,58 +2063,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact.setValue(0, 0, 0); depth = 5; normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, 2.5, 0); depth = 2.5; normal.setValue(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, 2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -2.5, 0); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -2.5, 0)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -1948,58 +2126,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) contact.setValue(0, 0, 0); depth = 5; normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, 2.5); depth = 2.5; normal.setValue(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, 2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -2.5); depth = 2.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } @@ -2023,64 +2201,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact.setValue(-2.5, 0, -5); depth = 5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(-2.5, 0, -5)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(-1.25, 0, -5); depth = 7.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(-1.25, 0, -5)); depth = 7.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-3.75, 0, -5); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-3.75, 0, -5)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contact.setValue(0.05, 0, -5); depth = 10.1; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contact = transform.transform(Vec3f(0.05, 0, -5)); depth = 10.1; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -2092,64 +2270,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact.setValue(0, -2.5, -5); depth = 5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, -2.5, -5)); depth = 5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, -1.25, -5); depth = 7.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, -1.25, -5)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -3.75, -5); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -3.75, -5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contact.setValue(0, 0.05, -5); depth = 10.1; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contact = transform.transform(Vec3f(0, 0.05, -5)); depth = 10.1; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -2161,64 +2339,64 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) contact.setValue(0, 0, -2.5); depth = 5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, -1.25); depth = 7.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, -1.25)); depth = 7.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -3.75); depth = 2.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -3.75)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contact.setValue(0, 0, 0.05); depth = 10.1; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); contact = transform.transform(Vec3f(0, 0, 0.05)); depth = 10.1; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) @@ -2241,58 +2419,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact.setValue(0, 0, 0); depth = 5; normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contact.setValue(2.5, 0, -2.5); depth = 2.5; normal.setValue(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contact = transform.transform(Vec3f(2.5, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contact.setValue(-2.5, 0, -2.5); depth = 2.5; normal.setValue(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contact = transform.transform(Vec3f(-2.5, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -2304,58 +2482,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact.setValue(0, 0, 0); depth = 5; normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contact.setValue(0, 2.5, -2.5); depth = 2.5; normal.setValue(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contact = transform.transform(Vec3f(0, 2.5, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contact.setValue(0, -2.5, -2.5); depth = 2.5; normal.setValue(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contact = transform.transform(Vec3f(0, -2.5, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); @@ -2367,58 +2545,58 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) contact.setValue(0, 0, 0); depth = 5; normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = transform; tf2 = transform; contact = transform.transform(Vec3f(0, 0, 0)); depth = 5; normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contact.setValue(0, 0, 2.5); depth = 2.5; normal.setValue(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contact = transform.transform(Vec3f(0, 0, 2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contact.setValue(0, 0, -2.5); depth = 2.5; normal.setValue(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contact = transform.transform(Vec3f(0, 0, -2.5)); depth = 2.5; normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); - testShapeInersection(s, tf1, hs, tf2, GST_LIBCCD, false); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } @@ -2716,69 +2894,69 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(); normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); normal.setValue(-1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) @@ -2803,32 +2981,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(q); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(q); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) @@ -2849,33 +3027,33 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.51, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); // built-in GJK solver returns incorrect normal. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) @@ -2896,32 +3074,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) @@ -2942,33 +3120,33 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true); // built-in GJK solver returns incorrect normal. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) @@ -2989,44 +3167,44 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); normal.setValue(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); // built-in GJK solver returns incorrect normal. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal.setValue(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); // built-in GJK solver returns incorrect normal. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) @@ -3047,49 +3225,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10, 0, 0)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); normal.setValue(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); // built-in GJK solver returns incorrect normal. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10)); normal.setValue(0, 0, 1); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } From 6247ed09420ade8a9d06bb9ebe3de481f6caf810 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 17:55:59 -0400 Subject: [PATCH 06/12] Fix normal direction of boxbox collision to point to second object --- src/narrowphase/narrowphase.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 0591ddd53..6bcec4886 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -1236,7 +1236,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, // Vec3f pointInWorld((pa + pb) * 0.5); // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); - contacts.push_back(ContactPoint(-normal,pb,-*depth)); + contacts.push_back(ContactPoint(normal,pb,-*depth)); *return_code = code; return 1; @@ -1421,7 +1421,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa); - contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); + contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); } } else @@ -1430,7 +1430,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; - contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); + contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); } } } @@ -1456,9 +1456,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, { Vec3f posInWorld = points[iret[j]] + (*pa); if(code < 4) - contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]])); + contacts.push_back(ContactPoint(normal, posInWorld, -dep[iret[j]])); else - contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); + contacts.push_back(ContactPoint(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); } cnum = maxc; } From 60f380d9437b2f6689f69e4c3ae8bdb144f7f3a2 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 17:56:51 -0400 Subject: [PATCH 07/12] Update boxbox intersection tests --- test/test_fcl_geometric_shapes.cpp | 101 ++++++++++++++++++++++------- 1 file changed, 78 insertions(+), 23 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 301a817e4..b7f527285 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -44,6 +44,7 @@ #include "test_fcl_utility.h" #include "fcl/ccd/motion.h" #include +#include using namespace fcl; @@ -224,11 +225,28 @@ bool inspectContactPoints(const S1& s1, const Transform3f& tf1, BOOST_CHECK(sameNumContacts); if (!sameNumContacts) { - std::cout << "[compareContacts] The numbers of expected contacts '" + std::cout << "\n" + << "===== [ geometric shape collision test failure report ] ======\n" + << "\n" + << "Solver type: " << getGJKSolverName(solver_type) << "\n" + << "\n" + << "[ Shape 1 ]\n" + << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" + << "tf1.quaternion : " << tf1.getQuatRotation() << "\n" + << "tf1.translation: " << tf1.getTranslation() << "\n" + << "\n" + << "[ Shape 2 ]\n" + << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" + << "tf2.quaternion : " << tf2.getQuatRotation() << "\n" + << "tf2.translation: " << tf2.getTranslation() << "\n" + << "\n" + << "The numbers of expected contacts '" << expected_contacts.size() << "' and the number of actual contacts '" << actual_contacts.size() - << "' are not equal.\n"; + << "' are not equal.\n" + << "\n"; + return false; } // Check if actual contacts and expected contacts are matched @@ -262,6 +280,7 @@ bool inspectContactPoints(const S1& s1, const Transform3f& tf1, { index_to_actual_contacts[i] = j; index_to_expected_contacts[j] = i; + break; } } @@ -271,7 +290,8 @@ bool inspectContactPoints(const S1& s1, const Transform3f& tf1, if (!foundAll) { - std::cout << "===== [geometric shape collision test fail report] ======\n" + std::cout << "\n" + << "===== [ geometric shape collision test failure report ] ======\n" << "\n" << "Solver type: " << getGJKSolverName(solver_type) << "\n" << "\n" @@ -366,6 +386,7 @@ void testShapeIntersection( { CollisionRequest request; request.gjk_solver_type = solver_type; + request.num_max_contacts = std::numeric_limits::max(); CollisionResult result; std::vector actual_contacts; @@ -542,11 +563,16 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -bool compareContactPoints(const Vec3f& c1,const Vec3f& c2) +bool compareContactPoints1(const Vec3f& c1,const Vec3f& c2) { return c1[2] < c2[2]; } // Ascending order +bool compareContactPoints2(const ContactPoint& cp1,const ContactPoint& cp2) +{ + return cp1.pos[2] < cp2.pos[2]; +} // Ascending order + void testBoxBoxContactPoints(const Matrix3f& R) { Box s1(100, 100, 100); @@ -575,17 +601,28 @@ void testBoxBoxContactPoints(const Matrix3f& R) std::vector contacts; + // Make sure the two boxes are colliding + bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contacts); + BOOST_CHECK(res); + // Compute global vertices for (int i = 0; i < 8; ++i) vertices[i] = tf2.transform(vertices[i]); // Sort the vertices so that the lowest vertex along z-axis comes first - std::sort(vertices.begin(), vertices.end(), compareContactPoints); - - // The lowest vertex along z-axis should be the contact point - contacts.resize(1); - contacts[0].pos = vertices[0]; - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, true, false, false); + std::sort(vertices.begin(), vertices.end(), compareContactPoints1); + std::sort(contacts.begin(), contacts.end(), compareContactPoints2); + + // The lowest n vertex along z-axis should be the contact point + size_t numContacts = contacts.size(); + numContacts = std::min(static_cast(1), numContacts); + // TODO: BoxBox algorithm seems not able to find all the colliding vertices. + // We just check the deepest one as workaround. + for (size_t i = 0; i < numContacts; ++i) + { + BOOST_CHECK(vertices[i].equal(contacts[i].pos)); + BOOST_CHECK(Vec3f(0, 0, 1).equal(contacts[i].normal)); + } } BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) @@ -599,9 +636,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); @@ -609,19 +644,31 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal.setValue(1, 0, 0); + contacts[1].normal.setValue(1, 0, 0); + contacts[2].normal.setValue(1, 0, 0); + contacts[3].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = Vec3f(1, 0, 0); + contacts[1].normal = Vec3f(1, 0, 0); + contacts[2].normal = Vec3f(1, 0, 0); + contacts[3].normal = Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15.01, 0, 0)); @@ -629,13 +676,21 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) tf1 = Transform3f(); tf2 = Transform3f(q); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = Vec3f(1, 0, 0); + contacts[1].normal = Vec3f(1, 0, 0); + contacts[2].normal = Vec3f(1, 0, 0); + contacts[3].normal = Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(q); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); FCL_UINT32 numTests = 1e+2; for (FCL_UINT32 i = 0; i < numTests; ++i) From 17f72c861566ef91d5443eec6cdd54de7ab09cc4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 19:05:04 -0400 Subject: [PATCH 08/12] Update other shape-shape intersection tests --- test/test_fcl_geometric_shapes.cpp | 135 ++++++++++++++++------------- 1 file changed, 75 insertions(+), 60 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index b7f527285..faec46a09 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -369,6 +369,7 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1, FCL_REAL tol = 1e-9) { // do nothing + BOOST_CHECK(false); } template @@ -712,20 +713,20 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). - normal.setValue(-1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); @@ -737,13 +738,15 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) @@ -757,48 +760,50 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25.1, 0, 0)); - normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } @@ -813,29 +818,31 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); @@ -857,29 +864,31 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.001, 0, 0)); @@ -891,13 +900,15 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); - normal.setValue(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); } BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) @@ -911,29 +922,31 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.061); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal, false, 0.46); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); @@ -945,13 +958,15 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); - normal.setValue(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.01)); From a14dd80f31ec40e7b09b9dd211c6edbc987e928b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 21:49:30 -0400 Subject: [PATCH 09/12] Update plane-[shape] intersection tests --- test/test_fcl_geometric_shapes.cpp | 1432 +++++++++++++++------------- 1 file changed, 784 insertions(+), 648 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index faec46a09..126f954d2 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -1109,51 +1109,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(-5, 0, 0); - depth = 10; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-5, 0, 0); + contacts[0].penetration_depth = 10; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-5, 0, 0)); - depth = 10; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-5, 0, 0)); + contacts[0].penetration_depth = 10; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); - contact.setValue(-2.5, 0, 0); - depth = 15; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, 0); + contacts[0].penetration_depth = 15; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); - depth = 15; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); + contacts[0].penetration_depth = 15; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); - contact.setValue(-7.5, 0, 0); - depth = 5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-7.5, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); - contact = transform.transform(Vec3f(-7.5, 0, 0)); - depth = 5; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-7.5, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); @@ -1165,17 +1169,19 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); - contact.setValue(0.05, 0, 0); - depth = 20.1; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0.05, 0, 0); + contacts[0].penetration_depth = 20.1; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); - depth = 20.1; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); + contacts[0].penetration_depth = 20.1; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); } BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) @@ -1189,51 +1195,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setZero(); - depth = 10; - normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setZero(); + contacts[0].penetration_depth = 10; + contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 10; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 10; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); - contact.setValue(5, 0, 0); - depth = 5; - normal.setValue(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(5, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); - contact = transform.transform(Vec3f(5, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(5, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); - contact.setValue(-5, 0, 0); - depth = 5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-5, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); - contact = transform.transform(Vec3f(-5, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-5, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); @@ -1263,65 +1273,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(-1.25, 0, 0); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.25, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-1.25, 0, 0)); - depth = 2.5; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); - contact.setValue(-0.625, 0, 0); - depth = 3.75; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-0.625, 0, 0); + contacts[0].penetration_depth = 3.75; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); - contact = transform.transform(Vec3f(-0.625, 0, 0)); - depth = 3.75; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-0.625, 0, 0)); + contacts[0].penetration_depth = 3.75; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); - contact.setValue(-1.875, 0, 0); - depth = 1.25; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.875, 0, 0); + contacts[0].penetration_depth = 1.25; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); - contact = transform.transform(Vec3f(-1.875, 0, 0)); - depth = 1.25; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0)); + contacts[0].penetration_depth = 1.25; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); - contact.setValue(0.005, 0, 0); - depth = 5.01; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0.005, 0, 0); + contacts[0].penetration_depth = 5.01; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); - contact = transform.transform(Vec3f(0.005, 0, 0)); - depth = 5.01; - normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0)); + contacts[0].penetration_depth = 5.01; + contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); @@ -1333,7 +1349,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true); + contacts.resize(1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) @@ -1347,51 +1364,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 2.5; - normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); - contact.setValue(1.25, 0, 0); - depth = 1.25; - normal.setValue(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(1.25, 0, 0); + contacts[0].penetration_depth = 1.25; + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); - contact = transform.transform(Vec3f(1.25, 0, 0)); - depth = 1.25; - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0)); + contacts[0].penetration_depth = 1.25; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); - contact.setValue(-1.25, 0, 0); - depth = 1.25; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.25, 0, 0); + contacts[0].penetration_depth = 1.25; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); - depth = 1.25; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); + contacts[0].penetration_depth = 1.25; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); @@ -1411,7 +1432,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true); + contacts.resize(1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) @@ -1425,65 +1447,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(-2.5, 0, 0); - depth = 5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(-1.25, 0, 0); - depth = 7.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.25, 0, 0); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-3.75, 0, 0); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-3.75, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-3.75, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - contact.setValue(0.05, 0, 0); - depth = 10.1; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0.05, 0, 0); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); @@ -1500,59 +1528,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, 0); - depth = 5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, -1.25, 0); - depth = 7.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -1.25, 0); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -3.75, 0); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -3.75, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -3.75, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - contact.setValue(0, 0.05, 0); - depth = 10.1; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0.05, 0); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0.05, 0)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); @@ -1569,59 +1605,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -5); - depth = 10; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -5); + contacts[0].penetration_depth = 10; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -5)); - depth = 10; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -5)); + contacts[0].penetration_depth = 10; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, -3.75); - depth = 12.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -3.75); + contacts[0].penetration_depth = 12.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); - depth = 12.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); + contacts[0].penetration_depth = 12.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -6.25); - depth = 7.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -6.25); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -6.25)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -6.25)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); - contact.setValue(0, 0, 0.05); - depth = 20.1; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0.05); + contacts[0].penetration_depth = 20.1; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); - depth = 20.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); + contacts[0].penetration_depth = 20.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); @@ -1643,51 +1687,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(2.5, 0, 0); - depth = 2.5; - normal.setValue(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(2.5, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(2.5, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(2.5, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-2.5, 0, 0); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); @@ -1712,45 +1760,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(0, 1, 0); // (0, 1, 0) or (0, -1, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 1, 0); // (0, 1, 0) or (0, -1, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, 2.5, 0); - depth = 2.5; - normal.setValue(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 2.5, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, 2.5, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 2.5, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -2.5, 0); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -2.5, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); @@ -1775,45 +1829,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 10; - normal.setValue(0, 0, 1); // (0, 0, 1) or (0, 0, -1) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 10; + contacts[0].normal.setValue(0, 0, 1); // (0, 0, 1) or (0, 0, -1) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 10; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 10; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, 2.5); - depth = 7.5; - normal.setValue(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 2.5); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, 2.5)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -2.5); - depth = 7.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -2.5); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -2.5)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); @@ -1843,65 +1903,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(-2.5, 0, 0); - depth = 5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(-1.25, 0, 0); - depth = 7.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.25, 0, 0); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-3.75, 0, 0); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-3.75, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-3.75, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - contact.setValue(0.05, 0, 0); - depth = 10.1; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0.05, 0, 0); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); @@ -1918,59 +1984,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, 0); - depth = 5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, -1.25, 0); - depth = 7.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -1.25, 0); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -3.75, 0); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -3.75, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -3.75, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - contact.setValue(0, 0.05, 0); - depth = 10.1; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0.05, 0); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0.05, 0)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); @@ -1987,59 +2061,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -2.5); - depth = 5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -2.5); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, -1.25); - depth = 7.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -1.25); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -3.75); - depth = 2.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -3.75); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); - contact.setValue(0, 0, 0.05); - depth = 10.1; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0.05); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); @@ -2061,51 +2143,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(2.5, 0, 0); - depth = 2.5; - normal.setValue(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(2.5, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(2.5, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(2.5, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-2.5, 0, 0); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); @@ -2130,45 +2216,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, 2.5, 0); - depth = 2.5; - normal.setValue(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 2.5, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, 2.5, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 2.5, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -2.5, 0); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, 0); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -2.5, 0)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); @@ -2193,45 +2285,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, 2.5); - depth = 2.5; - normal.setValue(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, 2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -2.5); - depth = 2.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); @@ -2262,65 +2360,71 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(-2.5, 0, -5); - depth = 5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, -5); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, -5)); - depth = 5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, -5)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(-1.25, 0, -5); - depth = 7.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-1.25, 0, -5); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, -5)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-1.25, 0, -5)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-3.75, 0, -5); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-3.75, 0, -5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, -5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-3.75, 0, -5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); - contact.setValue(0.05, 0, -5); - depth = 10.1; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0.05, 0, -5); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, -5)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0.05, 0, -5)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); @@ -2337,59 +2441,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, -5); - depth = 5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, -5); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, -5)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, -5)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, -1.25, -5); - depth = 7.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -1.25, -5); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, -5)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -1.25, -5)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -3.75, -5); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -3.75, -5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, -5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -3.75, -5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); - contact.setValue(0, 0.05, -5); - depth = 10.1; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0.05, -5); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, -5)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0.05, -5)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); @@ -2406,59 +2518,67 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -2.5); - depth = 5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -2.5); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, -1.25); - depth = 7.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -1.25); + contacts[0].penetration_depth = 7.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); - depth = 7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25)); + contacts[0].penetration_depth = 7.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -3.75); - depth = 2.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -3.75); + contacts[0].penetration_depth= 2.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); - contact.setValue(0, 0, 0.05); - depth = 10.1; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0.05); + contacts[0].penetration_depth = 10.1; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); - depth = 10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); + contacts[0].penetration_depth = 10.1; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); @@ -2480,51 +2600,55 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); - contact.setValue(2.5, 0, -2.5); - depth = 2.5; - normal.setValue(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(2.5, 0, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(2.5, 0, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(2.5, 0, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); - contact.setValue(-2.5, 0, -2.5); - depth = 2.5; - normal.setValue(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(-2.5, 0, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(-2.5, 0, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); @@ -2549,45 +2673,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); - contact.setValue(0, 2.5, -2.5); - depth = 2.5; - normal.setValue(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 2.5, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, 2.5, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 2.5, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); - contact.setValue(0, -2.5, -2.5); - depth = 2.5; - normal.setValue(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, -2.5, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -2.5, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, -2.5, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); @@ -2612,45 +2742,51 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, 0); - depth = 5; - normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 0); + contacts[0].penetration_depth = 5; + contacts[0].normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); - depth = 5; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal, true); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); + contacts[0].penetration_depth = 5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); - contact.setValue(0, 0, 2.5); - depth = 2.5; - normal.setValue(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, 2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, 2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); - contact.setValue(0, 0, -2.5); - depth = 2.5; - normal.setValue(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos.setValue(0, 0, -2.5); + contacts[0].penetration_depth = 2.5; + contacts[0].normal.setValue(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -2.5)); - depth = 2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, &contact, &depth, &normal); + contacts.resize(1); + contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); + contacts[0].penetration_depth = 2.5; + contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); From 10abbd27546add9da5cbfbe4683c6832c9788707 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 22:21:40 -0400 Subject: [PATCH 10/12] Update all the rest intersection tests --- test/test_fcl_geometric_shapes.cpp | 305 +++++++++++++++++------------ 1 file changed, 180 insertions(+), 125 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 126f954d2..56594adde 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -911,7 +911,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); } -BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) +BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone) { Cylinder s1(5, 10); Cone s2(5, 10); @@ -3094,9 +3094,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) Transform3f transform; generateRandomTransform(extents, transform); -// Vec3f contact; -// FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); @@ -3108,8 +3106,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + contacts[0].pos.setValue(20, 0, 0); + contacts[0].penetration_depth = 0.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); @@ -3117,47 +3118,66 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + contacts[0].pos.setValue(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[0].pos = transform.transform(Vec3f(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0)); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(); - normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) + contacts[0].pos.setZero(); + contacts[0].penetration_depth = 20.0 + 10.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform; - normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) + contacts[0].pos = transform.transform(Vec3f()); + contacts[0].penetration_depth = 20.0 + 10.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); - normal.setValue(-1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(-1, 0, 0); + contacts[0].pos.setValue(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(-1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); + contacts[0].pos = transform.transform(Vec3f(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0)); + contacts[0].penetration_depth = 0.1; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); - normal.setValue(-1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(-1, 0, 0); + contacts[0].pos.setValue(-20, 0, 0); + contacts[0].penetration_depth = 0.0; + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); - normal.setValue(-1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; @@ -3176,9 +3196,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); @@ -3186,19 +3204,31 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal.setValue(1, 0, 0); + contacts[1].normal.setValue(1, 0, 0); + contacts[2].normal.setValue(1, 0, 0); + contacts[3].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = Vec3f(1, 0, 0); + contacts[1].normal = Vec3f(1, 0, 0); + contacts[2].normal = Vec3f(1, 0, 0); + contacts[3].normal = Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); @@ -3206,13 +3236,21 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) tf1 = Transform3f(); tf2 = Transform3f(q); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = Vec3f(1, 0, 0); + contacts[1].normal = Vec3f(1, 0, 0); + contacts[2].normal = Vec3f(1, 0, 0); + contacts[3].normal = Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(q); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(4); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); + contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) @@ -3226,24 +3264,25 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.51, 0, 0)); @@ -3251,15 +3290,16 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) @@ -3273,38 +3313,40 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } @@ -3319,36 +3361,38 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true); + contacts.resize(1); + // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); @@ -3366,31 +3410,32 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - normal.setValue(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd + contacts.resize(1); + contacts[0].normal.setValue(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); @@ -3402,18 +3447,19 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); - normal.setValue(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + // contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } -BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) +BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercone) { Cylinder s1(5, 10); Cone s2(5, 10); @@ -3424,52 +3470,57 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecylinder) Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; - // FCL_REAL depth; - Vec3f normal; - + std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10, 0, 0)); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); - normal.setValue(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, NULL); + contacts.resize(1); + // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - // testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10)); - normal.setValue(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); + contacts.resize(1); + contacts[0].normal.setValue(0, 0, 1); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); @@ -3602,7 +3653,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) -BOOST_AUTO_TEST_CASE(spheresphere) +BOOST_AUTO_TEST_CASE(shapeDistanceGJK_spheresphere) { Sphere s1(20); Sphere s2(10); @@ -3663,7 +3714,7 @@ BOOST_AUTO_TEST_CASE(spheresphere) BOOST_CHECK_FALSE(res); } -BOOST_AUTO_TEST_CASE(boxbox) +BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); @@ -3699,7 +3750,7 @@ BOOST_AUTO_TEST_CASE(boxbox) BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(boxsphere) +BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); @@ -3735,7 +3786,7 @@ BOOST_AUTO_TEST_CASE(boxsphere) BOOST_CHECK(res); } -BOOST_AUTO_TEST_CASE(cylindercylinder) +BOOST_AUTO_TEST_CASE(hapeDistanceGJK_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); @@ -3773,7 +3824,7 @@ BOOST_AUTO_TEST_CASE(cylindercylinder) -BOOST_AUTO_TEST_CASE(conecone) +BOOST_AUTO_TEST_CASE(hapeDistanceGJK_conecone) { Cone s1(5, 10); Cone s2(5, 10); @@ -3818,35 +3869,39 @@ void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distan Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); - Vec3f contactA; - Vec3f contactB; - FCL_REAL depthA; - FCL_REAL depthB; - Vec3f normalA; - Vec3f normalB; + std::vector contactsA; + std::vector contactsB; bool resA; bool resB; const double tol = 1e-6; - resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA); - resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB); + resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactsA); + resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactsB); + + // normal should be opposite + for (size_t i = 0; i < contactsB.size(); ++i) + contactsB[i].normal = -contactsB[i].normal; BOOST_CHECK(resA); BOOST_CHECK(resB); - BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same - BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite - BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same + BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, GST_LIBCCD, + contactsA, contactsB, + true, true, true, false, tol)); + + resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactsA); + resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactsB); - resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA); - resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB); + // normal should be opposite + for (size_t i = 0; i < contactsB.size(); ++i) + contactsB[i].normal = -contactsB[i].normal; BOOST_CHECK(resA); BOOST_CHECK(resB); - BOOST_CHECK(contactA.equal(contactB, tol)); - BOOST_CHECK(normalA.equal(-normalB, tol)); - BOOST_CHECK_CLOSE(depthA, depthB, tol); + BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, GST_INDEP, + contactsA, contactsB, + true, true, true, false, tol)); } BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes) From d083df2be655513c7c8f8d63695e42ebc6da9e66 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 23:38:23 -0400 Subject: [PATCH 11/12] Give more tolerance for clang build --- test/test_fcl_geometric_shapes.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 56594adde..ba01f027f 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -746,7 +746,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) @@ -842,7 +842,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); @@ -888,7 +888,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.001, 0, 0)); @@ -908,7 +908,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); } BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone) @@ -966,7 +966,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone) tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.01)); @@ -1372,7 +1372,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; @@ -1380,7 +1380,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) - testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); From 75b59e69ad06ac96293ccf329b86a07f61cd6f1a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Oct 2015 23:46:13 -0400 Subject: [PATCH 12/12] Remove a legacy function in test_fcl_geometric_shapes.cpp --- test/test_fcl_geometric_shapes.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index ba01f027f..e581c0be7 100644 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -356,22 +356,6 @@ void getContactPointsFromResult(std::vector& contacts, const Colli } } -template -FCL_DEPRECATED -void testShapeIntersection(const S1& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - GJKSolverType solver_type, - bool expected_res, - Vec3f* expected_point, - FCL_REAL* expected_depth, - Vec3f* expected_normal, - bool check_opposite_normal = false, - FCL_REAL tol = 1e-9) -{ - // do nothing - BOOST_CHECK(false); -} - template void testShapeIntersection( const S1& s1, const Transform3f& tf1,