| Directory: | ./ |
|---|---|
| File: | python/distance.cc |
| Date: | 2025-04-13 14:25:19 |
| 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 |