Commit 7823f4b9 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[test] Update unit-tests to changes in class Convex.

parent 07ec65b4
......@@ -40,14 +40,46 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/convex.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
#include "utility.h"
using namespace hpp::fcl;
BOOST_AUTO_TEST_CASE(convex)
struct Quadrilateral
{
FCL_REAL l = 1, w = 1, d = 1;
public:
typedef std::size_t index_type;
typedef int size_type;
Quadrilateral() {}
Quadrilateral(index_type p0, index_type p1, index_type p2, index_type p3)
{
set(p0, p1, p2, p3);
}
/// @brief Set the vertex indices of the triangle
inline void set(index_type p0, index_type p1, index_type p2, index_type p3)
{
vids[0] = p0; vids[1] = p1; vids[2] = p2; vids[3] = p3;
}
/// @access the triangle index
inline index_type operator[](int i) const { return vids[i]; }
inline index_type& operator[](int i) { return vids[i]; }
static inline size_type size() { return 4; }
private:
index_type vids[4];
};
Convex<Quadrilateral> buildBox (FCL_REAL l, FCL_REAL w, FCL_REAL d)
{
Vec3f pts[8];
pts[0] = Vec3f( l, w, d);
pts[1] = Vec3f( l, w,-d);
......@@ -57,49 +89,26 @@ BOOST_AUTO_TEST_CASE(convex)
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 box (
std::vector<Quadrilateral> polygons (6);
polygons[0].set(0, 2, 3, 1); // x+ side
polygons[1].set(2, 6, 7, 3); // y- side
polygons[2].set(4, 5, 7, 6); // x- side
polygons[3].set(0, 1, 5, 4); // y+ side
polygons[4].set(1, 3, 7, 5); // z- side
polygons[5].set(0, 2, 6, 4); // z+ side
return Convex<Quadrilateral> (
pts, // points
8, // num points
polygons.data(),
6 // number of polygons
);
}
BOOST_AUTO_TEST_CASE(convex)
{
FCL_REAL l = 1, w = 1, d = 1;
Convex<Quadrilateral> box (buildBox (l, w, d));
// Check neighbors
for (int i = 0; i < 8; ++i) {
......@@ -137,3 +146,94 @@ BOOST_AUTO_TEST_CASE(convex)
BOOST_CHECK_EQUAL(box.neighbors[7][1], 5);
BOOST_CHECK_EQUAL(box.neighbors[7][2], 6);
}
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_TEST_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'
);
// TODO in one case, there is a mismatch between the distances and I cannot say
// which one is correct. To visualize the case, use script test/geometric_shapes.py
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(compare_convex_box)
{
FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10};
FCL_REAL l = 1, w = 1, d = 1;
Box box(l*2, w*2, d*2);
Convex<Quadrilateral> convex_box (buildBox (l, w, d));
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);
}
}
......@@ -189,72 +189,6 @@ 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_TEST_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'
);
// TODO in one case, there is a mismatch between the distances and I cannot say
// which one is correct. To visualize the case, use script test/geometric_shapes.py
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);
......@@ -504,82 +438,6 @@ 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