Directory: | ./ |
---|---|
File: | python/distance.cc |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 47 | 56 | 83.9% |
Branches: | 39 | 78 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Software License Agreement (BSD License) | ||
3 | // | ||
4 | // Copyright (c) 2019 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 | |||
37 | #include "coal.hh" | ||
38 | |||
39 | COAL_COMPILER_DIAGNOSTIC_PUSH | ||
40 | COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
41 | #include "coal/fwd.hh" | ||
42 | #include "coal/distance.h" | ||
43 | #include "coal/serialization/collision_data.h" | ||
44 | |||
45 | #include "deprecation.hh" | ||
46 | COAL_COMPILER_DIAGNOSTIC_POP | ||
47 | |||
48 | #include "serializable.hh" | ||
49 | |||
50 | #ifdef COAL_HAS_DOXYGEN_AUTODOC | ||
51 | #include "doxygen_autodoc/functions.h" | ||
52 | #include "doxygen_autodoc/coal/collision_data.h" | ||
53 | #endif | ||
54 | |||
55 | using namespace boost::python; | ||
56 | using namespace coal; | ||
57 | using namespace coal::python; | ||
58 | |||
59 | namespace dv = doxygen::visitor; | ||
60 | |||
61 | struct DistanceResultWrapper { | ||
62 | ✗ | static Vec3s getNearestPoint1(const DistanceResult& res) { | |
63 | ✗ | return res.nearest_points[0]; | |
64 | } | ||
65 | ✗ | static Vec3s getNearestPoint2(const DistanceResult& res) { | |
66 | ✗ | return res.nearest_points[1]; | |
67 | } | ||
68 | }; | ||
69 | |||
70 | 5 | void exposeDistanceAPI() { | |
71 | COAL_COMPILER_DIAGNOSTIC_PUSH | ||
72 | COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
73 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | if (!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>()) { |
74 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | class_<DistanceRequest, bases<QueryRequest> >( |
75 | "DistanceRequest", doxygen::class_doc<DistanceRequest>(), | ||
76 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | init<optional<bool, Scalar, Scalar> >( |
77 |
5/10✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
|
20 | (arg("self"), arg("enable_nearest_points"), arg("rel_err"), |
78 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | arg("abs_err")), |
79 | "Constructor")) | ||
80 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | .add_property( |
81 | "enable_nearest_points", | ||
82 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | bp::make_function( |
83 | ✗ | +[](DistanceRequest& self) -> bool { | |
84 | ✗ | return self.enable_nearest_points; | |
85 | }, | ||
86 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
10 | deprecated_warning_policy<>( |
87 | "enable_nearest_points has been marked as deprecated. " | ||
88 | "Nearest points are always computed when computing " | ||
89 | "distance. They are the points of the shapes that achieve " | ||
90 | "a distance of " | ||
91 | "DistanceResult::min_distance.\n" | ||
92 | "Use `enable_signed_distance` if you want to compute a " | ||
93 | "signed minimum " | ||
94 | "distance (and thus its corresponding nearest points).")), | ||
95 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | bp::make_function( |
96 | ✗ | +[](DistanceRequest& self, const bool value) -> void { | |
97 | ✗ | self.enable_nearest_points = value; | |
98 | ✗ | }, | |
99 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
|
10 | deprecated_warning_policy<>( |
100 | "enable_nearest_points has been marked as deprecated. " | ||
101 | "Nearest points are always computed when computing " | ||
102 | "distance. They are the points of the shapes that achieve " | ||
103 | "a distance of " | ||
104 | "DistanceResult::min_distance.\n" | ||
105 | "Use `enable_signed_distance` if you want to compute a " | ||
106 | "signed minimum " | ||
107 | "distance (and thus its corresponding nearest points).")), | ||
108 | doxygen::class_attrib_doc<DistanceRequest>("enable_nearest_points")) | ||
109 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceRequest, enable_signed_distance) |
110 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceRequest, rel_err) |
111 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err) |
112 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(SerializableVisitor<DistanceRequest>()); |
113 | } | ||
114 | COAL_COMPILER_DIAGNOSTIC_POP | ||
115 | |||
116 | 5 | if (!eigenpy::register_symbolic_link_to_registered_type< | |
117 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<DistanceRequest> >()) { |
118 | 10 | class_<std::vector<DistanceRequest> >("StdVec_DistanceRequest") | |
119 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(vector_indexing_suite<std::vector<DistanceRequest> >()); |
120 | } | ||
121 | |||
122 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | if (!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>()) { |
123 | 10 | class_<DistanceResult, bases<QueryResult> >( | |
124 | "DistanceResult", doxygen::class_doc<DistanceResult>(), no_init) | ||
125 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<DistanceResult>()) |
126 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceResult, min_distance) |
127 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceResult, normal) |
128 | //.def_readwrite ("nearest_points", &DistanceResult::nearest_points) | ||
129 | 10 | .def("getNearestPoint1", &DistanceResultWrapper::getNearestPoint1, | |
130 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | doxygen::class_attrib_doc<DistanceResult>("nearest_points")) |
131 | 10 | .def("getNearestPoint2", &DistanceResultWrapper::getNearestPoint2, | |
132 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | doxygen::class_attrib_doc<DistanceResult>("nearest_points")) |
133 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RO_CLASS_ATTRIB(DistanceResult, nearest_points) |
134 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RO_CLASS_ATTRIB(DistanceResult, o1) |
135 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RO_CLASS_ATTRIB(DistanceResult, o2) |
136 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceResult, b1) |
137 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .DEF_RW_CLASS_ATTRIB(DistanceResult, b2) |
138 | |||
139 | 10 | .def("clear", &DistanceResult::clear, | |
140 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | doxygen::member_func_doc(&DistanceResult::clear)) |
141 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(SerializableVisitor<DistanceResult>()); |
142 | } | ||
143 | |||
144 | 5 | if (!eigenpy::register_symbolic_link_to_registered_type< | |
145 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<DistanceResult> >()) { |
146 | 10 | class_<std::vector<DistanceResult> >("StdVec_DistanceResult") | |
147 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(vector_indexing_suite<std::vector<DistanceResult> >()); |
148 | } | ||
149 | |||
150 | 5 | doxygen::def( | |
151 | "distance", | ||
152 | static_cast<Scalar (*)(const CollisionObject*, const CollisionObject*, | ||
153 | const DistanceRequest&, DistanceResult&)>( | ||
154 | &distance)); | ||
155 | 5 | doxygen::def( | |
156 | "distance", | ||
157 | static_cast<Scalar (*)(const CollisionGeometry*, const Transform3s&, | ||
158 | const CollisionGeometry*, const Transform3s&, | ||
159 | const DistanceRequest&, DistanceResult&)>( | ||
160 | &distance)); | ||
161 | |||
162 | 5 | class_<ComputeDistance>("ComputeDistance", | |
163 | doxygen::class_doc<ComputeDistance>(), no_init) | ||
164 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(dv::init<ComputeDistance, const CollisionGeometry*, |
165 | 10 | const CollisionGeometry*>()) | |
166 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def("__call__", |
167 | static_cast<Scalar (ComputeDistance::*)( | ||
168 | const Transform3s&, const Transform3s&, const DistanceRequest&, | ||
169 | DistanceResult&) const>(&ComputeDistance::operator())); | ||
170 | 5 | } | |
171 |