diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 1aa4b3668..1526aab2a 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -87,7 +87,6 @@ if (PYTHONLIBS_FOUND) if (BUILD_TESTING) # Add the Python tests set(python_tests - Frustum_TEST python_TEST SignalStats_TEST Sphere_TEST diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 11e566f59..38c5e1b55 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -18,6 +18,7 @@ if (${pybind11_FOUND}) src/AxisAlignedBox.cc src/Color.cc src/DiffDriveOdometry.cc + src/Frustum.cc src/GaussMarkovProcess.cc src/Helpers.cc src/Kmeans.cc @@ -89,6 +90,7 @@ if (${pybind11_FOUND}) Cylinder_TEST DiffDriveOdometry_TEST Filter_TEST + Frustum_TEST GaussMarkovProcess_TEST Helpers_TEST Inertial_TEST diff --git a/src/python_pybind11/src/Frustum.cc b/src/python_pybind11/src/Frustum.cc new file mode 100644 index 000000000..7434ff440 --- /dev/null +++ b/src/python_pybind11/src/Frustum.cc @@ -0,0 +1,115 @@ +/* + * 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 + +#include "Frustum.hh" +#include + +#include +#include + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathFrustum(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Frustum; + std::string pyclass_name = typestr; + py::class_ (m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init(), + py::arg("_near") = 0, + py::arg("_far") = 0, + py::arg("_fov") = ignition::math::Angle(0), + py::arg("_aspectRatio") = 0, + py::arg("_pose") = ignition::math::Pose3d::Zero) + .def(py::init()) + .def("near", + &Class::Near, + "Get the near distance. This is the distance from the " + "frustum's vertex to the closest plane.") + .def("set_near", + &Class::SetNear, + "Set the near distance. This is the distance from the " + "frustum's vertex to the closest plane.") + .def("far", + &Class::Far, + "Get the far distance. This is the distance from the " + "frustum's vertex to the farthest plane.") + .def("set_far", + &Class::SetFar, + "Set the far distance. This is the distance from the " + "frustum's vertex to the farthest plane.") + .def("fov", + &Class::FOV, + "Get the horizontal field of view. The field of view is the " + "angle between the frustum's vertex and the edges of the near or far") + .def("set_fov", + &Class::SetFOV, + "Set the horizontal field of view. The field of view is the " + "angle between the frustum's vertex and the edges of the near or far") + .def("aspect_ratio", + &Class::AspectRatio, + "Get the aspect ratio, which is the width divided by height " + "of the near or far planes.") + .def("set_aspect_ratio", + &Class::SetAspectRatio, + "Get the aspect ratio, which is the width divided by height " + "of the near or far planes.") + .def("pose", + &Class::Pose, + "Get the pose of the frustum") + .def("set_pose", + &Class::SetPose, + "Set the pose of the frustum") + .def("contains", + py::overload_cast + (&Class::Contains, py::const_), + "Check if a box lies inside the pyramid frustum.") + .def("contains", + py::overload_cast + (&Class::Contains, py::const_), + "Check if a point lies inside the pyramid frustum.") + .def("plane", + &Class::Plane, + "Get a plane of the frustum."); + + py::enum_(m, "FrustumPlane") + .value("FRUSTUM_PLANE_NEAR", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_NEAR) + .value("FRUSTUM_PLANE_FAR", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_FAR) + .value("FRUSTUM_PLANE_LEFT", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_LEFT) + .value("FRUSTUM_PLANE_RIGHT", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_RIGHT) + .value("FRUSTUM_PLANE_TOP", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_TOP) + .value("FRUSTUM_PLANE_BOTTOM", + ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_BOTTOM) + .export_values(); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Frustum.hh b/src/python_pybind11/src/Frustum.hh new file mode 100644 index 000000000..688baf013 --- /dev/null +++ b/src/python_pybind11/src/Frustum.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__FRUSTUM_HH_ +#define IGNITION_MATH_PYTHON__FRUSTUM_HH_ + +#include +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Frustum +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathFrustum(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__FRUSTUM_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 04013111f..834068b1f 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -21,6 +21,7 @@ #include "Cylinder.hh" #include "DiffDriveOdometry.hh" #include "Filter.hh" +#include "Frustum.hh" #include "GaussMarkovProcess.hh" #include "Helpers.hh" #include "Inertial.hh" @@ -155,6 +156,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathInertial(m, "Inertiald"); + ignition::math::python::defineMathFrustum(m, "Frustum"); + ignition::math::python::defineMathFilter(m, "Filteri"); ignition::math::python::defineMathFilter(m, "Filterf"); ignition::math::python::defineMathFilter(m, "Filterd"); diff --git a/src/python/Frustum_TEST.py b/src/python_pybind11/test/Frustum_TEST.py similarity index 77% rename from src/python/Frustum_TEST.py rename to src/python_pybind11/test/Frustum_TEST.py index f54f4070c..ed0fa5ff3 100644 --- a/src/python/Frustum_TEST.py +++ b/src/python_pybind11/test/Frustum_TEST.py @@ -15,7 +15,7 @@ import math import unittest -from ignition.math import Angle, AxisAlignedBox, Frustum, IGN_PI, Planed, Pose3d, Vector3d +from ignition.math import Angle, AxisAlignedBox, Frustum, FrustumPlane, Planed, Pose3d, Vector3d class TestFrustrum(unittest.TestCase): @@ -25,7 +25,7 @@ def test_constructor(self): self.assertEqual(frustum.near(), 0.0) self.assertEqual(frustum.far(), 1.0) - self.assertEqual(frustum.fov().radian(), 45 * IGN_PI / 180.0) + self.assertEqual(frustum.fov().radian(), 45 * math.pi / 180.0) self.assertEqual(frustum.aspect_ratio(), 1.0) self.assertEqual(frustum.pose(), Pose3d.ZERO) @@ -38,7 +38,7 @@ def test_copy_constructor(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 320.0 / 240.0, # Pose @@ -52,23 +52,23 @@ def test_copy_constructor(self): self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_NEAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_NEAR).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_NEAR).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_NEAR).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_FAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_FAR).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_FAR).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_FAR).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_LEFT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_LEFT).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_LEFT).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_LEFT).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_RIGHT).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_RIGHT).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_TOP).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_TOP).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_TOP).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_TOP).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_BOTTOM).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_BOTTOM).normal()) def test_assignment_operator(self): # Frustum pointing to the +X+Y diagonal @@ -78,11 +78,11 @@ def test_assignment_operator(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 320.0/240.0, # Pose - Pose3d(0, 0, 0, 0, 0, 45 * IGN_PI / 180.0)) + Pose3d(0, 0, 0, 0, 0, 45 * math.pi / 180.0)) frustum2 = Frustum() frustum2 = frustum @@ -93,23 +93,23 @@ def test_assignment_operator(self): self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) self.assertEqual(frustum.aspect_ratio(), frustum2.aspect_ratio()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_NEAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_NEAR).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_NEAR).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_NEAR).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_FAR).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_FAR).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_FAR).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_FAR).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_LEFT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_LEFT).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_LEFT).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_LEFT).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_RIGHT).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_RIGHT).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_RIGHT).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_TOP).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_TOP).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_TOP).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_TOP).normal()) - self.assertEqual(frustum.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal(), - frustum2.plane(Frustum.FRUSTUM_PLANE_BOTTOM).normal()) + self.assertEqual(frustum.plane(FrustumPlane.FRUSTUM_PLANE_BOTTOM).normal(), + frustum2.plane(FrustumPlane.FRUSTUM_PLANE_BOTTOM).normal()) def test_pyramid_x_axis_pos(self): # Frustum pointing down the +x axis @@ -119,7 +119,7 @@ def test_pyramid_x_axis_pos(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 320.0/240.0, # Pose @@ -145,11 +145,11 @@ def test_pyramid_x_axis_neg(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 320.0/240.0, # Pose - Pose3d(0, 0, 0, 0, 0, IGN_PI)) + Pose3d(0, 0, 0, 0, 0, math.pi)) self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) self.assertFalse(frustum.contains(Vector3d(-0.5, 0, 0))) @@ -172,11 +172,11 @@ def test_pyramid_y_axis(self): # Far distance 5, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, 0, IGN_PI*0.5)) + Pose3d(0, 0, 0, 0, 0, math.pi*0.5)) self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) self.assertFalse(frustum.contains(Vector3d(1, 0, 0))) @@ -199,11 +199,11 @@ def test_pyramid_z_axis(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) self.assertFalse(frustum.contains(Vector3d(0, 0, -0.9))) @@ -227,11 +227,11 @@ def test_near_far(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) self.assertEqual(frustum.near(), 1.0) self.assertEqual(frustum.far(), 10.0) @@ -249,13 +249,13 @@ def test_fov(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) - self.assertEqual(frustum.fov(), Angle(45 * IGN_PI / 180.0)) + self.assertEqual(frustum.fov(), Angle(45 * math.pi / 180.0)) frustum.set_fov(Angle(1.5707)) @@ -268,11 +268,11 @@ def test_aspect_ratio(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) self.assertEqual(frustum.aspect_ratio(), 1) @@ -288,17 +288,17 @@ def test_pose(self): # Far distance 10, # Field of view - Angle(45 * IGN_PI / 180.0), + Angle(45 * math.pi / 180.0), # Aspect ratio 1.0, # Pose - Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) - self.assertEqual(frustum.pose(), Pose3d(0, 0, 0, 0, IGN_PI*0.5, 0)) + self.assertEqual(frustum.pose(), Pose3d(0, 0, 0, 0, math.pi*0.5, 0)) - frustum.set_pose(Pose3d(1, 2, 3, IGN_PI, 0, 0)) + frustum.set_pose(Pose3d(1, 2, 3, math.pi, 0, 0)) - self.assertEqual(frustum.pose(), Pose3d(1, 2, 3, IGN_PI, 0, 0)) + self.assertEqual(frustum.pose(), Pose3d(1, 2, 3, math.pi, 0, 0)) def test_pose_contains(self): frustum = Frustum( @@ -307,11 +307,11 @@ def test_pose_contains(self): # Far distance 10, # Field of view - Angle(60 * IGN_PI / 180.0), + Angle(60 * math.pi / 180.0), # Aspect ratio 1920.0/1080.0, # Pose - Pose3d(0, -5, 0, 0, 0, IGN_PI*0.5)) + Pose3d(0, -5, 0, 0, 0, math.pi*0.5)) # Test the near clip boundary self.assertFalse(frustum.contains(Vector3d(0, -4.01, 0))) @@ -330,44 +330,44 @@ def test_pose_contains(self): # Compute near clip points nearTopLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) + offset, + -math.tan(30 * math.pi / 180.0) + offset, frustum.pose().pos().y() + frustum.near() + offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) + math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() - offset) nearTopLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0) - offset, + -math.tan(30 * math.pi / 180.0) - offset, frustum.pose().pos().y() + frustum.near() - offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) + math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() + offset) nearTopRight = Vector3d( - math.tan(30 * IGN_PI / 180.0) - offset, + math.tan(30 * math.pi / 180.0) - offset, frustum.pose().pos().y() + frustum.near() + offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) + math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() - offset) nearTopRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0) + offset, + math.tan(30 * math.pi / 180.0) + offset, frustum.pose().pos().y() + frustum.near() - offset, - math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) + math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() + offset) nearBottomLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) + offset, + -math.tan(30 * math.pi / 180.0) + offset, frustum.pose().pos().y() + frustum.near() + offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) + -math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() + offset) nearBottomLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0) - offset, + -math.tan(30 * math.pi / 180.0) - offset, frustum.pose().pos().y() + frustum.near() - offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) + -math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() - offset) nearBottomRight = Vector3d( - math.tan(30 * IGN_PI / 180.0) - offset, + math.tan(30 * math.pi / 180.0) - offset, frustum.pose().pos().y() + frustum.near() + offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() + offset) + -math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() + offset) nearBottomRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0) + offset, + math.tan(30 * math.pi / 180.0) + offset, frustum.pose().pos().y() + frustum.near() - offset, - -math.tan(30 * IGN_PI / 180.0) / frustum.aspect_ratio() - offset) + -math.tan(30 * math.pi / 180.0) / frustum.aspect_ratio() - offset) # Test near clip corners self.assertTrue(frustum.contains(nearTopLeft)) @@ -384,44 +384,44 @@ def test_pose_contains(self): # Compute far clip points farTopLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0) * frustum.far() + offset, + -math.tan(30 * math.pi / 180.0) * frustum.far() + offset, frustum.pose().pos().y() + frustum.far() - offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) + (math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) farTopLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, + -math.tan(30 * math.pi / 180.0)*frustum.far() - offset, frustum.pose().pos().y() + frustum.far() + offset, - (math.tan(30 * IGN_PI / 180.0 * frustum.far())) / frustum.aspect_ratio() + offset) + (math.tan(30 * math.pi / 180.0 * frustum.far())) / frustum.aspect_ratio() + offset) farTopRight = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, + math.tan(30 * math.pi / 180.0)*frustum.far() - offset, frustum.pose().pos().y() + frustum.far() - offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) + (math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) farTopRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, + math.tan(30 * math.pi / 180.0)*frustum.far() + offset, frustum.pose().pos().y() + frustum.far() + offset, - (math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) + (math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) farBottomLeft = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, + -math.tan(30 * math.pi / 180.0)*frustum.far() + offset, frustum.pose().pos().y() + frustum.far() - offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) + (-math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) farBottomLeftBad = Vector3d( - -math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, + -math.tan(30 * math.pi / 180.0)*frustum.far() - offset, frustum.pose().pos().y() + frustum.far() + offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) + (-math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) farBottomRight = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() - offset, + math.tan(30 * math.pi / 180.0)*frustum.far() - offset, frustum.pose().pos().y() + frustum.far() - offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) + (-math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() + offset) farBottomRightBad = Vector3d( - math.tan(30 * IGN_PI / 180.0)*frustum.far() + offset, + math.tan(30 * math.pi / 180.0)*frustum.far() + offset, frustum.pose().pos().y() + frustum.far() + offset, - (-math.tan(30 * IGN_PI / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) + (-math.tan(30 * math.pi / 180.0) * frustum.far()) / frustum.aspect_ratio() - offset) # Test far clip corners self.assertTrue(frustum.contains(farTopLeft)) @@ -437,7 +437,7 @@ def test_pose_contains(self): self.assertFalse(frustum.contains(farBottomRightBad)) # Adjust to 45 degrees rotation - frustum.set_pose(Pose3d(1, 1, 0, 0, 0, -IGN_PI*0.25)) + frustum.set_pose(Pose3d(1, 1, 0, 0, 0, -math.pi*0.25)) self.assertTrue(frustum.contains(Vector3d(2, -1, 0))) self.assertFalse(frustum.contains(Vector3d(0, 0, 0))) self.assertFalse(frustum.contains(Vector3d(1, 1, 0)))