Merge branch 'ign-math6' into ahcorde/pybind11/massMatrix

include/ignition/math/AxisAlignedBox.hh

@@ -132,6 +132,16 @@ namespace ignition
       /// \return The new box
       public: AxisAlignedBox operator+(const Vector3d &_v);
 
+      /// \brief Subtract a vector from the min and max values
+      /// \param _v The vector to use during subtraction
+      /// \return The new box
+      public: AxisAlignedBox operator-(const Vector3d &_v) const;
+
+      /// \brief Add a vector to the min and max values
+      /// \param _v The vector to use during addition
+      /// \return The new box
+      public: AxisAlignedBox operator+(const Vector3d &_v) const;
+
       /// \brief Output operator
       /// \param[in] _out Output stream
       /// \param[in] _b AxisAlignedBox to output to the stream

src/AxisAlignedBox.cc

@@ -164,6 +164,18 @@ AxisAlignedBox AxisAlignedBox::operator+(const Vector3d &_v)
   return AxisAlignedBox(this->dataPtr->min + _v, this->dataPtr->max + _v);
 }
 
+//////////////////////////////////////////////////
+AxisAlignedBox AxisAlignedBox::operator-(const Vector3d &_v) const
+{
+  return AxisAlignedBox(this->dataPtr->min - _v, this->dataPtr->max - _v);
+}
+
+//////////////////////////////////////////////////
+AxisAlignedBox AxisAlignedBox::operator+(const Vector3d &_v) const
+{
+  return AxisAlignedBox(this->dataPtr->min + _v, this->dataPtr->max + _v);
+}
+
 //////////////////////////////////////////////////
 bool AxisAlignedBox::Intersects(const AxisAlignedBox &_box) const
 {

src/AxisAlignedBox_TEST.cc

@@ -201,22 +201,30 @@ TEST(AxisAlignedBoxTest, DefaultConstructor)
 TEST(AxisAlignedBoxTest, Minus)
 {
   AxisAlignedBox box1(1, 2, 3, 4, 5, 6);
+  const AxisAlignedBox box1Const(1, 2, 3, 4, 5, 6);
   Vector3d sub(1, 1, 1);
 
   AxisAlignedBox box2 = box1 - sub;
+  AxisAlignedBox box2Const = box1Const - sub;
   EXPECT_EQ(box2.Min(), box1.Min() - sub);
   EXPECT_EQ(box2.Max(), box1.Max() - sub);
+  EXPECT_EQ(box2Const.Min(), box1Const.Min() - sub);
+  EXPECT_EQ(box2Const.Max(), box1Const.Max() - sub);
 }
 
 /////////////////////////////////////////////////
 TEST(AxisAlignedBoxTest, Plus)
 {
   AxisAlignedBox box1(1, 2, 3, 4, 5, 6);
+  const AxisAlignedBox box1Const(1, 2, 3, 4, 5, 6);
   Vector3d add(1, 1, 1);
 
   AxisAlignedBox box2 = box1 + add;
+  const AxisAlignedBox box2Const = box1Const + add;
   EXPECT_EQ(box2.Min(), box1.Min() + add);
   EXPECT_EQ(box2.Max(), box1.Max() + add);
+  EXPECT_EQ(box2Const.Min(), box1Const.Min() + add);
+  EXPECT_EQ(box2Const.Max(), box1Const.Max() + add);
 }
 
 /////////////////////////////////////////////////

src/python/CMakeLists.txt

@@ -87,7 +87,6 @@ if (PYTHONLIBS_FOUND)
   if (BUILD_TESTING)
     # Add the Python tests
     set(python_tests
-      AxisAlignedBox_TEST
       Box_TEST
       Cylinder_TEST
       DiffDriveOdometry_TEST

src/python_pybind11/CMakeLists.txt

@@ -15,6 +15,7 @@ if (${pybind11_FOUND})
   pybind11_add_module(math SHARED
     src/_ignition_math_pybind11.cc
     src/Angle.cc
+    src/AxisAlignedBox.cc
     src/Color.cc
     src/GaussMarkovProcess.cc
     src/Helpers.cc
@@ -79,6 +80,7 @@ if (${pybind11_FOUND})
     # Add the Python tests
     set(python_tests
       Angle_TEST
+      AxisAlignedBox_TEST
       Color_TEST
       Filter_TEST
       GaussMarkovProcess_TEST

src/python_pybind11/src/AxisAlignedBox.cc

@@ -0,0 +1,122 @@
+/*
+ * 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.
+ *
+*/
+
+#include <string>
+
+#include <pybind11/operators.h>
+
+#include "AxisAlignedBox.hh"
+
+#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/Vector3.hh>
+
+using namespace pybind11::literals;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+void defineMathAxisAlignedBox(py::module &m, const std::string &typestr)
+{
+  using Class = ignition::math::AxisAlignedBox;
+  auto toString = [](const Class &si) {
+    std::stringstream stream;
+    stream << si;
+    return stream.str();
+  };
+  py::class_<Class>(
+    m,
+    typestr.c_str(),
+    py::buffer_protocol(),
+    py::dynamic_attr())
+    .def(py::init<>())
+    .def(py::init<const Class&>())
+    .def(py::init<double, double, double,
+                  double, double, double>())
+    .def(py::init<const ignition::math::Vector3d&,
+                  const ignition::math::Vector3d>())
+    .def("x_length",
+         &Class::XLength,
+         "Get the length along the x dimension")
+    .def("y_length",
+         &Class::YLength,
+         "Get the length along the y dimension")
+    .def("z_length",
+         &Class::ZLength,
+         "Get the length along the z dimension")
+    .def("size",
+         &Class::Size,
+         "Get the size of the box")
+    .def("center",
+         &Class::Center,
+         "Get the box center")
+    .def("merge",
+         &Class::Merge,
+         "Merge a box with this box")
+    .def("volume",
+         &Class::Volume,
+         "Get the volume of the box in m^3.")
+    .def(py::self + py::self)
+    .def(py::self += py::self)
+    .def(py::self + ignition::math::Vector3d())
+    .def(py::self - ignition::math::Vector3d())
+    .def(py::self == py::self)
+    .def(py::self != py::self)
+    .def("min",
+         py::overload_cast<>(&Class::Min, py::const_),
+         py::return_value_policy::reference,
+         "Get the minimum corner.")
+    .def("max",
+         py::overload_cast<>(&Class::Max, py::const_),
+         py::return_value_policy::reference,
+         "Get the maximum corner.")
+    .def("intersects",
+         &Class::Intersects,
+         "Test box intersection. This test will only work if "
+         " both box's minimum corner is less than or equal to their "
+         " maximum corner.")
+    .def("contains",
+         &Class::Contains,
+         "Check if a point lies inside the box.")
+    .def("intersect_check",
+         &Class::IntersectCheck,
+         "Check if a ray (origin, direction) intersects the box.")
+    .def("intersect_dist",
+         &Class::IntersectDist,
+         "Check if a ray (origin, direction) intersects the box.")
+    .def("intersect",
+         py::overload_cast<const Vector3d &, const Vector3d &,
+                           const double, const double>
+          (&Class::Intersect, py::const_),
+         "Check if a ray (origin, direction) intersects the box.")
+    .def("intersect",
+         py::overload_cast<const Line3d&>(&Class::Intersect, py::const_),
+         "Check if a ray (origin, direction) intersects the box.")
+    .def("__copy__", [](const Class &self) {
+      return Class(self);
+    })
+    .def("__deepcopy__", [](const Class &self, py::dict) {
+      return Class(self);
+    }, "memo"_a)
+    .def("__str__", toString)
+    .def("__repr__", toString);
+}
+}  // namespace python
+}  // namespace math
+}  // namespace ignition

src/python_pybind11/src/AxisAlignedBox.hh

@@ -0,0 +1,42 @@
+/*
+ * 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__AXISALIGNEDBOX_HPP_
+#define IGNITION_MATH_PYTHON__AXISALIGNEDBOX_HPP_
+
+#include <string>
+
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::AxisAlignedBox
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+void defineMathAxisAlignedBox(py::module &m, const std::string &typestr);
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__AXISALIGNEDBOX_HPP_

src/python_pybind11/src/_ignition_math_pybind11.cc

@@ -15,6 +15,7 @@
 #include <pybind11/pybind11.h>
 
 #include "Angle.hh"
+#include "AxisAlignedBox.hh"
 #include "Color.hh"
 #include "Filter.hh"
 #include "GaussMarkovProcess.hh"
@@ -48,6 +49,8 @@ PYBIND11_MODULE(math, m)
 
   ignition::math::python::defineMathAngle(m, "Angle");
 
+  ignition::math::python::defineMathAxisAlignedBox(m, "AxisAlignedBox");
+
   ignition::math::python::defineMathColor(m, "Color");
 
   ignition::math::python::defineMathGaussMarkovProcess(

src/python/AxisAlignedBox_TEST.py → src/python_pybind11/test/AxisAlignedBox_TEST.py

@@ -15,16 +15,15 @@
 import math
 import unittest
 
-from ignition.math import AxisAlignedBox, Line3d, Vector3d
-from ignition.math import IGN_SQRT2, LOW_D, MAX_D
+from ignition.math import AxisAlignedBox, Helpers, Line3d, Vector3d
 
 
 class TestAxisAlignedBox(unittest.TestCase):
 
     def test_empty_constructor(self):
         box = AxisAlignedBox()
-        self.assertEqual(Vector3d(MAX_D, MAX_D, MAX_D), box.min())
-        self.assertEqual(Vector3d(LOW_D, LOW_D, LOW_D), box.max())
+        self.assertEqual(Vector3d(Helpers.MAX_D, Helpers.MAX_D, Helpers.MAX_D), box.min())
+        self.assertEqual(Vector3d(Helpers.LOW_D, Helpers.LOW_D, Helpers.LOW_D), box.max())
 
     def test_constructor(self):
         box = AxisAlignedBox(Vector3d(0, -1, 2), Vector3d(1, -2, 3))
@@ -82,8 +81,8 @@ def test_merge_empty(self):
         box2 = AxisAlignedBox()
         box1.merge(box2)
 
-        self.assertEqual(Vector3d(MAX_D, MAX_D, MAX_D), box1.min())
-        self.assertEqual(Vector3d(LOW_D, LOW_D, LOW_D), box1.max())
+        self.assertEqual(Vector3d(Helpers.MAX_D, Helpers.MAX_D, Helpers.MAX_D), box1.min())
+        self.assertEqual(Vector3d(Helpers.LOW_D, Helpers.LOW_D, Helpers.LOW_D), box1.max())
 
     def test_default_constructor(self):
         defaultAxisAlignedBox1 = AxisAlignedBox()
@@ -125,12 +124,12 @@ def test_plus_empty(self):
         box2 = AxisAlignedBox()
         box1 += box2
 
-        self.assertEqual(Vector3d(MAX_D, MAX_D, MAX_D), box1.min())
-        self.assertEqual(Vector3d(LOW_D, LOW_D, LOW_D), box1.max())
+        self.assertEqual(Vector3d(Helpers.MAX_D, Helpers.MAX_D, Helpers.MAX_D), box1.min())
+        self.assertEqual(Vector3d(Helpers.LOW_D, Helpers.LOW_D, Helpers.LOW_D), box1.max())
 
         box3 = box2 + box1
-        self.assertEqual(Vector3d(MAX_D, MAX_D, MAX_D), box3.min())
-        self.assertEqual(Vector3d(LOW_D, LOW_D, LOW_D), box3.max())
+        self.assertEqual(Vector3d(Helpers.MAX_D, Helpers.MAX_D, Helpers.MAX_D), box3.min())
+        self.assertEqual(Vector3d(Helpers.LOW_D, Helpers.LOW_D, Helpers.LOW_D), box3.max())
 
     def test_merge(self):
         box = AxisAlignedBox(Vector3d(0, -1, 2), Vector3d(1, -2, 3))
@@ -259,7 +258,7 @@ def test_intersect(self):
         self.assertTrue(intersect)
         self.assertTrue(b.intersect_check(Vector3d(2, 2, 0),
                                           Vector3d(-1, -1, 0), 0, 1000))
-        self.assertEqual(dist, IGN_SQRT2)
+        self.assertEqual(dist, math.sqrt(2))
         self.assertEqual(b.intersect_dist(Vector3d(2, 2, 0),
                                           Vector3d(-1, -1, 0), 0, 1000)[1], dist)
         self.assertEqual(pt, Vector3d(1, 1, 0))
@@ -279,7 +278,7 @@ def test_intersect(self):
         self.assertTrue(intersect)
         self.assertTrue(b.intersect_check(Vector3d(-1, -2, 0),
                                           Vector3d(1, 1, 0), 0, 1000))
-        self.assertEqual(dist, 2*IGN_SQRT2)
+        self.assertEqual(dist, 2*math.sqrt(2))
         self.assertEqual(b.intersect_dist(Vector3d(-1, -2, 0),
                                           Vector3d(1, 1, 0), 0, 1000)[1], dist)
         self.assertEqual(pt, Vector3d(1, 0, 0))
@@ -289,7 +288,7 @@ def test_intersect(self):
         self.assertTrue(intersect)
         self.assertTrue(b.intersect_check(Vector3d(2, 1, 0),
                                           Vector3d(-1, -1, 0), 0, 1000))
-        self.assertEqual(dist, IGN_SQRT2)
+        self.assertEqual(dist, math.sqrt(2))
         self.assertEqual(b.intersect_dist(Vector3d(2, 1, 0),
                                           Vector3d(-1, -1, 0), 0, 1000)[1], dist)
         self.assertEqual(pt, Vector3d(1, 0, 0))