17 #include <boost/filesystem.hpp> 28 #include "fcl_resources/config.h" 45 template <
typename BV >
47 std::vector<Vec3f> pt;
48 std::vector<Triangle> tri;
55 model->addSubModel(pt, tri);
61 template <
typename BV >
79 std::vector<CollisionResult>
rs;
90 for (std::size_t i = 0; i < tf.size(); ++i) {
93 collide(o1, tf[i], o2, Id, request, results.
rs[i]);
99 const char*
sep =
", ";
102 std::cout <<
"Type 1" << sep <<
"Type 2" << sep <<
"mean time" << sep
103 <<
"time std dev" << sep <<
"min time" << sep <<
"max time" 108 double mean = rs.
times.mean();
109 double var = rs.
times.cwiseAbs2().mean() - mean * mean;
110 std::cout << g1.
type << sep << g2.
type << sep << mean << sep << std::sqrt(var)
111 << sep << rs.
times.minCoeff() << sep << rs.
times.maxCoeff()
115 #ifndef NDEBUG // if debug mode 118 size_t Ntransform = 100;
124 if (verbose) std::cout << x << std::endl 125 #define CHECK_PARAM_NB(NB, NAME) \ 126 if (iarg + NB >= argc) \ 127 throw std::invalid_argument(#NAME " requires " #NB " numbers") 130 while (iarg < argc) {
131 std::string
a(argv[iarg]);
132 if (a ==
"-nb_transform") {
134 Ntransform = (size_t)atoi(argv[iarg + 1]);
135 OUT(
"nb_transform = " << Ntransform);
137 }
else if (a ==
"-enable_distance_lower_bound") {
141 }
else if (a ==
"-limit") {
143 limit = atof(argv[iarg + 1]);
145 }
else if (a ==
"-verbose") {
153 #define CREATE_SHAPE_2(var, Name) \ 154 CHECK_PARAM_NB(2, Name); \ 155 var.reset(new Name(atof(argv[iarg + 1]), atof(argv[iarg + 2]))); \ 158 if (iarg >= argc)
throw std::invalid_argument(
"An argument is required.");
159 std::string
a(argv[iarg]);
164 o.reset(
new Box(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
165 atof(argv[iarg + 3])));
168 }
else if (a ==
"-sphere") {
170 o.reset(
new Sphere(atof(argv[iarg + 1])));
173 }
else if (a ==
"-mesh") {
175 OUT(
"Loading " << argv[iarg + 2] <<
" as BVHModel<" << argv[iarg + 1]
177 if (strcmp(argv[iarg + 1],
"obb") == 0) {
178 o = meshToGeom<OBB>(argv[iarg + 2]);
182 }
else if (strcmp(argv[iarg + 1],
"obbrss") == 0) {
183 o = meshToGeom<OBBRSS>(argv[iarg + 2]);
186 type =
"mesh_obbrss";
188 throw std::invalid_argument(
"BV type must be obb or obbrss");
191 if (iarg < argc && strcmp(argv[iarg],
"crop") == 0) {
194 atof(argv[iarg + 3])),
195 Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
196 atof(argv[iarg + 6])));
197 OUT(
"Cropping " << aabb.
min_.transpose() <<
" ---- " 198 << aabb.
max_.transpose() <<
" ...");
199 o->computeLocalAABB();
200 OUT(
"Mesh AABB is " << o->aabb_local.min_.transpose() <<
" ---- " 201 << o->aabb_local.max_.transpose() <<
" ...");
203 if (!o)
throw std::invalid_argument(
"Failed to crop.");
208 }
else if (a ==
"-capsule") {
211 }
else if (a ==
"-cone") {
214 }
else if (a ==
"-cylinder") {
218 throw std::invalid_argument(std::string(
"Unknown argument: ") + a);
223 int main(
int argc,
char** argv) {
224 std::vector<Transform3f> transforms;
238 collide(transforms, first.
o.get(), second.
o.get(), request, results);
245 std::vector<Geometry> geoms;
246 geoms.push_back(
Geometry(
"Box",
new Box(1, 2, 3)));
258 geoms.push_back(
Geometry(
"rob BVHModel<OBB>",
259 objToGeom<OBB>((path /
"rob.obj").
string())));
263 geoms.push_back(
Geometry(
"rob BVHModel<OBBRSS>",
264 objToGeom<OBBRSS>((path /
"rob.obj").
string())));
266 geoms.push_back(
Geometry(
"env BVHModel<OBB>",
267 objToGeom<OBB>((path /
"env.obj").
string())));
271 geoms.push_back(
Geometry(
"env BVHModel<OBBRSS>",
272 objToGeom<OBBRSS>((path /
"env.obj").
string())));
277 for (std::size_t i = 0; i < geoms.size(); ++i) {
278 for (std::size_t j = i; j < geoms.size(); ++j) {
279 if (!
supportedPair(geoms[i].o.get(), geoms[j].o.get()))
continue;
281 collide(transforms, geoms[i].o.get(), geoms[j].o.get(), request,
HPP_FCL_DLLAPI CollisionGeometry * extract(const CollisionGeometry *model, const Transform3f &pose, const AABB &aabb)
void printResultHeaders()
Vec3f min_
The min point in the AABB.
CollisionGeometryPtr_t meshToGeom(const std::string &filename, const Vec3f &scale=Vec3f(1, 1, 1))
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Cylinder along Z axis. The cylinder is defined at its centroid.
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]
each item in the collision matrix is a function to handle collision between objects of type1 and type...
void stop()
stop the timer
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
void handleParam(int &iarg, const int &argc, char **argv, CollisionRequest &request)
std::vector< CollisionResult > rs
virtual NODE_TYPE getNodeType() const
get the node type
Geometry(const std::string &t, const CollisionGeometryPtr_t &ob)
request to the collision algorithm
Center at zero point, axis aligned box.
virtual OBJECT_TYPE getObjectType() const
get the type of the object
CollisionGeometryPtr_t objToGeom(const std::string &filename)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Cone The base of the cone is at and the top is at .
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Vec3f max_
The max point in the AABB.
#define CHECK_PARAM_NB(NB, NAME)
void printResults(const Geometry &g1, const Geometry &g2, const Results &rs)
Center at zero point sphere.
CollisionFunctionMatrix lookupTable
Capsule It is where is the distance between the point x and the capsule segment AB...
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
#define CREATE_SHAPE_2(var, Name)
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
int main(int argc, char **argv)
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Geometry makeGeomFromParam(int &iarg, const int &argc, char **argv)
Geometry(const std::string &t, CollisionGeometry *ob)
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
bool supportedPair(const CollisionGeometry *o1, const CollisionGeometry *o2)
The geometry for the object for collision or distance computation.
void loadPolyhedronFromResource(const std::string &resource_path, const fcl::Vec3f &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
collision matrix stores the functions for collision between different types of objects and provides a...