distance.cc 5.43 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
//
// Software License Agreement (BSD License)
//
4
//  Copyright (c) 2019 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
35
//  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.

#include <boost/python.hpp>
36
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
37

38
#include <eigenpy/eigenpy.hpp>
39

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

#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/distance.h>

45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
#include "doxygen_autodoc/functions.h"
#include "doxygen_autodoc/hpp/fcl/collision_data.h"
#endif

#include "../doc/python/doxygen.hh"
#include "../doc/python/doxygen-boost.hh"

#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)                                          \
  def (#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB))
#define DEF_CLASS_FUNC2(CLASS, ATTRIB,policy)                                  \
  def (#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB),policy)

Joseph Mirabel's avatar
Joseph Mirabel committed
64
65
66
67
using namespace boost::python;

using namespace hpp::fcl;

68
69
namespace dv = doxygen::visitor;

70
71
72
73
74
75
struct DistanceRequestWrapper
{
  static Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; }
  static Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; }
};

Joseph Mirabel's avatar
Joseph Mirabel committed
76
77
void exposeDistanceAPI ()
{
78
79
  if(!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>())
  {
80
    class_ <DistanceRequest, bases<QueryRequest> > ("DistanceRequest",
81
        doxygen::class_doc<DistanceRequest>(),
82
83
84
85
        init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
                                                 arg("enable_nearest_points"),
                                                 arg("rel_err"),
                                                 arg("abs_err")),"Constructor"))
86
87
88
      .DEF_RW_CLASS_ATTRIB (DistanceRequest, enable_nearest_points)
      .DEF_RW_CLASS_ATTRIB (DistanceRequest, rel_err)
      .DEF_RW_CLASS_ATTRIB (DistanceRequest, abs_err)
89
90
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
91

92
93
  if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
  {
94
    class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",
95
                             doxygen::class_doc<DistanceResult>(),
96
97
                             no_init)
      .def (dv::init<DistanceResult>())
98
      .DEF_RW_CLASS_ATTRIB (DistanceResult, min_distance)
99
      .DEF_RW_CLASS_ATTRIB(DistanceResult, normal)
100
      //.def_readwrite ("nearest_points", &DistanceResult::nearest_points)
101
102
103
104
105
106
107
108
      .def("getNearestPoint1",&DistanceRequestWrapper::getNearestPoint1,
          doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
      .def("getNearestPoint2",&DistanceRequestWrapper::getNearestPoint2,
          doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
      .DEF_RO_CLASS_ATTRIB (DistanceResult, o1)
      .DEF_RO_CLASS_ATTRIB (DistanceResult, o2)
      .DEF_RW_CLASS_ATTRIB (DistanceResult, b1)
      .DEF_RW_CLASS_ATTRIB (DistanceResult, b2)
Joseph Mirabel's avatar
Joseph Mirabel committed
109

110
111
      .def ("clear", &DistanceResult::clear,
          doxygen::member_func_doc(&DistanceResult::clear))
112
113
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
114

115
116
117
118
119
120
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<DistanceResult> >())
  {
    class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
      .def(vector_indexing_suite< std::vector<DistanceResult> >())
      ;
  }
121

122
  doxygen::def ("distance", static_cast< FCL_REAL (*)(const CollisionObject*, const CollisionObject*,
Joseph Mirabel's avatar
Joseph Mirabel committed
123
        const DistanceRequest&, DistanceResult&) > (&distance));
124
  doxygen::def ("distance", static_cast< FCL_REAL (*)(
Joseph Mirabel's avatar
Joseph Mirabel committed
125
126
        const CollisionGeometry*, const Transform3f&,
        const CollisionGeometry*, const Transform3f&,
127
        DistanceRequest&, DistanceResult&) > (&distance));
Joseph Mirabel's avatar
Joseph Mirabel committed
128
}