Skip to content
Snippets Groups Projects
Commit 9dd83445 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Modify test_fcl_capsule_capsule 

  - use API distance function instead of selecting gjk solver.
parent 3e3c3a20
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
test/test_fcl_capsule_capsule.cpp
+ 96
− 77
View file @ 9dd83445
......@@ -36,119 +36,138 @@
/** \author Karsten Knese <Karsten.Knese@googlemail.com> */
#define BOOST_TEST_MODULE "FCL_CAPSULE_CAPSULE"
#include <boost/test/unit_test.hpp>
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include "fcl/collision.h"
#include "fcl/shape/geometric_shapes.h"
#include "fcl/narrowphase/narrowphase.h"
#include <boost/test/unit_test.hpp>
#include "math.h"
#include <cmath>
#include <fcl/distance.h>
#include <fcl/math/transform.h>
#include <fcl/collision.h>
#include <fcl/collision_object.h>
#include <fcl/shape/geometric_shapes.h>
using namespace fcl;
typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
GJKSolver_indep solver;
Capsule s1(5, 10);
Capsule s2(5, 10);
Transform3f tf1;
Transform3f tf2 (Vec3f(20.1, 0,0));
Vec3f closest_p1, closest_p2;
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
Transform3f transform;
Transform3f transform2;
transform2 = Transform3f(Vec3f(20.1, 0,0));
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
bool res;
FCL_REAL dist;
distance (&o1, &o2, distanceRequest, distanceResult);
res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
BOOST_CHECK(fabs(dist - 10.1) < 0.001);
BOOST_CHECK(res);
std::cerr << "Applied translation on two capsules";
std::cerr << " T1 = " << tf1.getTranslation()
<< ", T2 = " << tf2.getTranslation() << std::endl;
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
GJKSolver_indep solver;
Capsule s1(5, 10);
Capsule s2(5, 10);
Transform3f tf1;
Transform3f tf2 (Vec3f(20, 20,0));
Vec3f closest_p1, closest_p2;
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
Transform3f transform;
Transform3f transform2;
transform2 = Transform3f(Vec3f(20, 20,0));
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
bool res;
FCL_REAL dist;
distance (&o1, &o2, distanceRequest, distanceResult);
res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
float expected = sqrt(800) - 10;
BOOST_CHECK(fabs(expected-dist) < 0.01);
BOOST_CHECK(res);
std::cerr << "Applied translation on two capsules";
std::cerr << " T1 = " << tf1.getTranslation()
<< ", T2 = " << tf2.getTranslation() << std::endl;
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
FCL_REAL expected = sqrt(800) - 10;
BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
GJKSolver_indep solver;
Capsule s1(5, 10);
Capsule s2(5, 10);
Vec3f closest_p1, closest_p2;
Transform3f tf1;
Transform3f tf2 (Vec3f(0,0,20.1));
Transform3f transform;
Transform3f transform2;
transform2 = Transform3f(Vec3f(0,0,20.1));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
bool res;
FCL_REAL dist;
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
distance (&o1, &o2, distanceRequest, distanceResult);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
std::cerr << "Applied translation on two capsules";
std::cerr << " T1 = " << tf1.getTranslation()
<< ", T2 = " << tf2.getTranslation() << std::endl;
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
{
GJKSolver_indep solver;
Capsule s1(5, 10);
Capsule s2(5, 10);
Vec3f closest_p1, closest_p2;
Transform3f transform;
Transform3f transform2;
transform2 = Transform3f(Vec3f(0,0,25.1));
Matrix3f rot2;
rot2.setEulerZYX(0,M_PI_2,0);
transform2.setRotation(rot2);
bool res;
FCL_REAL dist;
res = solver.shapeDistance<Capsule, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2);
std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl;
std::cerr << "applied transformation of two caps: " << transform.getRotation() << " & " << transform2.getRotation() << std::endl;
std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl;
BOOST_CHECK(fabs(dist - 5.1) < 0.001);
BOOST_CHECK(res);
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
Transform3f tf1;
Transform3f tf2 (Quaternion3f (sqrt (2)/2, 0, sqrt (2)/2, 0),
Vec3f(0,0,25.1));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
std::cerr << "Applied rotation and translation on two capsules" << std::endl;
std::cerr << "R1 = " << tf1.getRotation ()
<< ", T1 = " << tf1.getTranslation() << std::endl
<< "R2 = " << tf2.getRotation ()
<< ", T2 = " << tf2.getTranslation() << std::endl;
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
const fcl::Vec3f& p1 = distanceResult.nearest_points [0];
const fcl::Vec3f& p2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
CHECK_CLOSE_TO_0 (p1 [0], 1e-4);
CHECK_CLOSE_TO_0 (p1 [1], 1e-4);
BOOST_CHECK_CLOSE (p1 [2], 10, 1e-4);
CHECK_CLOSE_TO_0 (p2 [0], 1e-4);
CHECK_CLOSE_TO_0 (p2 [1], 1e-4);
BOOST_CHECK_CLOSE (p2 [2], 20.1, 1e-4);
}
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