GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: python/collision-geometries.cc Lines: 341 396 86.1 %
Date: 2024-02-09 12:57:42 Branches: 340 716 47.5 %

Line Branch Exec Source
1
//
2
// Software License Agreement (BSD License)
3
//
4
//  Copyright (c) 2019-2022 CNRS-LAAS INRIA
5
//  Author: Joseph Mirabel
6
//  All rights reserved.
7
//
8
//  Redistribution and use in source and binary forms, with or without
9
//  modification, are permitted provided that the following conditions
10
//  are met:
11
//
12
//   * Redistributions of source code must retain the above copyright
13
//     notice, this list of conditions and the following disclaimer.
14
//   * Redistributions in binary form must reproduce the above
15
//     copyright notice, this list of conditions and the following
16
//     disclaimer in the documentation and/or other materials provided
17
//     with the distribution.
18
//   * Neither the name of CNRS-LAAS. nor the names of its
19
//     contributors may be used to endorse or promote products derived
20
//     from this software without specific prior written permission.
21
//
22
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
//  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
//  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
//  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
//  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
//  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
//  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
//  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
//  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
//  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
//  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
//  POSSIBILITY OF SUCH DAMAGE.
34
35
#include <eigenpy/eigenpy.hpp>
36
#include <eigenpy/eigen-to-python.hpp>
37
38
#include "fcl.hh"
39
#include "deprecation.hh"
40
41
#include <hpp/fcl/fwd.hh>
42
#include <hpp/fcl/shape/geometric_shapes.h>
43
#include <hpp/fcl/shape/convex.h>
44
#include <hpp/fcl/BVH/BVH_model.h>
45
#include <hpp/fcl/hfield.h>
46
47
#include <hpp/fcl/serialization/memory.h>
48
#include <hpp/fcl/serialization/AABB.h>
49
#include <hpp/fcl/serialization/BVH_model.h>
50
#include <hpp/fcl/serialization/hfield.h>
51
#include <hpp/fcl/serialization/geometric_shapes.h>
52
#include <hpp/fcl/serialization/convex.h>
53
54
#include "pickle.hh"
55
56
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
57
// FIXME for a reason I do not understand, doxygen fails to understand that
58
// BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h
59
#include <hpp/fcl/internal/BV_splitter.h>
60
#include <hpp/fcl/broadphase/detail/hierarchy_tree.h>
61
62
#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h"
63
#include "doxygen_autodoc/hpp/fcl/BV/AABB.h"
64
#include "doxygen_autodoc/hpp/fcl/hfield.h"
65
#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h"
66
#include "doxygen_autodoc/functions.h"
67
#endif
68
69
using namespace boost::python;
70
using namespace hpp::fcl;
71
namespace dv = doxygen::visitor;
72
namespace bp = boost::python;
73
74
using boost::noncopyable;
75
76
typedef std::vector<Vec3f> Vec3fs;
77
typedef std::vector<Triangle> Triangles;
78
79
struct BVHModelBaseWrapper {
80
  typedef Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> RowMatrixX3;
81
  typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
82
  typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
83
84
  static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) {
85
    if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range");
86
    return bvh.vertices[i];
87
  }
88
89
  static RefRowMatrixX3 vertices(BVHModelBase& bvh) {
90
    return MapRowMatrixX3(bvh.vertices[0].data(), bvh.num_vertices, 3);
91
  }
92
93
  static Triangle tri_indices(const BVHModelBase& bvh, unsigned int i) {
94
    if (i >= bvh.num_tris) throw std::out_of_range("index is out of range");
95
    return bvh.tri_indices[i];
96
  }
97
};
98
99
template <typename BV>
100
20
void exposeBVHModel(const std::string& bvname) {
101
  typedef BVHModel<BV> BVH;
102
103
20
  const std::string type_name = "BVHModel" + bvname;
104
  class_<BVH, bases<BVHModelBase>, shared_ptr<BVH> >(
105
      type_name.c_str(), doxygen::class_doc<BVH>(), no_init)
106
20
      .def(dv::init<BVH>())
107

60
      .def(dv::init<BVH, const BVH&>())
108

20
      .DEF_CLASS_FUNC(BVH, getNumBVs)
109

20
      .DEF_CLASS_FUNC(BVH, makeParentRelative)
110

20
      .DEF_CLASS_FUNC(BVHModelBase, memUsage)
111
20
      .def("clone", &BVH::clone, doxygen::member_func_doc(&BVH::clone),
112
           return_value_policy<manage_new_object>())
113

20
      .def_pickle(PickleObject<BVH>());
114
20
}
115
116
template <typename BV>
117
20
void exposeHeightField(const std::string& bvname) {
118
  typedef HeightField<BV> Geometry;
119
  typedef typename Geometry::Base Base;
120
  typedef typename Geometry::Node Node;
121
122
20
  const std::string type_name = "HeightField" + bvname;
123
  class_<Geometry, bases<Base>, shared_ptr<Geometry> >(
124
      type_name.c_str(), doxygen::class_doc<Geometry>(), no_init)
125
20
      .def(dv::init<HeightField<BV> >())
126

60
      .def(dv::init<HeightField<BV>, const HeightField<BV>&>())
127
20
      .def(dv::init<HeightField<BV>, FCL_REAL, FCL_REAL, const MatrixXf&,
128
20
                    bp::optional<FCL_REAL> >())
129
130

20
      .DEF_CLASS_FUNC(Geometry, getXDim)
131

20
      .DEF_CLASS_FUNC(Geometry, getYDim)
132

20
      .DEF_CLASS_FUNC(Geometry, getMinHeight)
133

20
      .DEF_CLASS_FUNC(Geometry, getMaxHeight)
134

20
      .DEF_CLASS_FUNC(Geometry, getNodeType)
135

20
      .DEF_CLASS_FUNC(Geometry, updateHeights)
136
137
20
      .def("clone", &Geometry::clone,
138
20
           doxygen::member_func_doc(&Geometry::clone),
139
           return_value_policy<manage_new_object>())
140
      .def("getXGrid", &Geometry::getXGrid,
141
20
           doxygen::member_func_doc(&Geometry::getXGrid),
142
           bp::return_value_policy<bp::copy_const_reference>())
143
20
      .def("getYGrid", &Geometry::getYGrid,
144
20
           doxygen::member_func_doc(&Geometry::getYGrid),
145
           bp::return_value_policy<bp::copy_const_reference>())
146
20
      .def("getHeights", &Geometry::getHeights,
147
20
           doxygen::member_func_doc(&Geometry::getHeights),
148
           bp::return_value_policy<bp::copy_const_reference>())
149
20
      .def("getBV", (Node & (Geometry::*)(unsigned int)) & Geometry::getBV,
150
20
           doxygen::member_func_doc((Node & (Geometry::*)(unsigned int)) &
151
                                    Geometry::getBV),
152
           bp::return_internal_reference<>())
153

40
      .def_pickle(PickleObject<Geometry>());
154
20
}
155
156
struct ConvexBaseWrapper {
157
  typedef Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> RowMatrixX3;
158
  typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
159
  typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
160
161
  static Vec3f& point(const ConvexBase& convex, unsigned int i) {
162
    if (i >= convex.num_points)
163
      throw std::out_of_range("index is out of range");
164
    return convex.points[i];
165
  }
166
167
  static RefRowMatrixX3 points(const ConvexBase& convex) {
168
    return MapRowMatrixX3(convex.points[0].data(), convex.num_points, 3);
169
  }
170
171
  static list neighbors(const ConvexBase& convex, unsigned int i) {
172
    if (i >= convex.num_points)
173
      throw std::out_of_range("index is out of range");
174
    list n;
175
    for (unsigned char j = 0; j < convex.neighbors[i].count(); ++j)
176
      n.append(convex.neighbors[i][j]);
177
    return n;
178
  }
179
180
1
  static ConvexBase* convexHull(const Vec3fs& points, bool keepTri,
181
                                const char* qhullCommand) {
182
1
    return ConvexBase::convexHull(points.data(), (unsigned int)points.size(),
183
                                  keepTri, qhullCommand);
184
  }
185
};
186
187
template <typename PolygonT>
188
struct ConvexWrapper {
189
  typedef Convex<PolygonT> Convex_t;
190
191
  static PolygonT polygons(const Convex_t& convex, unsigned int i) {
192
    if (i >= convex.num_polygons)
193
      throw std::out_of_range("index is out of range");
194
    return convex.polygons[i];
195
  }
196
197
3
  static shared_ptr<Convex_t> constructor(const Vec3fs& _points,
198
                                          const Triangles& _tris) {
199

14
    Vec3f* points = new Vec3f[_points.size()];
200
14
    for (std::size_t i = 0; i < _points.size(); ++i) points[i] = _points[i];
201

12
    Triangle* tris = new Triangle[_tris.size()];
202
12
    for (std::size_t i = 0; i < _tris.size(); ++i) tris[i] = _tris[i];
203
3
    return shared_ptr<Convex_t>(new Convex_t(true, points,
204
3
                                             (unsigned int)_points.size(), tris,
205
6
                                             (unsigned int)_tris.size()));
206
  }
207
};
208
209
template <typename T>
210
120
void defComputeMemoryFootprint() {
211
120
  doxygen::def("computeMemoryFootprint", &computeMemoryFootprint<T>);
212
120
}
213
214
5
void exposeComputeMemoryFootprint() {
215
5
  defComputeMemoryFootprint<Sphere>();
216
5
  defComputeMemoryFootprint<Ellipsoid>();
217
5
  defComputeMemoryFootprint<Cone>();
218
5
  defComputeMemoryFootprint<Capsule>();
219
5
  defComputeMemoryFootprint<Cylinder>();
220
5
  defComputeMemoryFootprint<Box>();
221
5
  defComputeMemoryFootprint<Plane>();
222
5
  defComputeMemoryFootprint<Halfspace>();
223
5
  defComputeMemoryFootprint<TriangleP>();
224
225
5
  defComputeMemoryFootprint<BVHModel<OBB> >();
226
5
  defComputeMemoryFootprint<BVHModel<RSS> >();
227
5
  defComputeMemoryFootprint<BVHModel<OBBRSS> >();
228
5
}
229
230
5
void exposeShapes() {
231
5
  class_<ShapeBase, bases<CollisionGeometry>, shared_ptr<ShapeBase>,
232
         noncopyable>("ShapeBase", doxygen::class_doc<ShapeBase>(), no_init)
233
      //.def ("getObjectType", &CollisionGeometry::getObjectType)
234
      ;
235
236
5
  class_<Box, bases<ShapeBase>, shared_ptr<Box> >(
237
      "Box", doxygen::class_doc<ShapeBase>(), no_init)
238
5
      .def(dv::init<Box>())
239
5
      .def(dv::init<Box, const Box&>())
240
5
      .def(dv::init<Box, FCL_REAL, FCL_REAL, FCL_REAL>())
241
5
      .def(dv::init<Box, const Vec3f&>())
242
5
      .DEF_RW_CLASS_ATTRIB(Box, halfSide)
243
5
      .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone),
244
5
           return_value_policy<manage_new_object>())
245
5
      .def_pickle(PickleObject<Box>());
246
247
5
  class_<Capsule, bases<ShapeBase>, shared_ptr<Capsule> >(
248
      "Capsule", doxygen::class_doc<Capsule>(), no_init)
249
5
      .def(dv::init<Capsule>())
250
5
      .def(dv::init<Capsule, FCL_REAL, FCL_REAL>())
251
5
      .def(dv::init<Capsule, const Capsule&>())
252
5
      .DEF_RW_CLASS_ATTRIB(Capsule, radius)
253
5
      .DEF_RW_CLASS_ATTRIB(Capsule, halfLength)
254
5
      .def("clone", &Capsule::clone, doxygen::member_func_doc(&Capsule::clone),
255
5
           return_value_policy<manage_new_object>())
256
5
      .def_pickle(PickleObject<Capsule>());
257
258
5
  class_<Cone, bases<ShapeBase>, shared_ptr<Cone> >(
259
      "Cone", doxygen::class_doc<Cone>(), no_init)
260
5
      .def(dv::init<Cone>())
261
5
      .def(dv::init<Cone, FCL_REAL, FCL_REAL>())
262
5
      .def(dv::init<Cone, const Cone&>())
263
5
      .DEF_RW_CLASS_ATTRIB(Cone, radius)
264
5
      .DEF_RW_CLASS_ATTRIB(Cone, halfLength)
265
5
      .def("clone", &Cone::clone, doxygen::member_func_doc(&Cone::clone),
266
5
           return_value_policy<manage_new_object>())
267
5
      .def_pickle(PickleObject<Cone>());
268
269
5
  class_<ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase>, noncopyable>(
270
      "ConvexBase", doxygen::class_doc<ConvexBase>(), no_init)
271
5
      .DEF_RO_CLASS_ATTRIB(ConvexBase, center)
272
5
      .DEF_RO_CLASS_ATTRIB(ConvexBase, num_points)
273
5
      .def("point", &ConvexBaseWrapper::point, bp::args("self", "index"),
274
           "Retrieve the point given by its index.",
275
10
           bp::return_internal_reference<>())
276
5
      .def("points", &ConvexBaseWrapper::point, bp::args("self", "index"),
277
           "Retrieve the point given by its index.",
278

10
           ::hpp::fcl::python::deprecated_member<
279
5
               bp::return_internal_reference<> >())
280
5
      .def("points", &ConvexBaseWrapper::points, bp::args("self"),
281
           "Retrieve all the points.",
282
10
           bp::with_custodian_and_ward_postcall<0, 1>())
283
      //    .add_property ("points",
284
      //                   bp::make_function(&ConvexBaseWrapper::points,bp::with_custodian_and_ward_postcall<0,1>()),
285
      //                   "Points of the convex.")
286
5
      .def("neighbors", &ConvexBaseWrapper::neighbors)
287
      .def("convexHull", &ConvexBaseWrapper::convexHull,
288
5
           doxygen::member_func_doc(&ConvexBase::convexHull),
289
5
           return_value_policy<manage_new_object>())
290
5
      .staticmethod("convexHull")
291
      .def("clone", &ConvexBase::clone,
292
5
           doxygen::member_func_doc(&ConvexBase::clone),
293
5
           return_value_policy<manage_new_object>());
294
295
5
  class_<Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >,
296
         noncopyable>("Convex", doxygen::class_doc<Convex<Triangle> >(),
297
                      no_init)
298

10
      .def("__init__", make_constructor(&ConvexWrapper<Triangle>::constructor))
299
5
      .def(dv::init<Convex<Triangle> >())
300
5
      .def(dv::init<Convex<Triangle>, const Convex<Triangle>&>())
301

5
      .DEF_RO_CLASS_ATTRIB(Convex<Triangle>, num_polygons)
302
5
      .def("polygons", &ConvexWrapper<Triangle>::polygons)
303
5
      .def_pickle(PickleObject<Convex<Triangle> >());
304
305
5
  class_<Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >(
306
      "Cylinder", doxygen::class_doc<Cylinder>(), no_init)
307
5
      .def(dv::init<Cylinder>())
308
5
      .def(dv::init<Cylinder, FCL_REAL, FCL_REAL>())
309
5
      .def(dv::init<Cylinder, const Cylinder&>())
310
5
      .DEF_RW_CLASS_ATTRIB(Cylinder, radius)
311
5
      .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength)
312
      .def("clone", &Cylinder::clone,
313
5
           doxygen::member_func_doc(&Cylinder::clone),
314
5
           return_value_policy<manage_new_object>())
315
5
      .def_pickle(PickleObject<Cylinder>());
316
317
5
  class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >(
318
      "Halfspace", doxygen::class_doc<Halfspace>(), no_init)
319
5
      .def(dv::init<Halfspace, const Vec3f&, FCL_REAL>())
320
5
      .def(dv::init<Halfspace, const Halfspace&>())
321
5
      .def(dv::init<Halfspace, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
322
5
      .def(dv::init<Halfspace>())
323
5
      .DEF_RW_CLASS_ATTRIB(Halfspace, n)
324
5
      .DEF_RW_CLASS_ATTRIB(Halfspace, d)
325
      .def("clone", &Halfspace::clone,
326
5
           doxygen::member_func_doc(&Halfspace::clone),
327
5
           return_value_policy<manage_new_object>())
328
5
      .def_pickle(PickleObject<Halfspace>());
329
330
5
  class_<Plane, bases<ShapeBase>, shared_ptr<Plane> >(
331
      "Plane", doxygen::class_doc<Plane>(), no_init)
332
5
      .def(dv::init<Plane, const Vec3f&, FCL_REAL>())
333
5
      .def(dv::init<Plane, const Plane&>())
334
5
      .def(dv::init<Plane, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
335
5
      .def(dv::init<Plane>())
336
5
      .DEF_RW_CLASS_ATTRIB(Plane, n)
337
5
      .DEF_RW_CLASS_ATTRIB(Plane, d)
338
5
      .def("clone", &Plane::clone, doxygen::member_func_doc(&Plane::clone),
339
5
           return_value_policy<manage_new_object>())
340
5
      .def_pickle(PickleObject<Plane>());
341
342
5
  class_<Sphere, bases<ShapeBase>, shared_ptr<Sphere> >(
343
      "Sphere", doxygen::class_doc<Sphere>(), no_init)
344
5
      .def(dv::init<Sphere>())
345
5
      .def(dv::init<Sphere, const Sphere&>())
346
5
      .def(dv::init<Sphere, FCL_REAL>())
347
5
      .DEF_RW_CLASS_ATTRIB(Sphere, radius)
348
5
      .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone),
349
5
           return_value_policy<manage_new_object>())
350
5
      .def_pickle(PickleObject<Sphere>());
351
352
5
  class_<Ellipsoid, bases<ShapeBase>, shared_ptr<Ellipsoid> >(
353
      "Ellipsoid", doxygen::class_doc<Ellipsoid>(), no_init)
354
5
      .def(dv::init<Ellipsoid>())
355
5
      .def(dv::init<Ellipsoid, FCL_REAL, FCL_REAL, FCL_REAL>())
356
5
      .def(dv::init<Ellipsoid, Vec3f>())
357
5
      .def(dv::init<Ellipsoid, const Ellipsoid&>())
358
5
      .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii)
359
      .def("clone", &Ellipsoid::clone,
360
5
           doxygen::member_func_doc(&Ellipsoid::clone),
361
5
           return_value_policy<manage_new_object>())
362
5
      .def_pickle(PickleObject<Ellipsoid>());
363
364
5
  class_<TriangleP, bases<ShapeBase>, shared_ptr<TriangleP> >(
365
      "TriangleP", doxygen::class_doc<TriangleP>(), no_init)
366
5
      .def(dv::init<TriangleP>())
367
5
      .def(dv::init<TriangleP, const Vec3f&, const Vec3f&, const Vec3f&>())
368
5
      .def(dv::init<TriangleP, const TriangleP&>())
369
5
      .DEF_RW_CLASS_ATTRIB(TriangleP, a)
370
5
      .DEF_RW_CLASS_ATTRIB(TriangleP, b)
371
5
      .DEF_RW_CLASS_ATTRIB(TriangleP, c)
372
      .def("clone", &TriangleP::clone,
373
5
           doxygen::member_func_doc(&TriangleP::clone),
374
5
           return_value_policy<manage_new_object>())
375
5
      .def_pickle(PickleObject<TriangleP>());
376
5
}
377
378
boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) {
379
  Vec3f P, Q;
380
  FCL_REAL distance = self.distance(other, &P, &Q);
381
  return boost::python::make_tuple(distance, P, Q);
382
}
383
384
5
void exposeCollisionGeometries() {
385
10
  enum_<BVHModelType>("BVHModelType")
386
5
      .value("BVH_MODEL_UNKNOWN", BVH_MODEL_UNKNOWN)
387
5
      .value("BVH_MODEL_TRIANGLES", BVH_MODEL_TRIANGLES)
388
5
      .value("BVH_MODEL_POINTCLOUD", BVH_MODEL_POINTCLOUD)
389
5
      .export_values();
390
391
10
  enum_<BVHBuildState>("BVHBuildState")
392
5
      .value("BVH_BUILD_STATE_EMPTY", BVH_BUILD_STATE_EMPTY)
393
5
      .value("BVH_BUILD_STATE_BEGUN", BVH_BUILD_STATE_BEGUN)
394
5
      .value("BVH_BUILD_STATE_PROCESSED", BVH_BUILD_STATE_PROCESSED)
395
5
      .value("BVH_BUILD_STATE_UPDATE_BEGUN", BVH_BUILD_STATE_UPDATE_BEGUN)
396
5
      .value("BVH_BUILD_STATE_UPDATED", BVH_BUILD_STATE_UPDATED)
397
5
      .value("BVH_BUILD_STATE_REPLACE_BEGUN", BVH_BUILD_STATE_REPLACE_BEGUN)
398
5
      .export_values();
399
400
5
  if (!eigenpy::register_symbolic_link_to_registered_type<OBJECT_TYPE>()) {
401
10
    enum_<OBJECT_TYPE>("OBJECT_TYPE")
402
5
        .value("OT_UNKNOWN", OT_UNKNOWN)
403
5
        .value("OT_BVH", OT_BVH)
404
5
        .value("OT_GEOM", OT_GEOM)
405
5
        .value("OT_OCTREE", OT_OCTREE)
406
5
        .value("OT_HFIELD", OT_HFIELD)
407
5
        .export_values();
408
  }
409
410
5
  if (!eigenpy::register_symbolic_link_to_registered_type<NODE_TYPE>()) {
411
10
    enum_<NODE_TYPE>("NODE_TYPE")
412
5
        .value("BV_UNKNOWN", BV_UNKNOWN)
413
5
        .value("BV_AABB", BV_AABB)
414
5
        .value("BV_OBB", BV_OBB)
415
5
        .value("BV_RSS", BV_RSS)
416
5
        .value("BV_kIOS", BV_kIOS)
417
5
        .value("BV_OBBRSS", BV_OBBRSS)
418
5
        .value("BV_KDOP16", BV_KDOP16)
419
5
        .value("BV_KDOP18", BV_KDOP18)
420
5
        .value("BV_KDOP24", BV_KDOP24)
421
5
        .value("GEOM_BOX", GEOM_BOX)
422
5
        .value("GEOM_SPHERE", GEOM_SPHERE)
423
5
        .value("GEOM_ELLIPSOID", GEOM_ELLIPSOID)
424
5
        .value("GEOM_CAPSULE", GEOM_CAPSULE)
425
5
        .value("GEOM_CONE", GEOM_CONE)
426
5
        .value("GEOM_CYLINDER", GEOM_CYLINDER)
427
5
        .value("GEOM_CONVEX", GEOM_CONVEX)
428
5
        .value("GEOM_PLANE", GEOM_PLANE)
429
5
        .value("GEOM_HALFSPACE", GEOM_HALFSPACE)
430
5
        .value("GEOM_TRIANGLE", GEOM_TRIANGLE)
431
5
        .value("GEOM_OCTREE", GEOM_OCTREE)
432
5
        .value("HF_AABB", HF_AABB)
433
5
        .value("HF_OBBRSS", HF_OBBRSS)
434
5
        .export_values();
435
  }
436
437
5
  class_<AABB>("AABB",
438
               "A class describing the AABB collision structure, which is a "
439
               "box in 3D space determined by two diagonal points",
440
               no_init)
441

10
      .def(init<>(bp::arg("self"), "Default constructor"))
442

10
      .def(init<AABB>(bp::args("self", "other"), "Copy constructor"))
443

10
      .def(init<Vec3f>(bp::args("self", "v"),
444
5
                       "Creating an AABB at position v with zero size."))
445

10
      .def(init<Vec3f, Vec3f>(bp::args("self", "a", "b"),
446
5
                              "Creating an AABB with two endpoints a and b."))
447
5
      .def(init<AABB, Vec3f>(
448
10
          bp::args("self", "core", "delta"),
449
5
          "Creating an AABB centered as core and is of half-dimension delta."))
450

10
      .def(init<Vec3f, Vec3f, Vec3f>(bp::args("self", "a", "b", "c"),
451
5
                                     "Creating an AABB contains three points."))
452
453
      .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain,
454
10
           bp::args("self", "p"), "Check whether the AABB contains a point p.")
455
      .def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain,
456
10
           bp::args("self", "other"),
457
5
           "Check whether the AABB contains another AABB.")
458
459
      .def("overlap", (bool(AABB::*)(const AABB&) const) & AABB::overlap,
460

10
           bp::args("self", "other"), "Check whether two AABB are overlap.")
461
      .def("overlap", (bool(AABB::*)(const AABB&, AABB&) const) & AABB::overlap,
462
10
           bp::args("self", "other", "overlapping_part"),
463
           "Check whether two AABB are overlaping and return the overloaping "
464
5
           "part if true.")
465
466
      .def("distance", (FCL_REAL(AABB::*)(const AABB&) const) & AABB::distance,
467

10
           bp::args("self", "other"), "Distance between two AABBs.")
468
      //    .def("distance",
469
      //         AABB_distance_proxy,
470
      //         bp::args("self","other"),
471
      //         "Distance between two AABBs.")
472
473
      .add_property(
474
          "min_",
475
5
          bp::make_function(
476
              +[](AABB& self) -> Vec3f& { return self.min_; },
477
              bp::return_internal_reference<>()),
478
10
          bp::make_function(
479
              +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }),
480

10
          "The min point in the AABB.")
481
      .add_property(
482
          "max_",
483
5
          bp::make_function(
484
              +[](AABB& self) -> Vec3f& { return self.max_; },
485
              bp::return_internal_reference<>()),
486
10
          bp::make_function(
487
              +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }),
488
5
          "The max point in the AABB.")
489
490
5
      .def(bp::self == bp::self)
491
5
      .def(bp::self != bp::self)
492
493
5
      .def(bp::self + bp::self)
494
5
      .def(bp::self += bp::self)
495

5
      .def(bp::self += Vec3f())
496
497

10
      .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.")
498

10
      .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.")
499

10
      .def("width", &AABB::width, bp::arg("self"), "Width of the AABB.")
500

10
      .def("height", &AABB::height, bp::arg("self"), "Height of the AABB.")
501

10
      .def("depth", &AABB::depth, bp::arg("self"), "Depth of the AABB.")
502

10
      .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.")
503
504
      .def("expand",
505
           static_cast<AABB& (AABB::*)(const AABB&, FCL_REAL)>(&AABB::expand),
506
           //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
507
           //         AABB &, FCL_REAL)>(&AABB::expand)),
508
           //         doxygen::member_func_args(static_cast<AABB&
509
           //         (AABB::*)(const AABB &, FCL_REAL)>(&AABB::expand)),
510
           bp::return_internal_reference<>())
511
      .def("expand",
512
           static_cast<AABB& (AABB::*)(const FCL_REAL)>(&AABB::expand),
513
           //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
514
           //         FCL_REAL)>(&AABB::expand)),
515
           //         doxygen::member_func_args(static_cast<AABB&
516
           //         (AABB::*)(const FCL_REAL)>(&AABB::expand)),
517
5
           bp::return_internal_reference<>())
518
      .def("expand", static_cast<AABB& (AABB::*)(const Vec3f&)>(&AABB::expand),
519
           //         doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
520
           //         Vec3f &)>(&AABB::expand)),
521
           //         doxygen::member_func_args(static_cast<AABB&
522
           //         (AABB::*)(const Vec3f &)>(&AABB::expand)),
523
5
           bp::return_internal_reference<>())
524

5
      .def_pickle(PickleObject<AABB>());
525
526
5
  def("translate", (AABB(*)(const AABB&, const Vec3f&)) & translate,
527
10
      bp::args("aabb", "t"), "Translate the center of AABB by t");
528
529
5
  def("rotate", (AABB(*)(const AABB&, const Matrix3f&)) & rotate,
530
10
      bp::args("aabb", "R"), "Rotate the AABB object by R");
531
532
5
  if (!eigenpy::register_symbolic_link_to_registered_type<
533
5
          CollisionGeometry>()) {
534
10
    class_<CollisionGeometry, CollisionGeometryPtr_t, noncopyable>(
535
        "CollisionGeometry", no_init)
536
5
        .def("getObjectType", &CollisionGeometry::getObjectType)
537
5
        .def("getNodeType", &CollisionGeometry::getNodeType)
538
539
5
        .def("computeLocalAABB", &CollisionGeometry::computeLocalAABB)
540
541
5
        .def("computeCOM", &CollisionGeometry::computeCOM)
542
        .def("computeMomentofInertia",
543
5
             &CollisionGeometry::computeMomentofInertia)
544
5
        .def("computeVolume", &CollisionGeometry::computeVolume)
545
        .def("computeMomentofInertiaRelatedToCOM",
546
5
             &CollisionGeometry::computeMomentofInertiaRelatedToCOM)
547
548
        .def_readwrite("aabb_radius", &CollisionGeometry::aabb_radius,
549
5
                       "AABB radius.")
550
        .def_readwrite("aabb_center", &CollisionGeometry::aabb_center,
551
5
                       "AABB center in local coordinate.")
552
        .def_readwrite("aabb_local", &CollisionGeometry::aabb_local,
553
                       "AABB in local coordinate, used for tight AABB when "
554
5
                       "only translation transform.")
555
556
10
        .def("isOccupied", &CollisionGeometry::isOccupied, bp::arg("self"),
557
5
             "Whether the object is completely occupied.")
558
10
        .def("isFree", &CollisionGeometry::isFree, bp::arg("self"),
559
5
             "Whether the object is completely free.")
560
10
        .def("isUncertain", &CollisionGeometry::isUncertain, bp::arg("self"),
561
5
             "Whether the object has some uncertainty.")
562
563
        .def_readwrite("cost_density", &CollisionGeometry::cost_density,
564
5
                       "Collision cost for unit volume.")
565
        .def_readwrite("threshold_occupied",
566
                       &CollisionGeometry::threshold_occupied,
567
5
                       "Threshold for occupied ( >= is occupied).")
568
        .def_readwrite("threshold_free", &CollisionGeometry::threshold_free,
569
5
                       "Threshold for free (<= is free).")
570
571
5
        .def(self == self)
572
5
        .def(self != self);
573
  }
574
575
5
  exposeShapes();
576
577
5
  class_<BVHModelBase, bases<CollisionGeometry>, BVHModelPtr_t, noncopyable>(
578
      "BVHModelBase", no_init)
579
5
      .def("vertex", &BVHModelBaseWrapper::vertex, bp::args("self", "index"),
580
           "Retrieve the vertex given by its index.",
581
10
           bp::return_internal_reference<>())
582
5
      .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"),
583
           "Retrieve the vertex given by its index.",
584

10
           ::hpp::fcl::python::deprecated_member<
585
5
               bp::return_internal_reference<> >())
586
5
      .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"),
587
           "Retrieve all the vertices.",
588
10
           bp::with_custodian_and_ward_postcall<0, 1>())
589
      //    .add_property ("vertices",
590
      //                   bp::make_function(&BVHModelBaseWrapper::vertices,bp::with_custodian_and_ward_postcall<0,1>()),
591
      //                   "Vertices of the BVH.")
592
      .def("tri_indices", &BVHModelBaseWrapper::tri_indices,
593
10
           bp::args("self", "index"),
594
5
           "Retrieve the triangle given by its index.")
595
5
      .def_readonly("num_vertices", &BVHModelBase::num_vertices)
596
5
      .def_readonly("num_tris", &BVHModelBase::num_tris)
597
5
      .def_readonly("build_state", &BVHModelBase::build_state)
598
599
5
      .def_readonly("convex", &BVHModelBase::convex)
600
601

5
      .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation)
602

5
      .DEF_CLASS_FUNC(BVHModelBase, buildConvexHull)
603
604
      // Expose function to build a BVH
605

5
      .def(dv::member_func("beginModel", &BVHModelBase::beginModel))
606

5
      .def(dv::member_func("addVertex", &BVHModelBase::addVertex))
607

5
      .def(dv::member_func("addVertices", &BVHModelBase::addVertices))
608

5
      .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle))
609

5
      .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles))
610
      .def(dv::member_func<int (BVHModelBase::*)(
611
               const Vec3fs&, const Triangles&)>("addSubModel",
612

5
                                                 &BVHModelBase::addSubModel))
613
      .def(dv::member_func<int (BVHModelBase::*)(const Vec3fs&)>(
614

5
          "addSubModel", &BVHModelBase::addSubModel))
615

5
      .def(dv::member_func("endModel", &BVHModelBase::endModel))
616
      // Expose function to replace a BVH
617
      .def(dv::member_func("beginReplaceModel",
618

5
                           &BVHModelBase::beginReplaceModel))
619

5
      .def(dv::member_func("replaceVertex", &BVHModelBase::replaceVertex))
620

5
      .def(dv::member_func("replaceTriangle", &BVHModelBase::replaceTriangle))
621

5
      .def(dv::member_func("replaceSubModel", &BVHModelBase::replaceSubModel))
622

5
      .def(dv::member_func("endReplaceModel", &BVHModelBase::endReplaceModel))
623

5
      .def(dv::member_func("getModelType", &BVHModelBase::getModelType));
624

5
  exposeBVHModel<OBB>("OBB");
625

5
  exposeBVHModel<OBBRSS>("OBBRSS");
626

5
  exposeHeightField<OBBRSS>("OBBRSS");
627

5
  exposeHeightField<AABB>("AABB");
628
5
  exposeComputeMemoryFootprint();
629
5
}
630
631
5
void exposeCollisionObject() {
632
  namespace bp = boost::python;
633
634
5
  if (!eigenpy::register_symbolic_link_to_registered_type<CollisionObject>()) {
635
10
    class_<CollisionObject, CollisionObjectPtr_t>("CollisionObject", no_init)
636
        .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
637
5
                      bp::optional<bool> >())
638
        .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
639
5
                      const Transform3f&, bp::optional<bool> >())
640
        .def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
641
5
                      const Matrix3f&, const Vec3f&, bp::optional<bool> >())
642
643

5
        .DEF_CLASS_FUNC(CollisionObject, getObjectType)
644

5
        .DEF_CLASS_FUNC(CollisionObject, getNodeType)
645

5
        .DEF_CLASS_FUNC(CollisionObject, computeAABB)
646
        .def(dv::member_func("getAABB",
647
                             static_cast<AABB& (CollisionObject::*)()>(
648
                                 &CollisionObject::getAABB),
649

5
                             bp::return_internal_reference<>()))
650
651
5
        .DEF_CLASS_FUNC2(CollisionObject, getTranslation,
652
                         bp::return_value_policy<bp::copy_const_reference>())
653

5
        .DEF_CLASS_FUNC(CollisionObject, setTranslation)
654
5
        .DEF_CLASS_FUNC2(CollisionObject, getRotation,
655
                         bp::return_value_policy<bp::copy_const_reference>())
656

5
        .DEF_CLASS_FUNC(CollisionObject, setRotation)
657
5
        .DEF_CLASS_FUNC2(CollisionObject, getTransform,
658
                         bp::return_value_policy<bp::copy_const_reference>())
659
        .def(dv::member_func(
660
            "setTransform",
661
            static_cast<void (CollisionObject::*)(const Transform3f&)>(
662

5
                &CollisionObject::setTransform)))
663
664

5
        .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform)
665

5
        .DEF_CLASS_FUNC(CollisionObject, setIdentityTransform)
666
5
        .DEF_CLASS_FUNC2(CollisionObject, setCollisionGeometry,
667
                         (bp::with_custodian_and_ward_postcall<1, 2>()))
668
669
        .def(dv::member_func(
670
            "collisionGeometry",
671
            static_cast<const CollisionGeometryPtr_t& (CollisionObject::*)()>(
672
                &CollisionObject::collisionGeometry),
673

5
            bp::return_value_policy<bp::copy_const_reference>()));
674
  }
675
5
}