Commit e9d9f0e5 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add a unit-test that compares results from Box and Convex.

parent 5d328482
......@@ -43,6 +43,7 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
#include "test_fcl_utility.h"
#include <iostream>
......@@ -54,6 +55,22 @@ FCL_REAL tol_gjk = 0.01;
GJKSolver_indep solver1;
GJKSolver_indep solver2;
Eigen::IOFormat fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
Eigen::IOFormat pyfmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
namespace hpp {
namespace fcl {
std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
{
return os << "[ " <<
tf.getTranslation().format(fmt)
<< ", "
<< tf.getQuatRotation().coeffs().format(fmt)
<< " ]" ;
}
}
}
#define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
template <typename S1, typename S2>
......@@ -164,7 +181,6 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
CollisionResult result;
Vec3f contact;
FCL_REAL depth;
Vec3f normal; // normal direction should be from object 1 to object 2
bool res;
......@@ -188,6 +204,70 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
}
}
template <typename Sa, typename Sb> void compareShapeIntersection (
const Sa& sa, const Sb& sb,
const Transform3f& tf1, const Transform3f& tf2,
FCL_REAL tol = 1e-9)
{
CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1);
CollisionResult resA, resB;
collide(&sa, tf1, &sa, tf2, request, resA);
collide(&sb, tf1, &sb, tf2, request, resB);
BOOST_CHECK_EQUAL(resA.isCollision(), resB.isCollision());
BOOST_CHECK_EQUAL(resA.numContacts(), resB.numContacts());
if (resA.isCollision() && resB.isCollision()) {
Contact cA = resA.getContact(0),
cB = resB.getContact(0);
BOOST_TEST_MESSAGE(
tf1 << '\n'
<< cA.pos.format(pyfmt) << '\n'
<< '\n'
<< tf2 << '\n'
<< cB.pos.format(pyfmt) << '\n'
);
// Only warnings because there are still some bugs.
BOOST_WARN_SMALL((cA.pos - cB.pos ).squaredNorm(), tol);
BOOST_WARN_SMALL((cA.normal - cB.normal).squaredNorm(), tol);
} else {
BOOST_CHECK_CLOSE(resA.distance_lower_bound, resB.distance_lower_bound, tol); // distances should be same
}
}
template <typename Sa, typename Sb> void compareShapeDistance (
const Sa& sa, const Sb& sb,
const Transform3f& tf1, const Transform3f& tf2,
FCL_REAL tol = 1e-9)
{
DistanceRequest request (true);
DistanceResult resA, resB;
distance(&sa, tf1, &sa, tf2, request, resA);
distance(&sb, tf1, &sb, tf2, request, resB);
BOOST_MESSAGE(
tf1 << '\n'
<< resA.normal.format(pyfmt) << '\n'
<< resA.nearest_points[0].format(pyfmt) << '\n'
<< resA.nearest_points[1].format(pyfmt) << '\n'
<< '\n'
<< tf2 << '\n'
<< resB.normal.format(pyfmt) << '\n'
<< resB.nearest_points[0].format(pyfmt) << '\n'
<< resB.nearest_points[1].format(pyfmt) << '\n'
);
//BOOST_WARN_CLOSE(resA.min_distance, resB.min_distance, tol);
BOOST_CHECK_CLOSE(resA.min_distance, resB.min_distance, tol);
// Only warnings because there are still some bugs.
BOOST_WARN_SMALL((resA.normal - resA.normal).squaredNorm(), tol);
BOOST_WARN_SMALL((resA.nearest_points[0] - resB.nearest_points[0]).squaredNorm(), tol);
BOOST_WARN_SMALL((resA.nearest_points[1] - resB.nearest_points[1]).squaredNorm(), tol);
}
BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
{
Cylinder s1 (0.029, 0.1);
......@@ -443,6 +523,82 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
}
}
BOOST_AUTO_TEST_CASE(compare_convex_box)
{
FCL_REAL l = 1, w = 1, d = 1;
Box box(l*2, w*2, d*2);
Vec3f pts[8];
pts[0] = Vec3f( l, w, d);
pts[1] = Vec3f( l, w,-d);
pts[2] = Vec3f( l,-w, d);
pts[3] = Vec3f( l,-w,-d);
pts[4] = Vec3f(-l, w, d);
pts[5] = Vec3f(-l, w,-d);
pts[6] = Vec3f(-l,-w, d);
pts[7] = Vec3f(-l,-w,-d);
std::vector<int> polygons;
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(2);
polygons.push_back(3);
polygons.push_back(1);
polygons.push_back(4);
polygons.push_back(2);
polygons.push_back(6);
polygons.push_back(7);
polygons.push_back(3);
polygons.push_back(4);
polygons.push_back(4);
polygons.push_back(5);
polygons.push_back(7);
polygons.push_back(6);
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(1);
polygons.push_back(5);
polygons.push_back(4);
polygons.push_back(4);
polygons.push_back(1);
polygons.push_back(3);
polygons.push_back(7);
polygons.push_back(5);
polygons.push_back(4);
polygons.push_back(0);
polygons.push_back(2);
polygons.push_back(6);
polygons.push_back(4);
Convex convex_box (
pts, // points
8, // num points
polygons.data(),
6 // number of polygons
);
Transform3f tf1;
Transform3f tf2;
tf2.setTranslation (Vec3f (3, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
tf2.setTranslation (Vec3f (0, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
for (int i = 0; i < 1000; ++i) {
generateRandomTransform(extents, tf2);
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
}
}
BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
{
Sphere s1(20);
......
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