Added Plane pybind11 interface (#346)

* Added Plane pybind11 interface Signed-off-by: Alejandro Hernández <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Dec 29, 2021 · b148af5 · b148af5
1 parent 49c82e7
commit b148af5
Show file tree

Hide file tree

Showing 7 changed files with 311 additions and 41 deletions.
diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt
@@ -87,9 +87,7 @@ if (PYTHONLIBS_FOUND)
   if (BUILD_TESTING)
     # Add the Python tests
     set(python_tests
-      Box_TEST
       Frustum_TEST
-      Plane_TEST
       python_TEST
       SignalStats_TEST
       Sphere_TEST

diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt
@@ -84,6 +84,7 @@ if (${pybind11_FOUND})
     set(python_tests
       Angle_TEST
       AxisAlignedBox_TEST
+      Box_TEST
       Color_TEST
       Cylinder_TEST
       DiffDriveOdometry_TEST
@@ -100,6 +101,7 @@ if (${pybind11_FOUND})
       MassMatrix3_TEST
       MovingWindowFilter_TEST
       OrientedBox_TEST
+      Plane_TEST
       PID_TEST
       Pose3_TEST
       Quaternion_TEST

diff --git a/src/python_pybind11/src/Box.hh b/src/python_pybind11/src/Box.hh
@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_MATH_PYTHON__BOX_HH_
+#define IGNITION_MATH_PYTHON__BOX_HH_
+
+#include <string>
+#include <vector>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+
+#include <ignition/math/Box.hh>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::Box
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+template<typename T>
+void defineMathBox(py::module &m, const std::string &typestr)
+{
+
+  using Class = ignition::math::Box<T>;
+  std::string pyclass_name = typestr;
+  py::class_<Class>(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr())
+    .def(py::init<>())
+    .def(py::init<const T, const T, const T>())
+    .def(py::init<const T, const T, const T, const ignition::math::Material&>())
+    .def(py::init<const T, const T, const T, const ignition::math::Material&>())
+    .def(py::init<const ignition::math::Vector3<T>&>())
+    .def(py::init<const ignition::math::Vector3<T>&,
+                  const ignition::math::Material&>())
+    .def(py::self != py::self)
+    .def(py::self == py::self)
+    .def("size",
+         &Class::Size,
+         "Get the size of the box.")
+    .def("set_size",
+         py::overload_cast<const ignition::math::Vector3<T>&>
+          (&Class::SetSize),
+         "Set the size of the box.")
+    .def("set_size",
+         py::overload_cast<const T, const T, const T>
+          (&Class::SetSize),
+         "Set the size of the box.")
+    .def("material",
+         &Class::Material,
+         "Get the material associated with this box.")
+    .def("set_material",
+         &Class::SetMaterial,
+         "Set the material associated with this box.")
+    .def("volume",
+         &Class::Volume,
+         "Get the volume of the box in m^3.")
+    .def("volume_below",
+         &Class::VolumeBelow,
+         "Get the volume of the box below a plane.")
+    .def("center_of_volume_below",
+         &Class::CenterOfVolumeBelow,
+         "Center of volume below the plane. This is useful when "
+         "calculating where buoyancy should be applied, for example.")
+    .def("vertices_below",
+         [](const Class &self, const Plane<T> &_plane)
+         {
+           std::vector<ignition::math::Vector3<T>> result;
+           auto vertices = self.VerticesBelow(_plane);
+           for (auto & v : vertices)
+           {
+             result.push_back(v);
+           }
+           return result;
+         },
+         "All the vertices which are on or below the plane.")
+    .def("density_from_mass",
+         &Class::DensityFromMass,
+         "Compute the box's density given a mass value.")
+    .def("set_density_from_mass",
+         &Class::SetDensityFromMass,
+         "Set the density of this box based on a mass value.")
+    .def("mass_matrix",
+         &Class::MassMatrix,
+         "Get the mass matrix for this box. This function "
+         "is only meaningful if the box's size and material "
+         "have been set.")
+    .def("intersections",
+         [](const Class &self, const Plane<T> &_plane)
+         {
+           std::vector<ignition::math::Vector3<T>> result;
+           auto vertices = self.Intersections(_plane);
+           for (auto & v : vertices)
+           {
+             result.push_back(v);
+           }
+           return result;
+         },
+         "Get intersection between a plane and the box's edges. "
+         "Edges contained on the plane are ignored.")
+    .def("__copy__", [](const Class &self) {
+      return Class(self);
+    })
+    .def("__deepcopy__", [](const Class &self, py::dict) {
+      return Class(self);
+    }, "memo"_a);
+}
+
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__BOX_HH_
diff --git a/src/python_pybind11/src/Plane.hh b/src/python_pybind11/src/Plane.hh
@@ -0,0 +1,124 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_MATH_PYTHON__PLANE_HH_
+#define IGNITION_MATH_PYTHON__PLANE_HH_
+
+#include <string>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+
+#include <ignition/math/Plane.hh>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::Plane
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+template<typename T>
+void defineMathPlane(py::module &m, const std::string &typestr)
+{
+  using Class = ignition::math::Plane<T>;
+  std::string pyclass_name = typestr;
+  py::class_<Class> plane(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr());
+  plane.def(py::init<>())
+    .def(py::init<const ignition::math::Vector3<T>&, T>(),
+         py::arg("_normal") = ignition::math::Vector3<T>::Zero,
+         py::arg("_offset") = 0.0)
+    .def(py::init<const ignition::math::Vector3<T>&,
+                  const ignition::math::Vector2<T>&, T>())
+    .def(py::init<const Class&>())
+    .def("set",
+         py::overload_cast<const ignition::math::Vector3<T>&, T>
+             (&Class::Set),
+         "Set the plane")
+    .def("set",
+         py::overload_cast<const ignition::math::Vector3<T>&,
+                           const ignition::math::Vector2<T>&, T>
+             (&Class::Set),
+         "Set the plane")
+    .def("distance",
+         py::overload_cast<const ignition::math::Vector3<T>&>
+             (&Class::Distance, py::const_),
+         "The distance to the plane from the given point. The "
+         "distance can be negative, which indicates the point is on the "
+         "negative side of the plane.")
+    .def("distance",
+         py::overload_cast<const ignition::math::Vector3<T>&,
+                           const ignition::math::Vector3<T>&>
+             (&Class::Distance, py::const_),
+         "Get distance to the plane give an origin and direction.")
+    .def("intersection",
+         &Class::Intersection,
+         py::arg("_point") = ignition::math::Vector3<T>::Zero,
+         py::arg("_gradient") = ignition::math::Vector3<T>::Zero,
+         py::arg("_tolerance") = 1e-6,
+         "Get the intersection of an infinite line with the plane, "
+         "given the line's gradient and a point in parametrized space.")
+    .def("side",
+         py::overload_cast<const ignition::math::Vector3<T>&>
+             (&Class::Side, py::const_),
+         "The side of the plane a point is on.")
+    .def("side",
+         py::overload_cast<const ignition::math::AxisAlignedBox&>
+             (&Class::Side, py::const_),
+         "The side of the plane a point is on.")
+    .def("size",
+         py::overload_cast<>(&Class::Size),
+         py::return_value_policy::reference,
+         "Get the plane size")
+    .def("normal",
+         py::overload_cast<>(&Class::Normal),
+         py::return_value_policy::reference,
+         "Get the plane size")
+    .def("offset",
+         &Class::Offset,
+         "Get the plane offset")
+    .def("__copy__", [](const Class &self) {
+      return Class(self);
+    })
+    .def("__deepcopy__", [](const Class &self, py::dict) {
+      return Class(self);
+    }, "memo"_a);
+
+    py::enum_<ignition::math::Planed::PlaneSide>(m, "PlaneSide")
+      .value("NEGATIVE_SIDE", ignition::math::Planed::PlaneSide::NEGATIVE_SIDE)
+      .value("POSITIVE_SIDE", ignition::math::Planed::PlaneSide::POSITIVE_SIDE)
+      .value("NO_SIDE", ignition::math::Planed::PlaneSide::NO_SIDE)
+      .value("BOTH_SIDE", ignition::math::Planed::PlaneSide::BOTH_SIDE)
+      .export_values();
+}
+
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__PLANE_HH_
diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc
@@ -16,8 +16,9 @@
 
 #include "Angle.hh"
 #include "AxisAlignedBox.hh"
-#include "Cylinder.hh"
+#include "Box.hh"
 #include "Color.hh"
+#include "Cylinder.hh"
 #include "DiffDriveOdometry.hh"
 #include "Filter.hh"
 #include "GaussMarkovProcess.hh"
@@ -33,6 +34,7 @@
 #include "MovingWindowFilter.hh"
 #include "OrientedBox.hh"
 #include "PID.hh"
+#include "Plane.hh"
 #include "Pose3.hh"
 #include "Quaternion.hh"
 #include "Rand.hh"
@@ -103,6 +105,11 @@ PYBIND11_MODULE(math, m)
   ignition::math::python::defineMathVector3<int>(m, "Vector3i");
   ignition::math::python::defineMathVector3<float>(m, "Vector3f");
 
+  ignition::math::python::defineMathPlane<double>(m, "Planed");
+
+  ignition::math::python::defineMathBox<double>(m, "Boxd");
+  ignition::math::python::defineMathBox<float>(m, "Boxf");
+
   ignition::math::python::defineMathVector4<double>(m, "Vector4d");
   ignition::math::python::defineMathVector4<int>(m, "Vector4i");
   ignition::math::python::defineMathVector4<float>(m, "Vector4f");