Unverified Commit dcf359c6 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #107 from jmirabel/devel

Fix generateBVHModel + reduced execution time of unit-tests in debug mode.
parents 25d90fd8 984a7a3a
......@@ -52,11 +52,11 @@ namespace fcl
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
{
double a = shape.halfSide[0];
double b = shape.halfSide[1];
double c = shape.halfSide[2];
FCL_REAL a = shape.halfSide[0];
FCL_REAL b = shape.halfSide[1];
FCL_REAL c = shape.halfSide[2];
std::vector<Vec3f> points(8);
Triangle tri_indices[12];
std::vector<Triangle> tri_indices(12);
points[0] = Vec3f ( a, -b, c);
points[1] = Vec3f ( a, b, c);
points[2] = Vec3f (-a, b, c);
......@@ -97,19 +97,19 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double phi, phid;
const double pi = boost::math::constants::pi<double>();
FCL_REAL r = shape.radius;
FCL_REAL phi, phid;
const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
phid = pi * 2 / seg;
phi = 0;
double theta, thetad;
FCL_REAL theta, thetad;
thetad = pi / (ring + 1);
theta = 0;
for(unsigned int i = 0; i < ring; ++i)
{
double theta_ = theta + thetad * (i + 1);
FCL_REAL theta_ = theta + thetad * (i + 1);
for(unsigned int j = 0; j < seg; ++j)
{
points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
......@@ -161,10 +161,10 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
{
double r = shape.radius;
double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
unsigned int ring = ceil(n_low_bound);
unsigned int seg = ceil(n_low_bound);
FCL_REAL r = shape.radius;
FCL_REAL n_low_bound = std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r;
unsigned int ring = (unsigned int) ceil(n_low_bound);
unsigned int seg = (unsigned int) ceil(n_low_bound);
generateBVHModel(model, shape, pose, seg, ring);
}
......@@ -177,31 +177,31 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double h = shape.lz;
double phi, phid;
const double pi = boost::math::constants::pi<double>();
FCL_REAL r = shape.radius;
FCL_REAL h = shape.halfLength;
FCL_REAL phi, phid;
const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
phid = pi * 2 / tot;
phi = 0;
double hd = h / h_num;
FCL_REAL hd = 2 * h / h_num;
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
for(unsigned int i = 0; i < h_num - 1; ++i)
{
for(unsigned int j = 0; j < tot; ++j)
{
points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd));
}
}
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
points.push_back(Vec3f(0, 0, h / 2));
points.push_back(Vec3f(0, 0, -h / 2));
points.push_back(Vec3f(0, 0, h));
points.push_back(Vec3f(0, 0, -h));
for(unsigned int i = 0; i < tot; ++i)
{
......@@ -248,15 +248,15 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
{
double r = shape.radius;
double h = shape.lz;
FCL_REAL r = shape.radius;
FCL_REAL h = 2 * shape.halfLength;
const double pi = boost::math::constants::pi<double>();
unsigned int tot = tot_for_unit_cylinder * r;
double phid = pi * 2 / tot;
const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
FCL_REAL phid = pi * 2 / tot;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
FCL_REAL circle_edge = phid * r;
unsigned int h_num = (unsigned int)ceil(h / circle_edge);
generateBVHModel(model, shape, pose, tot, h_num);
}
......@@ -269,20 +269,20 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double h = shape.lz;
FCL_REAL r = shape.radius;
FCL_REAL h = shape.halfLength;
double phi, phid;
const double pi = boost::math::constants::pi<double>();
FCL_REAL phi, phid;
const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
phid = pi * 2 / tot;
phi = 0;
double hd = h / h_num;
FCL_REAL hd = 2 * h / h_num;
for(unsigned int i = 0; i < h_num - 1; ++i)
{
double h_i = h / 2 - (i + 1) * hd;
double rh = r * (0.5 - h_i / h);
FCL_REAL h_i = h - (i + 1) * hd;
FCL_REAL rh = r * (0.5 - h_i / h / 2);
for(unsigned int j = 0; j < tot; ++j)
{
points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
......@@ -290,10 +290,10 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
}
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h));
points.push_back(Vec3f(0, 0, h / 2));
points.push_back(Vec3f(0, 0, -h / 2));
points.push_back(Vec3f(0, 0, h));
points.push_back(Vec3f(0, 0, -h));
for(unsigned int i = 0; i < tot; ++i)
{
......@@ -340,15 +340,15 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
{
double r = shape.radius;
double h = shape.lz;
FCL_REAL r = shape.radius;
FCL_REAL h = 2 * shape.halfLength;
const double pi = boost::math::constants::pi<double>();
unsigned int tot = tot_for_unit_cone * r;
double phid = pi * 2 / tot;
const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
FCL_REAL phid = pi * 2 / tot;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
FCL_REAL circle_edge = phid * r;
unsigned int h_num = (unsigned int)ceil(h / circle_edge);
generateBVHModel(model, shape, pose, tot, h_num);
}
......
......@@ -63,6 +63,7 @@
#include "fcl_resources/config.h"
using namespace hpp::fcl;
namespace utf = boost::unit_test::framework;
int num_max_contacts = std::numeric_limits<int>::max();
......@@ -587,7 +588,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
{
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
#ifndef NDEBUG // if debug mode
std::size_t n = 1;
#else
std::size_t n = 10;
#endif
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
generateRandomTransforms(extents, transforms, n);
......@@ -609,7 +615,12 @@ BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark)
{
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
#ifndef NDEBUG
std::size_t n = 0;
#else
std::size_t n = 10;
#endif
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
generateRandomTransforms(extents, transforms, n);
......
......@@ -51,6 +51,7 @@
#include "fcl_resources/config.h"
using namespace hpp::fcl;
namespace utf = boost::unit_test::framework;
bool verbose = false;
FCL_REAL DELTA = 0.001;
......@@ -96,7 +97,12 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
std::vector<Transform3f> transforms; // t0
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
#ifndef NDEBUG // if debug mode
std::size_t n = 1;
#else
std::size_t n = 10;
#endif
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
generateRandomTransforms(extents, transforms, n);
......@@ -323,8 +329,8 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
}
std::cout << "distance timing: " << dis_time << " sec" << std::endl;
std::cout << "collision timing: " << col_time << " sec" << std::endl;
BOOST_TEST_MESSAGE("distance timing: " << dis_time << " sec");
BOOST_TEST_MESSAGE("collision timing: " << col_time << " sec");
}
template<typename BV, typename TraversalNode>
......
......@@ -51,6 +51,7 @@
#include <boost/filesystem.hpp>
using namespace hpp::fcl;
namespace utf = boost::unit_test::framework;
template<typename BV>
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
......@@ -84,11 +85,12 @@ BOOST_AUTO_TEST_CASE(front_list)
std::vector<Transform3f> transforms2; // t1
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
FCL_REAL delta_trans[] = {1, 1, 1};
#ifdef NDEBUG
std::size_t n = 20;
#ifndef NDEBUG // if debug mode
std::size_t n = 2;
#else
std::size_t n = 5;
std::size_t n = 20;
#endif
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
bool verbose = false;
generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n);
......
......@@ -47,6 +47,7 @@
#include "utility.h"
#include <iostream>
#include <hpp/fcl/internal/tools.h>
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
using namespace hpp::fcl;
......@@ -189,6 +190,37 @@ void testShapeIntersection(const S1& s1, const Transform3f& tf1,
}
}
BOOST_AUTO_TEST_CASE (box_to_bvh)
{
Box shape (1,1,1);
BVHModel<OBB> bvh;
generateBVHModel (bvh, shape, Transform3f());
}
BOOST_AUTO_TEST_CASE (sphere_to_bvh)
{
Sphere shape (1);
BVHModel<OBB> bvh;
generateBVHModel (bvh, shape, Transform3f(), 10, 10);
generateBVHModel (bvh, shape, Transform3f(), 50);
}
BOOST_AUTO_TEST_CASE (cylinder_to_bvh)
{
Cylinder shape (1,1);
BVHModel<OBB> bvh;
generateBVHModel (bvh, shape, Transform3f(), 10, 10);
generateBVHModel (bvh, shape, Transform3f(), 50);
}
BOOST_AUTO_TEST_CASE (cone_to_bvh)
{
Cone shape (1,1);
BVHModel<OBB> bvh;
generateBVHModel (bvh, shape, Transform3f(), 10, 10);
generateBVHModel (bvh, shape, Transform3f(), 50);
}
BOOST_AUTO_TEST_CASE (shapeIntersection_cylinderbox)
{
Cylinder s1 (0.029, 0.1);
......
......@@ -45,6 +45,7 @@
#include "../src/BV/OBB.h"
#include "../src/distance_func_matrix.h"
#include "utility.h"
using namespace hpp::fcl;
......@@ -1331,9 +1332,15 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output)
Vec3f T;
CollisionRequest request;
#ifndef NDEBUG // if debug mode
static const size_t nbRandomOBB = 10;
static const size_t nbTransformPerOBB = 10;
static const size_t nbRunForTimeMeas = 10;
#else
static const size_t nbRandomOBB = 100;
static const size_t nbTransformPerOBB = 100;
static const size_t nbRunForTimeMeas = 1000;
#endif
static const FCL_REAL extentNorm = 1.;
if (output != NULL) *output << BenchmarkResult::headers << '\n';
......
......@@ -49,6 +49,8 @@
#include "utility.h"
#include "fcl_resources/config.h"
namespace utf = boost::unit_test::framework;
using hpp::fcl::Vec3f;
using hpp::fcl::Triangle;
using hpp::fcl::OBBRSS;
......@@ -136,7 +138,12 @@ BOOST_AUTO_TEST_CASE (OCTREE)
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000};
#ifndef NDEBUG // if debug mode
std::size_t N = 100;
#else
std::size_t N = 10000;
#endif
N = hpp::fcl::getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, N);
generateRandomTransforms(extents, transforms, 2*N);
......
......@@ -120,7 +120,11 @@ void printResults (const Geometry& g1, const Geometry& g2, const Results& rs)
std::cout << g1.type << sep << g2.type << sep << mean << sep << std::sqrt(var) << sep << rs.times.minCoeff() << sep << rs.times.maxCoeff() << std::endl;
}
#ifndef NDEBUG // if debug mode
int Ntransform = 1;
#else
int Ntransform = 100;
#endif
FCL_REAL limit = 20;
bool verbose = false;
......
......@@ -444,6 +444,15 @@ std::ostream& operator<< (std::ostream& os, const Transform3f& tf)
<< " ]" ;
}
std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue)
{
for (int i = 0; i < argc; ++i)
if (strcmp(argv[i], "--nb-run") == 0)
if (i+1 != argc)
return (std::size_t)strtol(argv[i+1], NULL, 10);
return defaultValue;
}
}
} // namespace hpp
......@@ -178,6 +178,9 @@ Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
std::ostream& operator<< (std::ostream& os, const Transform3f& tf);
/// Get the argument --nb-run from argv
std::size_t getNbRun (const int& argc, char const* const* argv, std::size_t defaultValue);
}
} // namespace hpp
......
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