diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h index e299e26ed..cfe58dac2 100644 --- a/include/fcl/collision_data.h +++ b/include/fcl/collision_data.h @@ -56,6 +56,31 @@ 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() : normal(Vec3f()), pos(Vec3f()), penetration_depth(0.0) {} + + /// @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 less 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 fbb5ff0a1..ef4860081 100644 --- a/include/fcl/narrowphase/narrowphase.h +++ b/include/fcl/narrowphase/narrowphase.h @@ -38,32 +38,54 @@ #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 + FCL_DEPRECATED 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; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + std::vector* contacts = NULL) 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); @@ -74,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); @@ -94,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); @@ -115,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); @@ -134,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); @@ -163,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); @@ -194,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() { @@ -243,149 +241,184 @@ 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::max_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; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule &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 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 Halfspace& s1, const Transform3f& tf1, const Sphere& 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 Halfspace& 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 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 Halfspace& s1, const Transform3f& tf1, const Capsule& 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 Halfspace& s1, const Transform3f& tf1, const Cylinder& 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 Cone& 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 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 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 Plane& s1, const Transform3f& tf1, const Sphere& 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 Plane& 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 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 Plane& s1, const Transform3f& tf1, const Capsule& 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 Plane& s1, const Transform3f& tf1, const Cylinder& 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 Plane& s1, const Transform3f& tf1, const Cone& 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<> @@ -445,24 +478,32 @@ bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Tran struct GJKSolver_indep { /// @brief intersection checking between two shapes + /// @deprecated use shapeIntersect(const S1&, const Transform3f&, const S2&, const Transform3f&, std::vector*) const template + FCL_DEPRECATED 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; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect(const S1& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + std::vector* contacts = NULL) 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: @@ -476,9 +517,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; @@ -495,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); @@ -544,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); @@ -593,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; @@ -632,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); @@ -681,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); @@ -731,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() @@ -793,148 +814,184 @@ 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::max_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_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; +bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3f& tf1, + const Capsule &s2, const Transform3f& tf2, + std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule &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 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 Halfspace& s1, const Transform3f& tf1, const Sphere& 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 Halfspace& 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 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 Halfspace& s1, const Transform3f& tf1, const Capsule& 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 Halfspace& s1, const Transform3f& tf1, const Cylinder& 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 Cone& 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 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 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 Plane& s1, const Transform3f& tf1, const Sphere& 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 Plane& 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 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 Plane& s1, const Transform3f& tf1, const Capsule& 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 Plane& s1, const Transform3f& tf1, const Cylinder& 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 Plane& s1, const Transform3f& tf1, const Cone& 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 b1cc27afa..24e056bd3 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -298,14 +298,14 @@ class OcTreeSolver Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL)) + if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { AABB overlap_part; AABB aabb1, aabb2; computeBV(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); } } @@ -326,7 +326,7 @@ class OcTreeSolver bool is_intersect = false; if(!crequest->enable_contact) { - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL)) + if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { is_intersect = true; if(cresult->numContacts() < crequest->num_max_contacts) @@ -335,15 +335,29 @@ class OcTreeSolver } else { - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - - if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal)) + std::vector contacts; + if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts)) { is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth)); + if(crequest->num_max_contacts > cresult->numContacts()) + { + const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + 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)); + } } } @@ -370,7 +384,7 @@ class OcTreeSolver Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL)) + if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { AABB overlap_part; AABB aabb1, aabb2; @@ -893,14 +907,29 @@ class OcTreeSolver constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); - Vec3f contact; - FCL_REAL depth; - Vec3f normal; - if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal)) + std::vector contacts; + if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts)) { is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth)); + if(crequest->num_max_contacts > cresult->numContacts()) + { + const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + 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 01f98d149..809b581b5 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -39,6 +39,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" @@ -50,7 +52,6 @@ namespace fcl { - /// @brief Traversal node for collision between two shapes template class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase @@ -78,18 +79,34 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase bool is_collision = false; if(request.enable_contact) { - Vec3f contact_point, normal; - FCL_REAL penetration_depth; - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal)) + std::vector contacts; + if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contacts)) { is_collision = true; if(request.num_max_contacts > result->numContacts()) - result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth)); + { + const size_t free_space = request.num_max_contacts - result->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + 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()) @@ -109,7 +126,7 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase } else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) { - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL)) + if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL)) { AABB aabb1, aabb2; computeBV(*model1, tf1, aabb1); diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 1b518589b..1cec6cb30 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -749,7 +749,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 @@ -773,7 +773,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 6f3be3724..246d84d63 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -774,7 +774,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 @@ -798,7 +798,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 2ba31b422..1f6ddaa6b 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -40,6 +40,11 @@ namespace fcl { +bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2) +{ + return _cp1.penetration_depth < _cp2.penetration_depth; +} + bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts()); diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 0fb235b99..6bcec4886 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -197,35 +197,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; @@ -266,35 +265,28 @@ 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 = tf2.transform(Vec3f()) - tf1.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 the centers of two sphere are at the same position, the normal is (0, 0, 0). - // Otherwise, normal is pointing from center of object 1 to center of object 2 - if(normal) + if(contacts) { - if(len > 0) - *normal = diff / len; - else - *normal = diff; + // If the centers of two sphere are at the same position, the normal is (0, 0, 0). + // Otherwise, normal is pointing from center of object 1 to center of object 2 + 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) @@ -735,17 +727,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) @@ -962,21 +943,21 @@ 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; normalR = &R1; 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; normalR = &R1; invert_normal = (tmp < 0); @@ -1031,7 +1012,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, invert_normal = (tmp < 0); code = 6; } - + FCL_REAL fudge2(1.0e-6); Q += fudge2; @@ -1045,7 +1026,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1063,7 +1044,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1075,13 +1056,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 > 0) { *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) @@ -1100,7 +1081,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1118,7 +1099,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1130,13 +1111,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 > 0) { *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) @@ -1155,7 +1136,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1173,7 +1154,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, if(s2 > 0) { *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) @@ -1185,13 +1166,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 > 0) { *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) @@ -1205,7 +1186,7 @@ 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 @@ -1214,27 +1195,27 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, normal = normalR->getColumn(best_col_id); else normal = R1 * normalC; - - if(invert_normal) + + if(invert_normal) normal.negate(); *depth = -s; // s is negative when the boxes are in collision // 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(T2); @@ -1247,17 +1228,17 @@ 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)); - contacts.push_back(ContactPoint(-normal,pb,-*depth)); + contacts.push_back(ContactPoint(normal,pb,-*depth)); *return_code = code; - + return 1; } @@ -1265,7 +1246,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; @@ -1291,9 +1272,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); @@ -1303,30 +1284,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; @@ -1336,28 +1317,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; @@ -1377,7 +1358,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]; @@ -1413,13 +1394,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]; @@ -1432,36 +1413,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])); + 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])); + 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; @@ -1471,13 +1452,13 @@ 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]] + (*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; } @@ -1486,14 +1467,12 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, return cnum; } -bool compareContactPoints(const ContactPoint& c1,const ContactPoint& c2) { return c1.depth < c2.depth; } // Accending order, assuming penetration depth is a negative number! - 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(), @@ -1501,18 +1480,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) - { - if(contacts.size() > 0) - { - std::sort(contacts.begin(), contacts.end(), compareContactPoints); - *contact_points = contacts[0].point; - if(penetration_depth_) *penetration_depth_ = -contacts[0].depth; - } - } + if(contacts_) + *contacts_ = contacts; return return_code != 0; } @@ -1537,20 +1506,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 @@ -1577,13 +1555,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(); @@ -1629,9 +1609,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; } @@ -1639,7 +1624,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); @@ -1655,9 +1640,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; } @@ -1670,13 +1660,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; @@ -1686,7 +1676,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); @@ -1702,9 +1692,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; } @@ -1727,9 +1722,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; } } @@ -1738,7 +1739,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); @@ -1755,9 +1756,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; } } @@ -1782,10 +1789,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; } } @@ -2006,23 +2018,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 @@ -2034,7 +2054,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); @@ -2090,9 +2110,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; } @@ -2123,12 +2148,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); @@ -2156,46 +2181,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; } } @@ -2228,10 +2267,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); @@ -2249,9 +2290,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; } } @@ -2292,27 +2338,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); @@ -2329,9 +2387,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; } } @@ -2380,41 +2444,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; } } @@ -2549,7 +2618,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); @@ -2589,134 +2658,140 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1, // | triangle |/////|////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ +void flipNormal(std::vector& contacts) +{ + for (std::vector::iterator it = contacts.begin(); it != contacts.end(); ++it) + (*it).normal *= -1.0; +} + 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 Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Cone& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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; @@ -2728,7 +2803,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; @@ -2740,7 +2815,7 @@ bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const T 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; @@ -2752,99 +2827,99 @@ bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, con 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 Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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); } @@ -2976,131 +3051,131 @@ 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 Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Cone& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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; @@ -3112,7 +3187,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; @@ -3124,7 +3199,7 @@ bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Tr 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; @@ -3136,99 +3211,99 @@ bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, cons 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 Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, - Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const + std::vector* contacts) const { - const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal); - if (normal) (*normal) *= -1.0; + const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); + if (contacts) flipNormal(*contacts); return res; } 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 a3c5d6faa..d791b205b 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -116,7 +116,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); } @@ -155,7 +155,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); } @@ -164,7 +164,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); } @@ -173,7 +173,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); } @@ -182,7 +182,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 f3f0d6897..e581c0be7 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; @@ -119,8 +120,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 +134,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 +150,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 +163,231 @@ 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 << "\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" + << "\n"; + return false; + } + + // 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; + break; + } + } + + if (index_to_actual_contacts[i] == -1) + foundAll = false; + } + + if (!foundAll) + { + 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" + << "[ 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 +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; + request.num_max_contacts = std::numeric_limits::max(); 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 +396,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 +412,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,82 +457,107 @@ 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) +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); @@ -391,12 +584,10 @@ void testBoxBoxContactPoints(const Matrix3f& R) Transform3f tf1 = Transform3f(Vec3f(0, 0, -50)); Transform3f tf2 = Transform3f(R); - Vec3f normal; - Vec3f point; - double penetration; + std::vector contacts; // Make sure the two boxes are colliding - bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &point, &penetration, &normal); + bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contacts); BOOST_CHECK(res); // Compute global vertices @@ -404,10 +595,19 @@ void testBoxBoxContactPoints(const Matrix3f& R) 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 - BOOST_CHECK(vertices[0].equal(point)); + 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) @@ -421,9 +621,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); @@ -431,33 +629,53 @@ 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); + 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); - testShapeInersection(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) @@ -479,38 +697,40 @@ 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); - testShapeInersection(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. - testShapeInersection(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)); - 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); + 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); - testShapeInersection(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, false, 1e-4); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) @@ -524,49 +744,51 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - 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) @@ -580,37 +802,39 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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, false, 1e-5); 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) @@ -624,50 +848,54 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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, false, 1e-5); 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); + 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); - testShapeInersection(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, false, 1e-5); } -BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder) +BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone) { Cylinder s1(5, 10); Cone s2(5, 10); @@ -678,55 +906,59 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); + 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); - testShapeInersection(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, false, 1e-5); 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) @@ -861,73 +1093,79 @@ 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); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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)); - testShapeInersection(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)); - 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); + 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)); - testShapeInersection(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) @@ -941,67 +1179,71 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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) @@ -1015,77 +1257,84 @@ 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); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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)); - testShapeInersection(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)); - 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); + contacts.resize(1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) @@ -1099,71 +1348,76 @@ 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) - testShapeInersection(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, true, true, true, true); 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); + 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, true, true, true, true); 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); + 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); + contacts.resize(1); + testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) @@ -1177,73 +1431,79 @@ 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1252,67 +1512,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, 0); - depth = 5; - normal.setValue(0, -1, 0); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1321,67 +1589,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -5); - depth = 10; - normal.setValue(0, 0, -1); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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) @@ -1395,67 +1671,71 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1464,61 +1744,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1527,61 +1813,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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) @@ -1595,73 +1887,79 @@ 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1670,67 +1968,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, 0); - depth = 5; - normal.setValue(0, -1, 0); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1739,67 +2045,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -2.5); - depth = 5; - normal.setValue(0, 0, -1); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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) @@ -1813,67 +2127,71 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1882,61 +2200,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -1945,61 +2269,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); } @@ -2014,73 +2344,79 @@ 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -2089,67 +2425,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, -2.5, -5); - depth = 5; - normal.setValue(0, -1, 0); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -2158,67 +2502,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) tf1 = Transform3f(); tf2 = Transform3f(); - contact.setValue(0, 0, -2.5); - depth = 5; - normal.setValue(0, 0, -1); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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) @@ -2232,67 +2584,71 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -2301,61 +2657,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); @@ -2364,61 +2726,67 @@ 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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); } @@ -2710,75 +3078,95 @@ 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)); - 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); + 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)); - 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); + 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); - testShapeInersection(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) - testShapeInersection(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) - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - 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) @@ -2792,9 +3180,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); @@ -2802,33 +3188,53 @@ 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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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)); - 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); + 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); - testShapeInersection(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) @@ -2842,40 +3248,42 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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)); - 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 + 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); - testShapeInersection(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. - // testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, true, NULL, NULL, &normal); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) @@ -2889,39 +3297,41 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) @@ -2935,40 +3345,42 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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. - // testShapeInersection(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); + 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)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) @@ -2982,54 +3394,56 @@ 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. - testShapeInersection(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. - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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. - // testShapeInersection(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); + 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); - testShapeInersection(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. - // testShapeInersection(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); @@ -3040,56 +3454,61 @@ 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. - testShapeInersection(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. - testShapeInersection(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)); - testShapeInersection(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)); - testShapeInersection(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)); - testShapeInersection(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)); - testShapeInersection(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); - testShapeInersection(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); - testShapeInersection(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. - // testShapeInersection(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); + 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)); - testShapeInersection(s1, tf1, s2, tf2, GST_INDEP, false); + testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } @@ -3218,7 +3637,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) -BOOST_AUTO_TEST_CASE(spheresphere) +BOOST_AUTO_TEST_CASE(shapeDistanceGJK_spheresphere) { Sphere s1(20); Sphere s2(10); @@ -3279,7 +3698,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); @@ -3315,7 +3734,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); @@ -3351,7 +3770,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); @@ -3389,7 +3808,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); @@ -3434,35 +3853,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) diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp index e9612b396..354192963 100644 --- a/test/test_fcl_sphere_capsule.cpp +++ b/test/test_fcl_sphere_capsule.cpp @@ -59,7 +59,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) @@ -73,7 +73,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) @@ -87,7 +87,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) @@ -103,30 +103,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) @@ -142,11 +144,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);