Skip to content

Commit

Permalink
Merge branch 'ign-math6' into ahcorde/pybind11/massMatrix
Browse files Browse the repository at this point in the history
  • Loading branch information
ahcorde authored Dec 28, 2021
2 parents 715ec6f + f564c74 commit 608ebac
Show file tree
Hide file tree
Showing 9 changed files with 211 additions and 14 deletions.
10 changes: 10 additions & 0 deletions include/ignition/math/AxisAlignedBox.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 12 additions & 0 deletions src/AxisAlignedBox.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
8 changes: 8 additions & 0 deletions src/AxisAlignedBox_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

/////////////////////////////////////////////////
Expand Down
1 change: 0 additions & 1 deletion src/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions src/python_pybind11/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -79,6 +80,7 @@ if (${pybind11_FOUND})
# Add the Python tests
set(python_tests
Angle_TEST
AxisAlignedBox_TEST
Color_TEST
Filter_TEST
GaussMarkovProcess_TEST
Expand Down
122 changes: 122 additions & 0 deletions src/python_pybind11/src/AxisAlignedBox.cc
Original file line number Diff line number Diff line change
@@ -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
42 changes: 42 additions & 0 deletions src/python_pybind11/src/AxisAlignedBox.hh
Original file line number Diff line number Diff line change
@@ -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_
3 changes: 3 additions & 0 deletions src/python_pybind11/src/_ignition_math_pybind11.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <pybind11/pybind11.h>

#include "Angle.hh"
#include "AxisAlignedBox.hh"
#include "Color.hh"
#include "Filter.hh"
#include "GaussMarkovProcess.hh"
Expand Down Expand Up @@ -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(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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))
Expand Down Expand Up @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand Down

0 comments on commit 608ebac

Please sign in to comment.