Commit 4affdb50 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

remove most references to ccd, ref #102

parent 59aca862
Pipeline #15038 failed with stage
in 50 seconds
......@@ -2,11 +2,12 @@
Dependencies:
============
- Eigen
- Boost (thread, date_time, unit_test_framework, filesystem)
- libccd (available at http://libccd.danfis.cz/)
- octomap (optional dependency, available at http://octomap.github.com)
- Qhull (optional dependency, available at http://www.qhull.org)
Boost and libccd are mandatory dependencies. If octomap is not found,
Boost and eigen are mandatory dependencies. If octomap is not found,
collision detection with octrees will not be possible.
For installation, CMake will also be needed (http://cmake.org).
......
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <iostream>
#include <hpp/fcl/collision.h>
#include <boost/foreach.hpp>
using namespace std;
using namespace hpp::fcl;
int main(int argc, char** argv)
{
shared_ptr<Box> box0(new Box(1,1,1));
shared_ptr<Box> box1(new Box(1,1,1));
GJKSolver_libccd solver;
Vec3f contact_points;
FCL_REAL distance;
Vec3f normal;
Transform3f tf0, tf1;
tf0.setIdentity();
tf0.setTranslation(Vec3f(.9,0,0));
tf0.setQuatRotation(Quaternion3f(.6, .8, 0, 0));
tf1.setIdentity();
bool res = solver.shapeIntersect(*box0, tf0, *box1, tf1, distance, true, &contact_points, &normal);
cout << "contact points: " << contact_points << endl;
cout << "signed distance: " << distance << endl;
cout << "normal: " << normal << endl;
cout << "result: " << res << endl;
static const int num_max_contacts = std::numeric_limits<int>::max();
static const bool enable_contact = true;
hpp::fcl::CollisionResult result;
hpp::fcl::CollisionRequest request(enable_contact, num_max_contacts, false);
CollisionObject co0(box0, tf0);
CollisionObject co1(box1, tf1);
hpp::fcl::collide(&co0, &co1, request, result);
vector<Contact> contacts;
result.getContacts(contacts);
cout << contacts.size() << " contacts found" << endl;
BOOST_FOREACH(Contact& contact, contacts) {
cout << "position: " << contact.pos << endl;
}
}
......@@ -51,7 +51,7 @@ using namespace hpp::fcl;
FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere)
{
Sphere s1(20);
Sphere s2(20);
......@@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
}
}
BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
BOOST_AUTO_TEST_CASE(consistency_distance_boxbox)
{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
......@@ -233,7 +233,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
}
}
BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder)
{
Cylinder s1(5, 10);
Cylinder s2(5, 10);
......@@ -324,7 +324,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
}
}
BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd)
BOOST_AUTO_TEST_CASE(consistency_distance_conecone)
{
Cone s1(5, 10);
Cone s2(5, 10);
......@@ -791,7 +791,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere)
{
Sphere s1(20);
Sphere s2(10);
......@@ -1008,7 +1008,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
BOOST_CHECK_FALSE(res);
}
BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
BOOST_AUTO_TEST_CASE(consistency_collision_boxbox)
{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
......@@ -1128,7 +1128,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
BOOST_CHECK(res);
}
BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
BOOST_AUTO_TEST_CASE(consistency_collision_spherebox)
{
Sphere s1(20);
Box s2(5, 5, 5);
......@@ -1248,7 +1248,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
BOOST_CHECK_FALSE(res);
}
BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder)
{
Cylinder s1(5, 10);
Cylinder s2(5, 10);
......@@ -1335,7 +1335,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
BOOST_CHECK_FALSE(res);
}
BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
BOOST_AUTO_TEST_CASE(consistency_collision_conecone)
{
Cone s1(5, 10);
Cone s2(5, 10);
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Martin Felis <martin.felis@iwr.uni-heidelberg.de> */
#define BOOST_TEST_MODULE FCL_SPHERE_CAPSULE
#include <boost/test/included/unit_test.hpp>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
using namespace hpp::fcl;
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 200));
FCL_REAL distance;
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, distance, false, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., 50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., -200));
FCL_REAL distance;
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, distance, false, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (150., 0., 0.));
FCL_REAL distance;
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, distance, false, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Matrix3f rotation;
// rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
setEulerZYX(rotation, M_PI * 0.5, 0., 0.);
Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.));
FCL_REAL distance;
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, distance, false, NULL, NULL));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 125));
FCL_REAL penetration = 0.;
Vec3f contact_point;
Vec3f normal;
bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal);
BOOST_CHECK (is_intersecting);
BOOST_CHECK (penetration == 25.);
BOOST_CHECK (isEqual(Vec3f (0., 0., 1.), normal));
BOOST_CHECK (isEqual(Vec3f (0., 0., 0.), contact_point));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., 0));
Capsule capsule (50, 200.);
Matrix3f rotation;
// rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
setEulerZYX(rotation, M_PI * 0.5, 0., 0.);
Transform3f capsule_transform (rotation, Vec3f (0., 50., 75));
FCL_REAL penetration = 0.;
Vec3f contact_point;
Vec3f normal;
bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contact_point, &penetration, &normal);
BOOST_CHECK (is_intersecting);
BOOST_CHECK_CLOSE (25, penetration, solver.collision_tolerance);
BOOST_CHECK (isEqual(Vec3f (0., 0., 1.),normal));
BOOST_CHECK (isEqual(Vec3f (0., 0., 50.), contact_point, solver.collision_tolerance));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 100));
FCL_REAL distance;
BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
{
GJKSolver_libccd solver;
Sphere sphere1 (50);
Transform3f sphere1_transform;
sphere1_transform.setTranslation (Vec3f (0., 0., -50));
Capsule capsule (50, 200.);
Transform3f capsule_transform (Vec3f (0., 0., 175));
FCL_REAL distance = 0.;
Vec3f p1;
Vec3f p2;
bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance);
BOOST_CHECK (is_separated);
BOOST_CHECK (distance == 25.);
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment