collision-geometries.cc 23.1 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
//
// Software License Agreement (BSD License)
//
4
//  Copyright (c) 2019-2021 CNRS-LAAS INRIA
Joseph Mirabel's avatar
Joseph Mirabel committed
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
//  Author: Joseph Mirabel
//  All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions
//  are met:
//
//   * Redistributions of source code must retain the above copyright
//     notice, this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above
//     copyright notice, this list of conditions and the following
//     disclaimer in the documentation and/or other materials provided
//     with the distribution.
//   * Neither the name of CNRS-LAAS. nor the names of its
//     contributors may be used to endorse or promote products derived
//     from this software without specific prior written permission.
//
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
//  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
//  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
//  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
//  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
//  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
//  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
//  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
//  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//  POSSIBILITY OF SUCH DAMAGE.

35
#include <eigenpy/eigenpy.hpp>
Justin Carpentier's avatar
Justin Carpentier committed
36
#include <eigenpy/eigen-to-python.hpp>
37

Joseph Mirabel's avatar
Joseph Mirabel committed
38
#include "fcl.hh"
39
#include "deprecation.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
43
44

#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/convex.h>
#include <hpp/fcl/BVH/BVH_model.h>
45
#include <hpp/fcl/serialization/memory.h>
46
#include <hpp/fcl/serialization/BVH_model.h>
Joseph Mirabel's avatar
Joseph Mirabel committed
47

48
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
49
50
51
// FIXME for a reason I do not understand, doxygen fails to understand that
// BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h
#include <hpp/fcl/internal/BV_splitter.h>
Joseph Mirabel's avatar
Joseph Mirabel committed
52
53
#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h"
#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h"
54
#include "doxygen_autodoc/functions.h"
55
56
57
#endif

#include "../doc/python/doxygen.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
58
#include "../doc/python/doxygen-boost.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
59

60
61
62
63
64
65
66
#define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB)                                     \
  def_readwrite (#ATTRIB, &CLASS::ATTRIB,                                      \
      doxygen::class_attrib_doc<CLASS>(#ATTRIB))
#define DEF_RO_CLASS_ATTRIB(CLASS, ATTRIB)                                     \
  def_readonly (#ATTRIB, &CLASS::ATTRIB,                                       \
      doxygen::class_attrib_doc<CLASS>(#ATTRIB))
#define DEF_CLASS_FUNC(CLASS, ATTRIB)                                          \
Joseph Mirabel's avatar
Joseph Mirabel committed
67
  def (dv::member_func(#ATTRIB, &CLASS::ATTRIB))
68
#define DEF_CLASS_FUNC2(CLASS, ATTRIB,policy)                                  \
69
  def (dv::member_func(#ATTRIB, &CLASS::ATTRIB, policy))
70

Joseph Mirabel's avatar
Joseph Mirabel committed
71
72
using namespace boost::python;
using namespace hpp::fcl;
Joseph Mirabel's avatar
Joseph Mirabel committed
73
namespace dv = doxygen::visitor;
74
namespace bp = boost::python;
Joseph Mirabel's avatar
Joseph Mirabel committed
75

Joseph Mirabel's avatar
Joseph Mirabel committed
76
77
using boost::noncopyable;

78
79
80
typedef std::vector<Vec3f> Vec3fs;
typedef std::vector<Triangle> Triangles;

Joseph Mirabel's avatar
Joseph Mirabel committed
81
struct BVHModelBaseWrapper
Joseph Mirabel's avatar
Joseph Mirabel committed
82
{
Justin Carpentier's avatar
Justin Carpentier committed
83
84
85
  typedef Eigen::Matrix<double,Eigen::Dynamic,3,Eigen::RowMajor> RowMatrixX3;
  typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
  typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
86
  
Justin Carpentier's avatar
Justin Carpentier committed
87
  static Vec3f & vertex (BVHModelBase & bvh, int i)
Joseph Mirabel's avatar
Joseph Mirabel committed
88
89
90
91
92
  {
    if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range");
    return bvh.vertices[i];
  }

Justin Carpentier's avatar
Justin Carpentier committed
93
  static RefRowMatrixX3 vertices(BVHModelBase & bvh)
94
  {
Justin Carpentier's avatar
Justin Carpentier committed
95
    return MapRowMatrixX3(bvh.vertices[0].data(),bvh.num_vertices,3);
96
97
  }
  
Joseph Mirabel's avatar
Joseph Mirabel committed
98
  static Triangle tri_indices (const BVHModelBase& bvh, int i)
Joseph Mirabel's avatar
Joseph Mirabel committed
99
100
101
102
103
104
105
106
107
  {
    if (i >= bvh.num_tris) throw std::out_of_range("index is out of range");
    return bvh.tri_indices[i];
  }
};

template <typename BV>
void exposeBVHModel (const std::string& bvname)
{
108
109
110
111
112
113
114
115
116
  typedef BVHModel<BV> BVH;

  const std::string type_name = "BVHModel" + bvname;
  class_ <BVH, bases<BVHModelBase>, shared_ptr<BVH> >
    (type_name.c_str(), doxygen::class_doc<BVH>(), no_init)
    .def (dv::init<BVH>())
    .def (dv::init<BVH, BVH>())
    .DEF_CLASS_FUNC(BVH,getNumBVs)
    .DEF_CLASS_FUNC(BVH,makeParentRelative)
Justin Carpentier's avatar
Justin Carpentier committed
117
    .DEF_CLASS_FUNC(BVHModelBase,memUsage)
118
119
120
    .def ("clone", &BVH::clone,
          doxygen::member_func_doc(&BVH::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
121
122
123
124
125
    ;
}

struct ConvexBaseWrapper
{
126
127
128
129
130
  typedef Eigen::Matrix<double,Eigen::Dynamic,3,Eigen::RowMajor> RowMatrixX3;
  typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
  typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
  
  static Vec3f & point (const ConvexBase& convex, int i)
Joseph Mirabel's avatar
Joseph Mirabel committed
131
132
133
134
  {
    if (i >= convex.num_points) throw std::out_of_range("index is out of range");
    return convex.points[i];
  }
135
136
137
138
139
  
  static RefRowMatrixX3 points (const ConvexBase& convex)
  {
    return MapRowMatrixX3(convex.points[0].data(),convex.num_points,3);
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
140
141
142
143
144
145
146
147
148

  static list neighbors (const ConvexBase& convex, int i)
  {
    if (i >= convex.num_points) throw std::out_of_range("index is out of range");
    list n;
    for (unsigned char j = 0; j < convex.neighbors[i].count(); ++j)
      n.append (convex.neighbors[i][j]);
    return n;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
149
150
151
152
153
154
155

  static ConvexBase* convexHull (const Vec3fs& points, bool keepTri,
      const char* qhullCommand)
  {
    return ConvexBase::convexHull (points.data(), (int)points.size(), keepTri,
        qhullCommand);
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
156
157
158
159
160
161
162
163
164
165
166
167
};

template <typename PolygonT>
struct ConvexWrapper
{
  typedef Convex<PolygonT> Convex_t;

  static PolygonT polygons (const Convex_t& convex, int i)
  {
    if (i >= convex.num_polygons) throw std::out_of_range("index is out of range");
    return convex.polygons[i];
  }
168
169
170
171
172
173
174
175
176
177

  static shared_ptr<Convex_t> constructor (const Vec3fs& _points, const Triangles& _tris)
  {
    Vec3f* points = new Vec3f[_points.size()];
    for (std::size_t i = 0; i < _points.size(); ++i) points[i] = _points[i];
    Triangle* tris = new Triangle[_tris.size()];
    for (std::size_t i = 0; i < _tris.size(); ++i) tris[i] = _tris[i];
    return shared_ptr<Convex_t>(new Convex_t(true, points, (int)_points.size(),
          tris, (int)_tris.size()));
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
178
179
};

180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202

template<typename T>
void defComputeMemoryFootprint()
{
  doxygen::def("computeMemoryFootprint",&computeMemoryFootprint<T>);
}

void exposeComputeMemoryFootprint()
{
  defComputeMemoryFootprint<Sphere>();
  defComputeMemoryFootprint<Cone>();
  defComputeMemoryFootprint<Capsule>();
  defComputeMemoryFootprint<Cylinder>();
  defComputeMemoryFootprint<Box>();
  defComputeMemoryFootprint<Plane>();
  defComputeMemoryFootprint<Halfspace>();
  defComputeMemoryFootprint<TriangleP>();
  
  defComputeMemoryFootprint< BVHModel<OBB> >();
  defComputeMemoryFootprint< BVHModel<RSS> >();
  defComputeMemoryFootprint< BVHModel<OBBRSS> >();
}

Joseph Mirabel's avatar
Joseph Mirabel committed
203
204
205
void exposeShapes ()
{
  class_ <ShapeBase, bases<CollisionGeometry>, shared_ptr<ShapeBase>, noncopyable>
Joseph Mirabel's avatar
Joseph Mirabel committed
206
    ("ShapeBase", doxygen::class_doc<ShapeBase>(), no_init)
Joseph Mirabel's avatar
Joseph Mirabel committed
207
208
209
210
    //.def ("getObjectType", &CollisionGeometry::getObjectType)
    ;

  class_ <Box, bases<ShapeBase>, shared_ptr<Box> >
Joseph Mirabel's avatar
Joseph Mirabel committed
211
    ("Box", doxygen::class_doc<ShapeBase>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
212
213
214
    .def (dv::init<Box>())
    .def (dv::init<Box, FCL_REAL,FCL_REAL,FCL_REAL>())
    .def (dv::init<Box, const Vec3f&>())
215
    .DEF_RW_CLASS_ATTRIB(Box,halfSide)
216
217
218
    .def ("clone", &Box::clone,
        doxygen::member_func_doc(&Box::clone),
        return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
219
220
221
    ;

  class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
Joseph Mirabel's avatar
Joseph Mirabel committed
222
    ("Capsule", doxygen::class_doc<Capsule>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
223
    .def (dv::init<Capsule, FCL_REAL, FCL_REAL>())
224
225
    .DEF_RW_CLASS_ATTRIB (Capsule, radius)
    .DEF_RW_CLASS_ATTRIB (Capsule, halfLength)
226
227
228
    .def ("clone", &Capsule::clone,
      doxygen::member_func_doc(&Capsule::clone),
      return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
229
230
231
    ;

  class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> >
Joseph Mirabel's avatar
Joseph Mirabel committed
232
    ("Cone", doxygen::class_doc<Cone>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
233
    .def (dv::init<Cone, FCL_REAL, FCL_REAL>())
234
235
    .DEF_RW_CLASS_ATTRIB (Cone, radius)
    .DEF_RW_CLASS_ATTRIB (Cone, halfLength)
236
237
238
    .def ("clone", &Cone::clone,
      doxygen::member_func_doc(&Cone::clone),
      return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
239
240
241
    ;

  class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable>
Joseph Mirabel's avatar
Joseph Mirabel committed
242
    ("ConvexBase", doxygen::class_doc<ConvexBase>(), no_init)
243
244
    .DEF_RO_CLASS_ATTRIB (ConvexBase, center)
    .DEF_RO_CLASS_ATTRIB (ConvexBase, num_points)
245
246
247
248
249
250
251
252
253
254
255
256
    .def ("point", &ConvexBaseWrapper::point,
          bp::args("self","index"),"Retrieve the point given by its index.",
          bp::return_internal_reference<>())
    .def ("points", &ConvexBaseWrapper::point,
          bp::args("self","index"),"Retrieve the point given by its index.",
          ::hpp::fcl::python::deprecated_member< bp::return_internal_reference<> >())
    .def ("points", &ConvexBaseWrapper::points,
          bp::args("self"),"Retrieve all the points.",
          bp::with_custodian_and_ward_postcall<0,1>())
  //    .add_property ("points",
  //                   bp::make_function(&ConvexBaseWrapper::points,bp::with_custodian_and_ward_postcall<0,1>()),
  //                   "Points of the convex.")
Joseph Mirabel's avatar
Joseph Mirabel committed
257
    .def ("neighbors", &ConvexBaseWrapper::neighbors)
Joseph Mirabel's avatar
Joseph Mirabel committed
258
259
260
261
    .def ("convexHull", &ConvexBaseWrapper::convexHull,
        doxygen::member_func_doc(&ConvexBase::convexHull),
        return_value_policy<manage_new_object>())
    .staticmethod("convexHull")
262
263
264
    .def ("clone", &ConvexBase::clone,
      doxygen::member_func_doc(&ConvexBase::clone),
      return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
265
266
267
    ;

  class_ <Convex<Triangle>, bases<ConvexBase>, shared_ptr<Convex<Triangle> >, noncopyable>
Joseph Mirabel's avatar
Joseph Mirabel committed
268
    ("Convex", doxygen::class_doc< Convex<Triangle> >(), no_init)
269
    .def("__init__", make_constructor(&ConvexWrapper<Triangle>::constructor))
270
    .DEF_RO_CLASS_ATTRIB (Convex<Triangle>, num_polygons)
Joseph Mirabel's avatar
Joseph Mirabel committed
271
272
273
274
    .def ("polygons", &ConvexWrapper<Triangle>::polygons)
    ;

  class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >
Joseph Mirabel's avatar
Joseph Mirabel committed
275
    ("Cylinder", doxygen::class_doc<Cylinder>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
276
    .def (dv::init<Cylinder, FCL_REAL, FCL_REAL>())
277
278
    .DEF_RW_CLASS_ATTRIB (Cylinder, radius)
    .DEF_RW_CLASS_ATTRIB (Cylinder, halfLength)
279
280
281
    .def ("clone", &Cylinder::clone,
          doxygen::member_func_doc(&Cylinder::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
282
283
284
    ;

  class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >
Joseph Mirabel's avatar
Joseph Mirabel committed
285
    ("Halfspace", doxygen::class_doc<Halfspace>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
286
287
288
    .def (dv::init<Halfspace, const Vec3f&, FCL_REAL>())
    .def (dv::init<Halfspace, FCL_REAL,FCL_REAL,FCL_REAL,FCL_REAL>())
    .def (dv::init<Halfspace>())
289
290
    .DEF_RW_CLASS_ATTRIB (Halfspace, n)
    .DEF_RW_CLASS_ATTRIB (Halfspace, d)
291
292
293
    .def ("clone", &Halfspace::clone,
          doxygen::member_func_doc(&Halfspace::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
294
295
296
    ;

  class_ <Plane, bases<ShapeBase>, shared_ptr<Plane> >
Joseph Mirabel's avatar
Joseph Mirabel committed
297
    ("Plane", doxygen::class_doc<Plane>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
298
299
300
    .def (dv::init<Plane, const Vec3f&, FCL_REAL>())
    .def (dv::init<Plane, FCL_REAL,FCL_REAL,FCL_REAL,FCL_REAL>())
    .def (dv::init<Plane>())
301
302
    .DEF_RW_CLASS_ATTRIB (Plane, n)
    .DEF_RW_CLASS_ATTRIB (Plane, d)
303
304
305
    .def ("clone", &Plane::clone,
          doxygen::member_func_doc(&Plane::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
306
307
308
    ;

  class_ <Sphere, bases<ShapeBase>, shared_ptr<Sphere> >
Joseph Mirabel's avatar
Joseph Mirabel committed
309
    ("Sphere", doxygen::class_doc<Sphere>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
310
    .def (dv::init<Sphere, FCL_REAL>())
311
    .DEF_RW_CLASS_ATTRIB (Sphere, radius)
312
313
314
    .def ("clone", &Sphere::clone,
          doxygen::member_func_doc(&Sphere::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
315
316
317
    ;

  class_ <TriangleP, bases<ShapeBase>, shared_ptr<TriangleP> >
Joseph Mirabel's avatar
Joseph Mirabel committed
318
    ("TriangleP", doxygen::class_doc<TriangleP>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
319
    .def (dv::init<TriangleP, const Vec3f&, const Vec3f&, const Vec3f&>())
320
321
322
    .DEF_RW_CLASS_ATTRIB (TriangleP, a)
    .DEF_RW_CLASS_ATTRIB (TriangleP, b)
    .DEF_RW_CLASS_ATTRIB (TriangleP, c)
323
324
325
    .def ("clone", &TriangleP::clone,
          doxygen::member_func_doc(&TriangleP::clone),
          return_value_policy<manage_new_object>())
Joseph Mirabel's avatar
Joseph Mirabel committed
326
327
328
329
    ;

}

330
331
332
333
boost::python::tuple AABB_distance_proxy(const AABB & self, const AABB & other)
{
  Vec3f P,Q;
  FCL_REAL distance = self.distance(other,&P,&Q);
334
  return boost::python::make_tuple(distance,P,Q);
335
336
}

Joseph Mirabel's avatar
Joseph Mirabel committed
337
338
void exposeCollisionGeometries ()
{
339
  
340
341
342
343
  enum_<BVHModelType>("BVHModelType")
    .value ("BVH_MODEL_UNKNOWN"    , BVH_MODEL_UNKNOWN)
    .value ("BVH_MODEL_TRIANGLES"  , BVH_MODEL_TRIANGLES)
    .value ("BVH_MODEL_POINTCLOUD" , BVH_MODEL_POINTCLOUD)
344
    .export_values()
345
346
  ;
  
347
348
349
350
351
352
353
354
  enum_<BVHBuildState>("BVHBuildState")
  .value("BVH_BUILD_STATE_EMPTY",BVH_BUILD_STATE_EMPTY)
  .value("BVH_BUILD_STATE_BEGUN",BVH_BUILD_STATE_BEGUN)
  .value("BVH_BUILD_STATE_PROCESSED",BVH_BUILD_STATE_PROCESSED)
  .value("BVH_BUILD_STATE_UPDATE_BEGUN",BVH_BUILD_STATE_UPDATE_BEGUN)
  .value("BVH_BUILD_STATE_UPDATED",BVH_BUILD_STATE_UPDATED)
  .value("BVH_BUILD_STATE_REPLACE_BEGUN",BVH_BUILD_STATE_REPLACE_BEGUN)
  .export_values()
355
356
  ;
  
357
358
359
360
361
362
363
  if(!eigenpy::register_symbolic_link_to_registered_type<OBJECT_TYPE>())
  {
    enum_<OBJECT_TYPE>("OBJECT_TYPE")
      .value ("OT_UNKNOWN", OT_UNKNOWN)
      .value ("OT_BVH"    , OT_BVH)
      .value ("OT_GEOM"   , OT_GEOM)
      .value ("OT_OCTREE" , OT_OCTREE)
364
      .export_values()
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
      ;
  }
  
  if(!eigenpy::register_symbolic_link_to_registered_type<NODE_TYPE>())
  {
    enum_<NODE_TYPE>("NODE_TYPE")
      .value ("BV_UNKNOWN", BV_UNKNOWN)
      .value ("BV_AABB"  , BV_AABB)
      .value ("BV_OBB"   , BV_OBB)
      .value ("BV_RSS"   , BV_RSS)
      .value ("BV_kIOS"  , BV_kIOS)
      .value ("BV_OBBRSS", BV_OBBRSS)
      .value ("BV_KDOP16", BV_KDOP16)
      .value ("BV_KDOP18", BV_KDOP18)
      .value ("BV_KDOP24", BV_KDOP24)
      .value ("GEOM_BOX"      , GEOM_BOX)
      .value ("GEOM_SPHERE"   , GEOM_SPHERE)
      .value ("GEOM_CAPSULE"  , GEOM_CAPSULE)
      .value ("GEOM_CONE"     , GEOM_CONE)
      .value ("GEOM_CYLINDER" , GEOM_CYLINDER)
      .value ("GEOM_CONVEX"   , GEOM_CONVEX)
      .value ("GEOM_PLANE"    , GEOM_PLANE)
      .value ("GEOM_HALFSPACE", GEOM_HALFSPACE)
      .value ("GEOM_TRIANGLE" , GEOM_TRIANGLE)
      .value ("GEOM_OCTREE"   , GEOM_OCTREE)
390
      .export_values()
391
392
      ;
  }
393
394
395
396
  
  class_<AABB>("AABB",
               "A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points",
               no_init)
397
398
399
400
401
    .def(init<>(bp::arg("self"), "Default constructor"))
    .def(init<Vec3f>(bp::args("self","v"),"Creating an AABB at position v with zero size."))
    .def(init<Vec3f,Vec3f>(bp::args("self","a","b"),"Creating an AABB with two endpoints a and b."))
    .def(init<AABB,Vec3f>(bp::args("self","core","delta"),"Creating an AABB centered as core and is of half-dimension delta."))
    .def(init<Vec3f,Vec3f,Vec3f>(bp::args("self","a","b","c"),"Creating an AABB contains three points."))
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
  
    .def("contain",
         (bool (AABB::*)(const Vec3f &) const)&AABB::contain,
         bp::args("self","p"),
         "Check whether the AABB contains a point p.")
    .def("contain",
         (bool (AABB::*)(const AABB &) const)&AABB::contain,
         bp::args("self","other"),
         "Check whether the AABB contains another AABB.")
  
    .def("overlap",
         (bool (AABB::*)(const AABB &) const)&AABB::overlap,
         bp::args("self","other"),
         "Check whether two AABB are overlap.")
    .def("overlap",
         (bool (AABB::*)(const AABB&, AABB&) const)&AABB::overlap,
         bp::args("self","other","overlapping_part"),
         "Check whether two AABB are overlaping and return the overloaping part if true.")
  
    .def("distance",
         (FCL_REAL (AABB::*)(const AABB &) const)&AABB::distance,
         bp::args("self","other"),
         "Distance between two AABBs.")
//    .def("distance",
//         AABB_distance_proxy,
//         bp::args("self","other"),
//         "Distance between two AABBs.")
  
    .def(bp::self + bp::self)
    .def(bp::self += bp::self)
    .def(bp::self += Vec3f())
  
    .def("size",&AABB::volume,bp::arg("self"),"Size of the AABB.")
    .def("center",&AABB::center,bp::arg("self"),"Center of the AABB.")
    .def("width",&AABB::width,bp::arg("self"),"Width of the AABB.")
    .def("height",&AABB::height,bp::arg("self"),"Height of the AABB.")
    .def("depth",&AABB::depth,bp::arg("self"),"Depth of the AABB.")
    .def("volume",&AABB::volume,bp::arg("self"),"Volume of the AABB.")
  
    .def("expand",
         (AABB& (AABB::*)(const AABB &, FCL_REAL))&AABB::expand,
         bp::args("self","core","ratio"),
         "Expand the AABB by increase the thickness of the plate by a ratio.",
         bp::return_internal_reference<>())
    .def("expand",
         (AABB& (AABB::*)(const Vec3f &))&AABB::expand,
         bp::args("self","delta"),
         "Expand the half size of the AABB by delta, and keep the center unchanged.",
         bp::return_internal_reference<>());
  
  def("translate",
      (AABB (*)(const AABB&, const Vec3f&))&translate,
      bp::args("aabb","t"),
      "Translate the center of AABB by t");
  
  def("rotate",
      (AABB (*)(const AABB&, const Matrix3f&))&rotate,
      bp::args("aabb","R"),
      "Rotate the AABB object by R");
Joseph Mirabel's avatar
Joseph Mirabel committed
461

462
463
464
465
466
467
  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionGeometry>())
  {
    class_ <CollisionGeometry, CollisionGeometryPtr_t, noncopyable>
      ("CollisionGeometry", no_init)
      .def ("getObjectType", &CollisionGeometry::getObjectType)
      .def ("getNodeType", &CollisionGeometry::getNodeType)
Joseph Mirabel's avatar
Joseph Mirabel committed
468

469
      .def ("computeLocalAABB", &CollisionGeometry::computeLocalAABB)
Joseph Mirabel's avatar
Joseph Mirabel committed
470

471
472
473
474
      .def ("computeCOM", &CollisionGeometry::computeCOM)
      .def ("computeMomentofInertia", &CollisionGeometry::computeMomentofInertia)
      .def ("computeVolume", &CollisionGeometry::computeVolume)
      .def ("computeMomentofInertiaRelatedToCOM", &CollisionGeometry::computeMomentofInertiaRelatedToCOM)
475

476
477
478
479
480
481
482
483
484
485
486
      .def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius.")
      .def_readwrite("aabb_center",&CollisionGeometry::aabb_center,"AABB center in local coordinate.")
      .def_readwrite("aabb_local",&CollisionGeometry::aabb_local,"AABB in local coordinate, used for tight AABB when only translation transform.")
    
      .def("isOccupied",&CollisionGeometry::isOccupied,bp::arg("self"),"Whether the object is completely occupied.")
      .def("isFree",&CollisionGeometry::isFree,bp::arg("self"),"Whether the object is completely free.")
      .def("isUncertain",&CollisionGeometry::isUncertain,bp::arg("self"),"Whether the object has some uncertainty.")

      .def_readwrite("cost_density",&CollisionGeometry::cost_density,"Collision cost for unit volume.")
      .def_readwrite("threshold_occupied",&CollisionGeometry::threshold_occupied,"Threshold for occupied ( >= is occupied).")
      .def_readwrite("threshold_free",&CollisionGeometry::threshold_free,"Threshold for free (<= is free).")
487
488
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
489
490
491

  exposeShapes();

Joseph Mirabel's avatar
Joseph Mirabel committed
492
493
  class_ <BVHModelBase, bases<CollisionGeometry>, BVHModelPtr_t, noncopyable>
    ("BVHModelBase", no_init)
Justin Carpentier's avatar
Justin Carpentier committed
494
    .def ("vertex", &BVHModelBaseWrapper::vertex,
495
496
          bp::args("self","index"),"Retrieve the vertex given by its index.",
          bp::return_internal_reference<>())
Justin Carpentier's avatar
Justin Carpentier committed
497
    .def ("vertices", &BVHModelBaseWrapper::vertex,
Justin Carpentier's avatar
Justin Carpentier committed
498
          bp::args("self","index"),"Retrieve the vertex given by its index.",
Justin Carpentier's avatar
Justin Carpentier committed
499
          ::hpp::fcl::python::deprecated_member< bp::return_internal_reference<> >())
500
    .def ("vertices", &BVHModelBaseWrapper::vertices,
Justin Carpentier's avatar
Justin Carpentier committed
501
          bp::args("self"),"Retrieve all the vertices.",
502
          bp::with_custodian_and_ward_postcall<0,1>())
Justin Carpentier's avatar
Justin Carpentier committed
503
504
505
//    .add_property ("vertices",
//                   bp::make_function(&BVHModelBaseWrapper::vertices,bp::with_custodian_and_ward_postcall<0,1>()),
//                   "Vertices of the BVH.")
506
507
    .def ("tri_indices", &BVHModelBaseWrapper::tri_indices,
          bp::args("self","index"),"Retrieve the triangle given by its index.")
Joseph Mirabel's avatar
Joseph Mirabel committed
508
509
    .def_readonly ("num_vertices", &BVHModelBase::num_vertices)
    .def_readonly ("num_tris", &BVHModelBase::num_tris)
510
    .def_readonly ("build_state", &BVHModelBase::build_state)
Joseph Mirabel's avatar
Joseph Mirabel committed
511
512
513

    .def_readonly ("convex", &BVHModelBase::convex)

514
515
    .DEF_CLASS_FUNC(BVHModelBase, buildConvexRepresentation)
    .DEF_CLASS_FUNC(BVHModelBase, buildConvexHull)
516
517
518
519

    // Expose function to build a BVH
    .def(dv::member_func("beginModel", &BVHModelBase::beginModel))
    .def(dv::member_func("addVertex", &BVHModelBase::addVertex))
Justin Carpentier's avatar
Justin Carpentier committed
520
    .def(dv::member_func("addVertices", &BVHModelBase::addVertices))
521
    .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle))
522
    .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles))
523
524
525
526
527
528
529
530
531
    .def(dv::member_func<int (BVHModelBase::*)(const Vec3fs&, const Triangles&)>("addSubModel", &BVHModelBase::addSubModel))
    .def(dv::member_func<int (BVHModelBase::*)(const Vec3fs&                  )>("addSubModel", &BVHModelBase::addSubModel))
    .def(dv::member_func("endModel", &BVHModelBase::endModel))
    // Expose function to replace a BVH
    .def(dv::member_func("beginReplaceModel", &BVHModelBase::beginReplaceModel))
    .def(dv::member_func("replaceVertex", &BVHModelBase::replaceVertex))
    .def(dv::member_func("replaceTriangle", &BVHModelBase::replaceTriangle))
    .def(dv::member_func("replaceSubModel", &BVHModelBase::replaceSubModel))
    .def(dv::member_func("endReplaceModel", &BVHModelBase::endReplaceModel))
532
    .def(dv::member_func("getModelType", &BVHModelBase::getModelType))
Joseph Mirabel's avatar
Joseph Mirabel committed
533
    ;
Joseph Mirabel's avatar
Joseph Mirabel committed
534
535
  exposeBVHModel<OBB    >("OBB"    );
  exposeBVHModel<OBBRSS >("OBBRSS" );
536
  exposeComputeMemoryFootprint();
Joseph Mirabel's avatar
Joseph Mirabel committed
537
}
538
539
540
541
542
543
544
545
546
547
548
549
550

void exposeCollisionObject ()
{
  namespace bp = boost::python;

  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionObject>())
  {
    class_ <CollisionObject, CollisionObjectPtr_t>
      ("CollisionObject", no_init)
      .def (dv::init<CollisionObject, const CollisionGeometryPtr_t&>())
      .def (dv::init<CollisionObject, const CollisionGeometryPtr_t&, const Transform3f&>())
      .def (dv::init<CollisionObject, const CollisionGeometryPtr_t&, const Matrix3f&, const Vec3f&>())

Justin Carpentier's avatar
Justin Carpentier committed
551
552
553
      .DEF_CLASS_FUNC(CollisionObject, getObjectType)
      .DEF_CLASS_FUNC(CollisionObject, getNodeType)
      .DEF_CLASS_FUNC(CollisionObject, computeAABB)
554
      .DEF_CLASS_FUNC2(CollisionObject, getAABB, bp::return_value_policy<bp::copy_const_reference>())
555
556
557
558
559
560
561
562
563
564
565
566

      .DEF_CLASS_FUNC2(CollisionObject, getTranslation, bp::return_value_policy<bp::copy_const_reference>())
      .DEF_CLASS_FUNC(CollisionObject, setTranslation)
      .DEF_CLASS_FUNC2(CollisionObject, getRotation, bp::return_value_policy<bp::copy_const_reference>())
      .DEF_CLASS_FUNC(CollisionObject, setRotation)
      .DEF_CLASS_FUNC2(CollisionObject, getTransform, bp::return_value_policy<bp::copy_const_reference>())
      .def(dv::member_func("setTransform",
          static_cast<void (CollisionObject::*) (const Transform3f&)>(&CollisionObject::setTransform)))

      .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform)
      .DEF_CLASS_FUNC(CollisionObject, setIdentityTransform)
      
Justin Carpentier's avatar
Justin Carpentier committed
567
      .def(dv::member_func("collisionGeometry",
568
          static_cast<const CollisionGeometryPtr_t& (CollisionObject::*) ()>(&CollisionObject::collisionGeometry),
Justin Carpentier's avatar
Justin Carpentier committed
569
          bp::return_value_policy<bp::copy_const_reference>()))
570
571
572
      ;
  }
}