| 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 | #include <eigenpy/geometry.hpp> | ||
| 37 | |||
| 38 | #include "coal/fwd.hh" | ||
| 39 | #include "coal/math/transform.h" | ||
| 40 | #include "coal/serialization/transform.h" | ||
| 41 | |||
| 42 | #include "coal.hh" | ||
| 43 | #include "pickle.hh" | ||
| 44 | #include "serializable.hh" | ||
| 45 | |||
| 46 | #ifdef COAL_HAS_DOXYGEN_AUTODOC | ||
| 47 | #include "doxygen_autodoc/coal/math/transform.h" | ||
| 48 | #endif | ||
| 49 | |||
| 50 | using namespace boost::python; | ||
| 51 | using namespace coal; | ||
| 52 | using namespace coal::python; | ||
| 53 | |||
| 54 | namespace dv = doxygen::visitor; | ||
| 55 | |||
| 56 | template <typename Integer> | ||
| 57 | struct TriangleWrapper { | ||
| 58 | ✗ | static typename TriangleTpl<Integer>::IndexType getitem( | |
| 59 | const TriangleTpl<Integer>& t, int i) { | ||
| 60 | ✗ | if (i >= 3 || i <= -3) | |
| 61 | ✗ | PyErr_SetString(PyExc_IndexError, "Index out of range"); | |
| 62 | ✗ | return t[static_cast<typename coal::TriangleTpl<Integer>::IndexType>(i % | |
| 63 | ✗ | 3)]; | |
| 64 | } | ||
| 65 | ✗ | static void setitem(TriangleTpl<Integer>& t, int i, | |
| 66 | typename TriangleTpl<Integer>::IndexType v) { | ||
| 67 | ✗ | if (i >= 3 || i <= -3) | |
| 68 | ✗ | PyErr_SetString(PyExc_IndexError, "Index out of range"); | |
| 69 | ✗ | t[static_cast<typename coal::TriangleTpl<Integer>::IndexType>(i % 3)] = v; | |
| 70 | ✗ | } | |
| 71 | }; | ||
| 72 | |||
| 73 | template <typename IndexType> | ||
| 74 | 20 | void exposeTriangle(const std::string& classname) { | |
| 75 | typedef TriangleTpl<IndexType> TriangleType; | ||
| 76 | |||
| 77 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
20 | class_<TriangleType>(classname.c_str(), no_init) |
| 78 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
20 | .def(dv::init<TriangleType>()) |
| 79 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | .def(dv::init<TriangleType, typename TriangleType::IndexType, |
| 80 | typename TriangleType::IndexType, | ||
| 81 | 20 | typename TriangleType::IndexType>()) | |
| 82 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | .def("__getitem__", |
| 83 | &TriangleWrapper<typename TriangleType::IndexType>::getitem) | ||
| 84 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | .def("__setitem__", |
| 85 | &TriangleWrapper<typename TriangleType::IndexType>::setitem) | ||
| 86 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | .def(dv::member_func("set", &TriangleType::set)) |
| 87 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | .def(dv::member_func("size", &TriangleType::size)) |
| 88 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | .staticmethod("size") |
| 89 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
20 | .def(self == self); |
| 90 | 20 | } | |
| 91 | |||
| 92 | 5 | void exposeMaths() { | |
| 93 | 5 | eigenpy::enableEigenPy(); | |
| 94 | |||
| 95 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::Quaterniond>()) |
| 96 | 5 | eigenpy::exposeQuaternion(); | |
| 97 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>()) |
| 98 | 5 | eigenpy::exposeAngleAxis(); | |
| 99 | |||
| 100 | 5 | eigenpy::enableEigenPySpecific<Matrix3s>(); | |
| 101 | 5 | eigenpy::enableEigenPySpecific<Vec3s>(); | |
| 102 | |||
| 103 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | class_<Transform3s>("Transform3s", doxygen::class_doc<Transform3s>(), no_init) |
| 104 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s>()) |
| 105 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(dv::init<Transform3s, const Matrix3s::MatrixBase&, |
| 106 | 5 | const Vec3s::MatrixBase&>()) | |
| 107 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s, const Quats&, const Vec3s::MatrixBase&>()) |
| 108 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s, const Matrix3s&>()) |
| 109 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s, const Quats&>()) |
| 110 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s, const Vec3s&>()) |
| 111 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(dv::init<Transform3s, const Transform3s&>()) |
| 112 | |||
| 113 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation)) |
| 114 | 10 | .def("getTranslation", &Transform3s::getTranslation, | |
| 115 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | doxygen::member_func_doc(&Transform3s::getTranslation), |
| 116 | ✗ | return_value_policy<copy_const_reference>()) | |
| 117 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def("getRotation", &Transform3s::getRotation, |
| 118 | ✗ | return_value_policy<copy_const_reference>()) | |
| 119 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def("isIdentity", &Transform3s::isIdentity, |
| 120 | ✗ | (bp::arg("self"), | |
| 121 |
3/6✓ 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.
|
10 | bp::arg("prec") = Eigen::NumTraits<Scalar>::dummy_precision()), |
| 122 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | doxygen::member_func_doc(&Transform3s::getTranslation)) |
| 123 | |||
| 124 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation)) |
| 125 | 10 | .def("setTranslation", &Transform3s::setTranslation<Vec3s>) | |
| 126 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def("setRotation", &Transform3s::setRotation<Matrix3s>) |
| 127 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
5 | .def(dv::member_func("setTransform", |
| 128 | &Transform3s::setTransform<Matrix3s, Vec3s>)) | ||
| 129 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(dv::member_func( |
| 130 | "setTransform", | ||
| 131 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | static_cast<void (Transform3s::*)(const Quats&, const Vec3s&)>( |
| 132 | &Transform3s::setTransform))) | ||
| 133 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("setIdentity", &Transform3s::setIdentity)) |
| 134 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("Identity", &Transform3s::Identity)) |
| 135 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .staticmethod("Identity") |
| 136 | |||
| 137 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("setRandom", &Transform3s::setRandom)) |
| 138 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("Random", &Transform3s::Random)) |
| 139 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .staticmethod("Random") |
| 140 | |||
| 141 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("transform", &Transform3s::transform<Vec3s>)) |
| 142 | 10 | .def("inverseInPlace", &Transform3s::inverseInPlace, | |
| 143 | ✗ | return_internal_reference<>(), | |
| 144 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | doxygen::member_func_doc(&Transform3s::inverseInPlace)) |
| 145 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("inverse", &Transform3s::inverse)) |
| 146 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes)) |
| 147 | |||
| 148 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(self * self) |
| 149 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(self *= self) |
| 150 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(self == self) |
| 151 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
5 | .def(self != self) |
| 152 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def_pickle(PickleObject<Transform3s>()) |
| 153 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(SerializableVisitor<Transform3s>()); |
| 154 | |||
| 155 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | exposeTriangle<Triangle32::IndexType>("Triangle32"); |
| 156 |
5/10✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
|
5 | bp::scope().attr("Triangle") = bp::scope().attr("Triangle32"); |
| 157 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | exposeTriangle<Triangle16::IndexType>("Triangle16"); |
| 158 | |||
| 159 | 5 | if (!eigenpy::register_symbolic_link_to_registered_type< | |
| 160 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<Vec3s> >()) { |
| 161 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | class_<std::vector<Vec3s> >("StdVec_Vec3s") |
| 162 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(vector_indexing_suite<std::vector<Vec3s> >()); |
| 163 | } | ||
| 164 | 5 | if (!eigenpy::register_symbolic_link_to_registered_type< | |
| 165 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<Triangle32> >()) { |
| 166 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | class_<std::vector<Triangle32> >("StdVec_Triangle32") |
| 167 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(vector_indexing_suite<std::vector<Triangle32> >()); |
| 168 |
5/10✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
|
5 | bp::scope().attr("StdVec_Triangle") = bp::scope().attr("StdVec_Triangle32"); |
| 169 | } | ||
| 170 | 5 | if (!eigenpy::register_symbolic_link_to_registered_type< | |
| 171 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | std::vector<Triangle16> >()) { |
| 172 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | class_<std::vector<Triangle16> >("StdVec_Triangle16") |
| 173 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | .def(vector_indexing_suite<std::vector<Triangle16> >()); |
| 174 | } | ||
| 175 | 5 | } | |
| 176 |