collision.cc 5.11 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
39
#include <eigenpy/registration.hpp>

Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
43
44
45
46
47
48
49
50
#include "fcl.hh"

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

using namespace boost::python;

using namespace hpp::fcl;

void exposeCollisionAPI ()
{
51
52
53
54
55
56
57
58
  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequestFlag>())
  {
    enum_ <CollisionRequestFlag> ("CollisionRequestFlag")
      .value ("CONTACT", CONTACT)
      .value ("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND)
      .value ("NO_REQUEST", NO_REQUEST)
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
59

60
61
62
63
64
65
66
67
68
69
70
71
72
73
  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>())
  {
    class_ <CollisionRequest> ("CollisionRequest", init<>())
      .def (init<CollisionRequestFlag, size_t>())
    
      .def_readwrite ("num_max_contacts"           , &CollisionRequest::num_max_contacts)
      .def_readwrite ("enable_contact"             , &CollisionRequest::enable_contact)
      .def_readwrite ("enable_distance_lower_bound", &CollisionRequest::enable_distance_lower_bound)
      .def_readwrite ("enable_cached_gjk_guess"    , &CollisionRequest::enable_cached_gjk_guess)
      .def_readwrite ("cached_gjk_guess"           , &CollisionRequest::cached_gjk_guess)
      .def_readwrite ("security_margin"            , &CollisionRequest::security_margin)
      .def_readwrite ("break_distance"             , &CollisionRequest::break_distance)
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
74

75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
  if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
  {
    class_ <Contact> ("Contact", init<>())
      //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
      //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
      .def_readonly ("o1", &Contact::o1)
      .def_readonly ("o2", &Contact::o2)
      .def_readwrite ("b1", &Contact::b1)
      .def_readwrite ("b2", &Contact::b2)
      .def_readwrite ("normal", &Contact::normal)
      .def_readwrite ("pos", &Contact::pos)
      .def_readwrite ("penetration_depth", &Contact::penetration_depth)
      .def (self == self)
      .def (self != self)
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
91

92
93
94
95
96
97
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<Contact> >())
  {
    class_< std::vector<Contact> >("StdVec_Contact")
      .def(vector_indexing_suite< std::vector<Contact> >())
      ;
  }
98

99
100
101
102
103
104
105
106
107
108
109
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionResult> >())
  {
    class_ <CollisionResult> ("CollisionResult", init<>())
      .def ("isCollision", &CollisionResult::isCollision)
      .def ("numContacts", &CollisionResult::numContacts)
      .def ("getContact" , &CollisionResult::getContact , return_value_policy<copy_const_reference>())
      .def ("getContacts", &CollisionResult::getContacts, return_internal_reference<>())
      .def ("addContact" , &CollisionResult::addContact )
      .def ("clear", &CollisionResult::clear)
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
110

111
112
113
114
115
116
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionResult> >())
  {
    class_< std::vector<CollisionResult> >("CollisionResult")
      .def(vector_indexing_suite< std::vector<CollisionResult> >())
      ;
  }
117

Joseph Mirabel's avatar
Joseph Mirabel committed
118
119
120
121
122
123
124
  def ("collide", static_cast< std::size_t (*)(const CollisionObject*, const CollisionObject*,
        const CollisionRequest&, CollisionResult&) > (&collide));
  def ("collide", static_cast< std::size_t (*)(
        const CollisionGeometry*, const Transform3f&,
        const CollisionGeometry*, const Transform3f&,
        const CollisionRequest&, CollisionResult&) > (&collide));
}