Commit 428c6dc2 authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Merge remote-tracking branch 'upstream/master' into fix_inconsistent_normal

parents 75e0980e 45e6c029
file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h)
file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h)
set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_CONFIG_OUT_DIR)
configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h")
......
This diff is collapsed.
file(GLOB_RECURSE FCL_SOURCE_CODE ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
if(FCL_STATIC_LIBRARY)
add_library(${PROJECT_NAME} STATIC ${FCL_SOURCE_CODE})
add_library(${PROJECT_NAME} STATIC ${FCL_HEADERS} ${FCL_SOURCE_CODE})
else()
add_library(${PROJECT_NAME} SHARED ${FCL_SOURCE_CODE})
add_library(${PROJECT_NAME} SHARED ${FCL_HEADERS} ${FCL_SOURCE_CODE})
endif()
target_link_libraries(${PROJECT_NAME} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${Boost_LIBRARIES})
......
......@@ -89,6 +89,8 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
if(other_objs.empty()) return;
if(size() > 0)
BroadPhaseCollisionManager::registerObjects(other_objs);
else
......@@ -272,6 +274,8 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
void SaPCollisionManager::setup()
{
if(size() == 0) return;
FCL_REAL scale[3];
scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
......
......@@ -634,6 +634,8 @@ bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
if(other_objs.empty()) return;
if(size() > 0)
{
BroadPhaseCollisionManager::registerObjects(other_objs);
......
......@@ -657,6 +657,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nod
void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
if(other_objs.empty()) return;
if(size() > 0)
{
BroadPhaseCollisionManager::registerObjects(other_objs);
......
This diff is collapsed.
......@@ -149,6 +149,26 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance)
broad_phase_self_distance_test(200, 5000);
}
/// check broad phase collision for empty collision object set and queries
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty)
{
broad_phase_collision_test(2000, 0, 0, 10, false, false);
broad_phase_collision_test(2000, 0, 1000, 10, false, false);
broad_phase_collision_test(2000, 100, 0, 10, false, false);
broad_phase_collision_test(2000, 0, 0, 10, false, true);
broad_phase_collision_test(2000, 0, 1000, 10, false, true);
broad_phase_collision_test(2000, 100, 0, 10, false, true);
broad_phase_collision_test(2000, 0, 0, 10, true, false);
broad_phase_collision_test(2000, 0, 1000, 10, true, false);
broad_phase_collision_test(2000, 100, 0, 10, true, false);
broad_phase_collision_test(2000, 0, 0, 10, true, true);
broad_phase_collision_test(2000, 0, 1000, 10, true, true);
broad_phase_collision_test(2000, 100, 0, 10, true, true);
}
/// check broad phase collision and self collision, only return collision or not
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary)
{
......
......@@ -3377,3 +3377,177 @@ BOOST_AUTO_TEST_CASE(conecone)
template<typename S1, typename S2>
void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
{
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;
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);
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
resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
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_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
{
// This test check whether a shape intersection algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule intersection
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently short distance so that all the primitive shapes can intersect
FCL_REAL distance = 5.0;
// If new shape intersection algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeIntersection(box, sphere, distance);
// testReversibleShapeIntersection(box, capsule, distance);
// testReversibleShapeIntersection(box, cone, distance);
// testReversibleShapeIntersection(box, cylinder, distance);
testReversibleShapeIntersection(box, plane, distance);
testReversibleShapeIntersection(box, halfspace, distance);
testReversibleShapeIntersection(sphere, capsule, distance);
// testReversibleShapeIntersection(sphere, cone, distance);
// testReversibleShapeIntersection(sphere, cylinder, distance);
testReversibleShapeIntersection(sphere, plane, distance);
testReversibleShapeIntersection(sphere, halfspace, distance);
// testReversibleShapeIntersection(capsule, cone, distance);
// testReversibleShapeIntersection(capsule, cylinder, distance);
testReversibleShapeIntersection(capsule, plane, distance);
testReversibleShapeIntersection(capsule, halfspace, distance);
// testReversibleShapeIntersection(cone, cylinder, distance);
testReversibleShapeIntersection(cone, plane, distance);
testReversibleShapeIntersection(cone, halfspace, distance);
testReversibleShapeIntersection(cylinder, plane, distance);
testReversibleShapeIntersection(cylinder, halfspace, distance);
testReversibleShapeIntersection(plane, halfspace, distance);
}
template<typename S1, typename S2>
void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
{
Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
FCL_REAL distA;
FCL_REAL distB;
Vec3f p1A;
Vec3f p1B;
Vec3f p2A;
Vec3f p2B;
bool resA;
bool resB;
const double tol = 1e-6;
resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
BOOST_CHECK(p2A.equal(p1B, tol));
resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol);
BOOST_CHECK(p1A.equal(p2B, tol));
BOOST_CHECK(p2A.equal(p1B, tol));
}
BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
{
// This test check whether a shape distance algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule distance
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently long distance so that all the primitive shapes CANNOT intersect
FCL_REAL distance = 15.0;
// If new shape distance algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeDistance(box, sphere, distance);
// testReversibleShapeDistance(box, capsule, distance);
// testReversibleShapeDistance(box, cone, distance);
// testReversibleShapeDistance(box, cylinder, distance);
// testReversibleShapeDistance(box, plane, distance);
// testReversibleShapeDistance(box, halfspace, distance);
testReversibleShapeDistance(sphere, capsule, distance);
// testReversibleShapeDistance(sphere, cone, distance);
// testReversibleShapeDistance(sphere, cylinder, distance);
// testReversibleShapeDistance(sphere, plane, distance);
// testReversibleShapeDistance(sphere, halfspace, distance);
// testReversibleShapeDistance(capsule, cone, distance);
// testReversibleShapeDistance(capsule, cylinder, distance);
// testReversibleShapeDistance(capsule, plane, distance);
// testReversibleShapeDistance(capsule, halfspace, distance);
// testReversibleShapeDistance(cone, cylinder, distance);
// testReversibleShapeDistance(cone, plane, distance);
// testReversibleShapeDistance(cone, halfspace, distance);
// testReversibleShapeDistance(cylinder, plane, distance);
// testReversibleShapeDistance(cylinder, halfspace, distance);
// testReversibleShapeDistance(plane, halfspace, distance);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment