diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index ed44210f9..c724e5d04 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -87,14 +87,7 @@ if (PYTHONLIBS_FOUND) if (BUILD_TESTING) # Add the Python tests set(python_tests - Box_TEST - Cylinder_TEST - Frustum_TEST - Inertial_TEST - OrientedBox_TEST - Plane_TEST python_TEST - Sphere_TEST SphericalCoordinates_TEST Vector3Stats_TEST ) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index d5629e76e..29f23dc02 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 @@ -85,20 +86,26 @@ if (${pybind11_FOUND}) set(python_tests Angle_TEST AxisAlignedBox_TEST + Box_TEST Color_TEST + Cylinder_TEST DiffDriveOdometry_TEST Filter_TEST + Frustum_TEST GaussMarkovProcess_TEST Helpers_TEST + Inertial_TEST Kmeans_TEST Line2_TEST Line3_TEST + MassMatrix3_TEST Material_TEST Matrix3_TEST Matrix4_TEST - MassMatrix3_TEST MovingWindowFilter_TEST + OrientedBox_TEST PID_TEST + Plane_TEST Pose3_TEST Quaternion_TEST Rand_TEST @@ -106,11 +113,12 @@ if (${pybind11_FOUND}) RotationSpline_TEST SemanticVersion_TEST SignalStats_TEST + Sphere_TEST Spline_TEST StopWatch_TEST Temperature_TEST - Triangle_TEST Triangle3_TEST + Triangle_TEST Vector2_TEST Vector3_TEST Vector4_TEST diff --git a/src/python_pybind11/src/Box.hh b/src/python_pybind11/src/Box.hh new file mode 100644 index 000000000..49a7c79ab --- /dev/null +++ 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 +#include + +#include +#include +#include + +#include + +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 +void defineMathBox(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::Box; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::init&>()) + .def(py::init&, + 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&> + (&Class::SetSize), + "Set the size of the box.") + .def("set_size", + py::overload_cast + (&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 &_plane) + { + std::vector> 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 &_plane) + { + std::vector> 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/Cylinder.hh b/src/python_pybind11/src/Cylinder.hh new file mode 100644 index 000000000..5c2f32ae6 --- /dev/null +++ b/src/python_pybind11/src/Cylinder.hh @@ -0,0 +1,118 @@ +/* + * 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__CYLINDER_HH_ +#define IGNITION_MATH_PYTHON__CYLINDER_HH_ + +#include + +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Cylinder +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathCylinder(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::Cylinder; + 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("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rotOffset") = ignition::math::Quaternion::Identity) + .def(py::init&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_material") = ignition::math::Material(), + py::arg("_rotOffset") = ignition::math::Quaternion::Identity) + .def(py::self == py::self) + .def("radius", + &Class::Radius, + "Get the radius in meters.") + .def("set_radius", + &Class::SetRadius, + "Set the radius in meters.") + .def("length", + &Class::Length, + "Get the length in meters.") + .def("set_length", + &Class::SetLength, + "Set the length in meters.") + .def("rotational_offset", + &Class::RotationalOffset, + "Get the rotation offset.") + .def("set_rotational_offset", + &Class::SetRotationalOffset, + "Set the rotation offset.") + .def("mat", + &Class::Mat, + py::return_value_policy::reference, + "Get the material associated with this box.") + .def("set_mat", + &Class::SetMat, + "Set the material associated with this box.") + .def("volume", + &Class::Volume, + "Get the volume of the box in m^3.") + .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("__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__CYLINDER_HH_ 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/Inertial.hh b/src/python_pybind11/src/Inertial.hh new file mode 100644 index 000000000..4fd83731f --- /dev/null +++ b/src/python_pybind11/src/Inertial.hh @@ -0,0 +1,104 @@ +/* + * 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__INERTIAL_HH_ +#define IGNITION_MATH_PYTHON__INERTIAL_HH_ + +#include + +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Inertial +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathInertial(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::Inertial; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init&, + const ignition::math::Pose3&>()) + .def(py::init()) + .def(py::self == py::self) + .def(py::self != py::self) + .def(py::self += py::self) + .def(py::self + py::self) + .def("set_mass_matrix", + &Class::SetMassMatrix, + py::arg("_m") = ignition::math::MassMatrix3(), + py::arg("_tolerance") = IGN_MASSMATRIX3_DEFAULT_TOLERANCE, + "Set the mass and inertia matrix.") + .def("mass_matrix", + &Class::MassMatrix, + py::return_value_policy::reference, + "Get the mass and inertia matrix.") + .def("set_pose", + &Class::SetPose, + "Set the pose of the center of mass reference frame.") + .def("pose", + &Class::Pose, + py::return_value_policy::reference, + "Get the pose of the center of mass reference frame.") + .def("moi", + &Class::Moi, + "Get the moment of inertia matrix computer about the body's " + "center of mass and expressed in this Inertial object’s frame F.") + .def("set_inertial_rotation", + &Class::SetInertialRotation, + "Set the inertial pose rotation without affecting the " + "MOI in the base coordinate frame.") + .def("set_mass_matrix_rotation", + &Class::SetMassMatrixRotation, + py::arg("_q") = ignition::math::Quaternion::Identity, + py::arg("_tol") = 1e-6, + py::return_value_policy::reference, + "Set the MassMatrix rotation (eigenvectors of inertia matrix) " + "without affecting the MOI in the base coordinate frame.") + .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__INERTIAL_HH_ diff --git a/src/python_pybind11/src/OrientedBox.hh b/src/python_pybind11/src/OrientedBox.hh new file mode 100644 index 000000000..efb73e72d --- /dev/null +++ b/src/python_pybind11/src/OrientedBox.hh @@ -0,0 +1,129 @@ +/* + * 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__ORIENTEDBOX_HH_ +#define IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ + +#include + +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::OrientedBox +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathOrientedBox(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::OrientedBox; + std::string pyclass_name = typestr; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init&, + const ignition::math::Pose3&>()) + .def(py::init&, + const ignition::math::Pose3&, + const ignition::math::Material&>()) + .def(py::init&>()) + .def(py::init()) + .def(py::init&, + const ignition::math::Material&>()) + .def(py::self != py::self) + .def(py::self == py::self) + .def("pose", + py::overload_cast&>(&Class::Pose), + "Set the box pose, which is the pose of its center.") + .def("pose", + py::overload_cast<>(&Class::Pose, py::const_), + py::return_value_policy::reference, + "Get the box pose, which is the pose of its center.") + .def("size", + py::overload_cast<>(&Class::Size, py::const_), + "Get the size of the OrientedBox.") + .def("size", + py::overload_cast&> + (&Class::Size), + "Set the size of the OrientedBox.") + .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("material", + &Class::Material, + "Get the material associated with this OrientedBox.") + .def("set_material", + &Class::SetMaterial, + "Set the material associated with this OrientedBox.") + .def("volume", + &Class::Volume, + "Get the volume of the OrientedBox in m^3.") + .def("contains", + &Class::Contains, + "Check if a point lies inside the box.") + .def("density_from_mass", + &Class::DensityFromMass, + "Compute the OrientedBox's density given a mass value.") + .def("set_density_from_mass", + &Class::SetDensityFromMass, + "Set the density of this OrientedBox based on a mass value.") + .def("mass_matrix", + &Class::MassMatrix, + "Get the mass matrix for this OrientedBox. This function " + "is only meaningful if the OrientedBox's size and material " + "have been set.") + .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 + +#endif // IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ diff --git a/src/python_pybind11/src/Plane.hh b/src/python_pybind11/src/Plane.hh new file mode 100644 index 000000000..857fa2065 --- /dev/null +++ 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 + +#include +#include +#include + +#include + +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 +void defineMathPlane(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Plane; + std::string pyclass_name = typestr; + py::class_ plane(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()); + plane.def(py::init<>()) + .def(py::init&, T>(), + py::arg("_normal") = ignition::math::Vector3::Zero, + py::arg("_offset") = 0.0) + .def(py::init&, + const ignition::math::Vector2&, T>()) + .def(py::init()) + .def("set", + py::overload_cast&, T> + (&Class::Set), + "Set the plane") + .def("set", + py::overload_cast&, + const ignition::math::Vector2&, T> + (&Class::Set), + "Set the plane") + .def("distance", + py::overload_cast&> + (&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&> + (&Class::Distance, py::const_), + "Get distance to the plane give an origin and direction.") + .def("intersection", + &Class::Intersection, + py::arg("_point") = ignition::math::Vector3::Zero, + py::arg("_gradient") = ignition::math::Vector3::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&> + (&Class::Side, py::const_), + "The side of the plane a point is on.") + .def("side", + py::overload_cast + (&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_(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/Sphere.hh b/src/python_pybind11/src/Sphere.hh new file mode 100644 index 000000000..4e005e80c --- /dev/null +++ b/src/python_pybind11/src/Sphere.hh @@ -0,0 +1,103 @@ +/* + * 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__SPHERE_HH_ +#define IGNITION_MATH_PYTHON__SPHERE_HH_ + +#include +#include + +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Sphere +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathSphere(py::module &m, const std::string &typestr) +{ + + using Class = ignition::math::Sphere; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::self != py::self) + .def(py::self == py::self) + .def("radius", + &Class::Radius, + "Get the radius in meters.") + .def("set_radius", + &Class::SetRadius, + "Set the radius in meters.") + .def("material", + &Class::Material, + "Get the material associated with this sphere.") + .def("set_material", + &Class::SetMaterial, + "Set the material associated with this sphere.") + .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("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("__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__SPHERE_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index b7ba194c0..61d4dd2a6 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -16,11 +16,15 @@ #include "Angle.hh" #include "AxisAlignedBox.hh" +#include "Box.hh" #include "Color.hh" +#include "Cylinder.hh" #include "DiffDriveOdometry.hh" #include "Filter.hh" +#include "Frustum.hh" #include "GaussMarkovProcess.hh" #include "Helpers.hh" +#include "Inertial.hh" #include "Kmeans.hh" #include "Line2.hh" #include "Line3.hh" @@ -29,7 +33,9 @@ #include "Matrix3.hh" #include "Matrix4.hh" #include "MovingWindowFilter.hh" +#include "OrientedBox.hh" #include "PID.hh" +#include "Plane.hh" #include "Pose3.hh" #include "Quaternion.hh" #include "Rand.hh" @@ -37,6 +43,7 @@ #include "RotationSpline.hh" #include "SemanticVersion.hh" #include "SignalStats.hh" +#include "Sphere.hh" #include "Spline.hh" #include "StopWatch.hh" #include "Temperature.hh" @@ -112,6 +119,11 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathVector3(m, "Vector3i"); ignition::math::python::defineMathVector3(m, "Vector3f"); + ignition::math::python::defineMathPlane(m, "Planed"); + + ignition::math::python::defineMathBox(m, "Boxd"); + ignition::math::python::defineMathBox(m, "Boxf"); + ignition::math::python::defineMathVector4(m, "Vector4d"); ignition::math::python::defineMathVector4(m, "Vector4i"); ignition::math::python::defineMathVector4(m, "Vector4f"); @@ -144,6 +156,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathQuaternion(m, "Quaterniond"); ignition::math::python::defineMathQuaternion(m, "Quaternionf"); + ignition::math::python::defineMathOrientedBox(m, "OrientedBoxd"); + ignition::math::python::defineMathPose3(m, "Pose3i"); ignition::math::python::defineMathPose3(m, "Pose3d"); ignition::math::python::defineMathPose3(m, "Pose3f"); @@ -151,6 +165,14 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathMassMatrix3(m, "MassMatrix3d"); ignition::math::python::defineMathMassMatrix3(m, "MassMatrix3f"); + ignition::math::python::defineMathSphere(m, "Sphered"); + + ignition::math::python::defineMathCylinder(m, "Cylinderd"); + + 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/Box_TEST.py b/src/python_pybind11/test/Box_TEST.py similarity index 91% rename from src/python/Box_TEST.py rename to src/python_pybind11/test/Box_TEST.py index 905716243..bb442c3d5 100644 --- a/src/python/Box_TEST.py +++ b/src/python_pybind11/test/Box_TEST.py @@ -46,30 +46,30 @@ def test_constructor(self): self.assertEqual(box, box2) # Dimension and mat constructor - box = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType_WOOD)) + box = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType.WOOD)) self.assertEqual(Vector3d(1.0, 2.0, 5.0), box.size()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), box.material()) + self.assertEqual(Material(ignition.math.MaterialType.WOOD), box.material()) - box2 = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType_WOOD)) + box2 = Boxd(1.0, 2.0, 5.0, Material(ignition.math.MaterialType.WOOD)) self.assertEqual(box, box2) # Vector Dimension and mat constructor - box = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType_WOOD)) + box = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType.WOOD)) self.assertEqual(Vector3d(2.2, 2.0, 10.0), box.size()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), box.material()) + self.assertEqual(Material(ignition.math.MaterialType.WOOD), box.material()) - box2 = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType_WOOD)) + box2 = Boxd(Vector3d(2.2, 2.0, 10.0), Material(ignition.math.MaterialType.WOOD)) self.assertEqual(box, box2) def test_mutators(self): box = Boxd() box.set_size(100.1, 2.3, 5.6) - box.set_material(Material(ignition.math.MaterialType_PINE)) + box.set_material(Material(ignition.math.MaterialType.PINE)) self.assertEqual(100.1, box.size().x()) self.assertEqual(2.3, box.size().y()) self.assertEqual(5.6, box.size().z()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), box.material()) + self.assertEqual(Material(ignition.math.MaterialType.PINE), box.material()) box.set_size(Vector3d(3.4, 1.2, 0.5)) self.assertEqual(3.4, box.size().x()) @@ -93,14 +93,14 @@ def test_intersections(self): # No intersections box = Boxd(2.0, 2.0, 2.0) plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertEqual(0, box.intersections(plane).size()) + self.assertEqual(0, len(box.intersections(plane))) # Plane crosses 4 edges box = Boxd(2.0, 2.0, 2.0) - plane = Planed(Vector3d(0.0, 0.0, 1.0), 0) + plane = Planed(Vector3d(0.0, 0.0, 1.0), 0.0) intersections = box.intersections(plane) - self.assertEqual(4, intersections.size()) + self.assertEqual(4, len(intersections)) self.assertEqual(intersections.count(Vector3d(-1.0, -1.0, 0.0)), 1) self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.0)), 1) self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.0)), 1) @@ -111,7 +111,7 @@ def test_intersections(self): plane = Planed(Vector3d(0.0, 0.0, 1.0), 1.0) intersections = box.intersections(plane) - self.assertEqual(4, intersections.size()) + self.assertEqual(4, len(intersections)) self.assertEqual(intersections.count(Vector3d(-1.0, -1.0, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 1.0)), 1) @@ -122,7 +122,7 @@ def test_intersections(self): plane = Planed(Vector3d(1.0, 1.0, 1.0), 1.0) intersections = box.intersections(plane) - self.assertEqual(3, intersections.size()) + self.assertEqual(3, len(intersections)) self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(1.0, 1.0, -1.0)), 1) @@ -132,7 +132,7 @@ def test_intersections(self): plane = Planed(Vector3d(1.0, 1.0, 1.0), 0.5) intersections = box.intersections(plane) - self.assertEqual(6, intersections.size()) + self.assertEqual(6, len(intersections)) self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.5)), 1) self.assertEqual(intersections.count(Vector3d(-1.0, 0.5, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.5)), 1) @@ -146,7 +146,7 @@ def test_intersections(self): plane = Planed(Vector3d(1.0, 1.0, 2.0), 0.5) intersections = box.intersections(plane) - self.assertEqual(5, intersections.size()) + self.assertEqual(5, len(intersections)) self.assertEqual(intersections.count(Vector3d(-1.0, 1.0, 0.25)), 1) self.assertEqual(intersections.count(Vector3d(-1.0, -0.5, 1.0)), 1) self.assertEqual(intersections.count(Vector3d(1.0, -1.0, 0.25)), 1) @@ -269,24 +269,25 @@ def test_vertices_below(self): # Fully above plane = Planed(Vector3d(0.0, 0.0, 1.0), -5.0) - self.assertTrue(box.vertices_below(plane).empty()) + self.assertTrue(len(box.vertices_below(plane)) == 0) # Fully below plane = Planed(Vector3d(0.0, 0.0, 1.0), 20.0) - self.assertEqual(8, box.vertices_below(plane).size()) + vertices = box.vertices_below(plane) + self.assertEqual(8, len(vertices)) # Fully below (because plane is rotated down) plane = Planed(Vector3d(0.0, 0.0, -1.0), 20.0) - self.assertEqual(8, box.vertices_below(plane).size()) + self.assertEqual(8, len(box.vertices_below(plane))) # 4 vertices plane = Planed(Vector3d(0, 0, 1.0), 0) vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) + self.assertEqual(4, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) self.assertEqual(vertices.count(nXpYnZ), 1) @@ -296,7 +297,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(0, 1, 0), 0.5) vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) + self.assertEqual(4, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) self.assertEqual(vertices.count(nXnYpZ), 1) @@ -306,7 +307,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(-1, 0, 0), -0.5) vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) + self.assertEqual(4, len(vertices)) self.assertEqual(vertices.count(pXnYnZ), 1) self.assertEqual(vertices.count(pXnYpZ), 1) @@ -316,7 +317,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(1, 1, 1), 0.0) vertices = box.vertices_below(plane) - self.assertEqual(4, vertices.size()) + self.assertEqual(4, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) self.assertEqual(vertices.count(nXnYpZ), 1) @@ -327,7 +328,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(-1, -1, 0), 0.3) vertices = box.vertices_below(plane) - self.assertEqual(6, vertices.size()) + self.assertEqual(6, len(vertices)) self.assertEqual(vertices.count(nXpYnZ), 1) self.assertEqual(vertices.count(nXpYpZ), 1) @@ -339,7 +340,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(0, 1, 1), 0.9) vertices = box.vertices_below(plane) - self.assertEqual(6, vertices.size()) + self.assertEqual(6, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) self.assertEqual(vertices.count(nXnYpZ), 1) @@ -352,7 +353,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(-1, -1, 0), -0.5) vertices = box.vertices_below(plane) - self.assertEqual(2, vertices.size()) + self.assertEqual(2, len(vertices)) self.assertEqual(vertices.count(pXpYnZ), 1) self.assertEqual(vertices.count(pXpYpZ), 1) @@ -361,7 +362,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(1, 1, 1), 1.0) vertices = box.vertices_below(plane) - self.assertEqual(7, vertices.size()) + self.assertEqual(7, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) self.assertEqual(vertices.count(nXnYpZ), 1) @@ -375,7 +376,7 @@ def test_vertices_below(self): plane = Planed(Vector3d(1, 1, 1), -1.2) vertices = box.vertices_below(plane) - self.assertEqual(1, vertices.size()) + self.assertEqual(1, len(vertices)) self.assertEqual(vertices.count(nXnYnZ), 1) diff --git a/src/python/Cylinder_TEST.py b/src/python_pybind11/test/Cylinder_TEST.py similarity index 91% rename from src/python/Cylinder_TEST.py rename to src/python_pybind11/test/Cylinder_TEST.py index d5c866342..dc6da79ed 100644 --- a/src/python/Cylinder_TEST.py +++ b/src/python_pybind11/test/Cylinder_TEST.py @@ -16,7 +16,7 @@ import unittest import ignition -from ignition.math import Cylinderd, IGN_PI, MassMatrix3d, Material, Quaterniond +from ignition.math import Cylinderd, MassMatrix3d, Material, Quaterniond class TestCylinder(unittest.TestCase): @@ -54,14 +54,14 @@ def test_constructor(self): self.assertEqual(cylinder, cylinder2) # Length, radius, mat and rot constructor - cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), + cylinder = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType.WOOD), Quaterniond(0.1, 0.2, 0.3)) self.assertEqual(1.0, cylinder.length()) self.assertEqual(2.0, cylinder.radius()) self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), cylinder.mat()) + self.assertEqual(Material(ignition.math.MaterialType.WOOD), cylinder.mat()) - cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType_WOOD), + cylinder2 = Cylinderd(1.0, 2.0, Material(ignition.math.MaterialType.WOOD), Quaterniond(0.1, 0.2, 0.3)) self.assertEqual(cylinder, cylinder2) @@ -75,17 +75,17 @@ def test_mutators(self): cylinder.set_length(100.1) cylinder.set_radius(.123) cylinder.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4)) - cylinder.set_mat(Material(ignition.math.MaterialType_PINE)) + cylinder.set_mat(Material(ignition.math.MaterialType.PINE)) self.assertEqual(100.1, cylinder.length()) self.assertEqual(.123, cylinder.radius()) self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cylinder.rotational_offset()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), cylinder.mat()) + self.assertEqual(Material(ignition.math.MaterialType.PINE), cylinder.mat()) def test_volume_and_density(self): mass = 1.0 cylinder = Cylinderd(1.0, 0.001) - expectedVolume = (IGN_PI * math.pow(0.001, 2) * 1.0) + expectedVolume = (math.pi * math.pow(0.001, 2) * 1.0) self.assertEqual(expectedVolume, cylinder.volume()) expectedDensity = mass / expectedVolume 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))) diff --git a/src/python/Inertial_TEST.py b/src/python_pybind11/test/Inertial_TEST.py similarity index 92% rename from src/python/Inertial_TEST.py rename to src/python_pybind11/test/Inertial_TEST.py index 289a556d8..a4a759b10 100644 --- a/src/python/Inertial_TEST.py +++ b/src/python_pybind11/test/Inertial_TEST.py @@ -16,7 +16,7 @@ import math import unittest -from ignition.math import IGN_PI, IGN_PI_2, IGN_PI_4, Inertiald, Quaterniond, Pose3d, Matrix3d, MassMatrix3d, Vector3d +from ignition.math import Inertiald, Quaterniond, Pose3d, Matrix3d, MassMatrix3d, Vector3d class TestInertial(unittest.TestCase): @@ -45,7 +45,7 @@ def test_constructor_non_default_values(self): m = MassMatrix3d(mass, Ixxyyzz, Ixyxzyz) self.assertTrue(m.is_positive()) self.assertTrue(m.is_valid()) - pose = Pose3d(1, 2, 3, IGN_PI/6, 0, 0) + pose = Pose3d(1, 2, 3, math.pi/6, 0, 0) inertial = Inertiald(m, pose) # Should not match simple constructor @@ -83,7 +83,7 @@ def test_setters(self): m = MassMatrix3d(mass, Ixxyyzz, Ixyxzyz) self.assertTrue(m.is_positive()) self.assertTrue(m.is_valid()) - pose = Pose3d(1, 2, 3, IGN_PI/6, 0, 0) + pose = Pose3d(1, 2, 3, math.pi/6, 0, 0) inertial = Inertiald() # Initially valid @@ -114,28 +114,28 @@ def test_moi_diagonal(self): self.assertEqual(inertial.moi(), m.moi()) # 90 deg rotation about X axis, expect different MOI - pose = Pose3d(0, 0, 0, IGN_PI_2, 0, 0) + pose = Pose3d(0, 0, 0, math.pi/2, 0, 0) expectedMOI = Matrix3d(2, 0, 0, 0, 4, 0, 0, 0, 3) inertial = Inertiald(m, pose) self.assertNotEqual(inertial.moi(), m.moi()) self.assertEqual(inertial.moi(), expectedMOI) # 90 deg rotation about Y axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, IGN_PI_2, 0) + pose = Pose3d(0, 0, 0, 0, math.pi/2, 0) expectedMOI = Matrix3d(4, 0, 0, 0, 3, 0, 0, 0, 2) inertial = Inertiald(m, pose) self.assertNotEqual(inertial.moi(), m.moi()) self.assertEqual(inertial.moi(), expectedMOI) # 90 deg rotation about Z axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, 0, IGN_PI_2) + pose = Pose3d(0, 0, 0, 0, 0, math.pi/2) expectedMOI = Matrix3d(3, 0, 0, 0, 2, 0, 0, 0, 4) inertial = Inertiald(m, pose) self.assertNotEqual(inertial.moi(), m.moi()) self.assertEqual(inertial.moi(), expectedMOI) # 45 deg rotation about Z axis, expect different MOI - pose = Pose3d(0, 0, 0, 0, 0, IGN_PI_4) + pose = Pose3d(0, 0, 0, 0, 0, math.pi/4) expectedMOI = Matrix3d(2.5, -0.5, 0, -0.5, 2.5, 0, 0, 0, 4) inertial = Inertiald(m, pose) self.assertNotEqual(inertial.moi(), m.moi()) @@ -161,18 +161,18 @@ def SetRotation(self, _mass, _ixxyyzz, _ixyxzyz, _unique=True): rotations = [ Quaterniond.IDENTITY, - Quaterniond(IGN_PI, 0, 0), - Quaterniond(0, IGN_PI, 0), - Quaterniond(0, 0, IGN_PI), - Quaterniond(IGN_PI_2, 0, 0), - Quaterniond(0, IGN_PI_2, 0), - Quaterniond(0, 0, IGN_PI_2), - Quaterniond(IGN_PI_4, 0, 0), - Quaterniond(0, IGN_PI_4, 0), - Quaterniond(0, 0, IGN_PI_4), - Quaterniond(IGN_PI/6, 0, 0), - Quaterniond(0, IGN_PI/6, 0), - Quaterniond(0, 0, IGN_PI/6), + Quaterniond(math.pi, 0, 0), + Quaterniond(0, math.pi, 0), + Quaterniond(0, 0, math.pi), + Quaterniond(math.pi/2, 0, 0), + Quaterniond(0, math.pi/2, 0), + Quaterniond(0, 0, math.pi/2), + Quaterniond(math.pi/4, 0, 0), + Quaterniond(0, math.pi/4, 0), + Quaterniond(0, 0, math.pi/4), + Quaterniond(math.pi/6, 0, 0), + Quaterniond(0, math.pi/6, 0), + Quaterniond(0, 0, math.pi/6), Quaterniond(0.1, 0.2, 0.3), Quaterniond(-0.1, 0.2, -0.3), Quaterniond(0.4, 0.2, 0.5), @@ -323,12 +323,12 @@ def test_addition(self): size = Vector3d(1, 1, 1) cubeMM3 = MassMatrix3d() self.assertTrue(cubeMM3.set_from_box(mass, size)) - cube = Inertiald(cubeMM3, Pose3d(0, 0, 0, IGN_PI_4, 0, 0)) + cube = Inertiald(cubeMM3, Pose3d(0, 0, 0, math.pi/4, 0, 0)) half = MassMatrix3d() self.assertTrue(half.set_from_box(0.5*mass, Vector3d(0.5, 1, 1))) - left = Inertiald(half, Pose3d(-0.25, 0, 0, IGN_PI_4, 0, 0)) - right = Inertiald(half, Pose3d(0.25, 0, 0, IGN_PI_4, 0, 0)) + left = Inertiald(half, Pose3d(-0.25, 0, 0, math.pi/4, 0, 0)) + right = Inertiald(half, Pose3d(0.25, 0, 0, math.pi/4, 0, 0)) # objects won't match exactly # since inertia matrices will all be in base frame @@ -367,12 +367,12 @@ def test_addition(self): self.assertTrue(cubeMM3.set_from_box(mass, size)) addedCube = Inertiald( Inertiald(cubeMM3, Pose3d(-0.5, -0.5, -0.5, 0, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, IGN_PI_2, 0, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, IGN_PI_2, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, IGN_PI_2)) + - Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, IGN_PI, 0, 0)) + - Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, IGN_PI, 0)) + - Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, IGN_PI)) + + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, -0.5, math.pi/2, 0, 0)) + + Inertiald(cubeMM3, Pose3d(0.5, -0.5, -0.5, 0, math.pi/2, 0)) + + Inertiald(cubeMM3, Pose3d(0.5, 0.5, -0.5, 0, 0, math.pi/2)) + + Inertiald(cubeMM3, Pose3d(-0.5, -0.5, 0.5, math.pi, 0, 0)) + + Inertiald(cubeMM3, Pose3d(-0.5, 0.5, 0.5, 0, math.pi, 0)) + + Inertiald(cubeMM3, Pose3d(0.5, -0.5, 0.5, 0, 0, math.pi)) + Inertiald(cubeMM3, Pose3d(0.5, 0.5, 0.5, 0, 0, 0))) trueCubeMM3 = MassMatrix3d() diff --git a/src/python/OrientedBox_TEST.py b/src/python_pybind11/test/OrientedBox_TEST.py similarity index 98% rename from src/python/OrientedBox_TEST.py rename to src/python_pybind11/test/OrientedBox_TEST.py index 5eb671383..d750bd4ed 100644 --- a/src/python/OrientedBox_TEST.py +++ b/src/python_pybind11/test/OrientedBox_TEST.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import unittest -from ignition.math import IGN_PI, OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d +from ignition.math import OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d g_tolerance = 1e-6 @@ -205,7 +206,7 @@ def test_contains_oriented_position(self): def test_contains_oriented_rotation(self): # Rotate PI/2 about +x: swap Z and Y - box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, IGN_PI*0.5, 0, 0)) + box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, math.pi*0.5, 0, 0)) # Doesn't contain non-rotated vertices self.assertFalse(box.contains(Vector3d(-0.5, -1.0, -1.5))) diff --git a/src/python/Plane_TEST.py b/src/python_pybind11/test/Plane_TEST.py similarity index 88% rename from src/python/Plane_TEST.py rename to src/python_pybind11/test/Plane_TEST.py index c1e1a6d25..b038c7fd5 100644 --- a/src/python/Plane_TEST.py +++ b/src/python_pybind11/test/Plane_TEST.py @@ -14,7 +14,7 @@ import unittest -from ignition.math import AxisAlignedBox, Planed, Vector2d, Vector3d +from ignition.math import AxisAlignedBox, Planed, PlaneSide, Vector2d, Vector3d class TestPlane(unittest.TestCase): @@ -76,46 +76,46 @@ def test_side_point(self): # On the negative side of the plane (below the plane) point = Vector3d(0, 0, 0) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + self.assertEqual(plane.side(point), PlaneSide.NEGATIVE_SIDE) # Still on the negative side of the plane (below the plane) point.set(1, 1, 0) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + self.assertEqual(plane.side(point), PlaneSide.NEGATIVE_SIDE) # Above the plane (positive side) point.set(1, 1, 2) - self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) + self.assertEqual(plane.side(point), PlaneSide.POSITIVE_SIDE) # On the plane point.set(0, 0, 1) - self.assertEqual(plane.side(point), Planed.NO_SIDE) + self.assertEqual(plane.side(point), PlaneSide.NO_SIDE) # Change the plane, but the point is still on the negative side plane.set(Vector3d(1, 0, 0), 4) - self.assertEqual(plane.side(point), Planed.NEGATIVE_SIDE) + self.assertEqual(plane.side(point), PlaneSide.NEGATIVE_SIDE) # Point is now on the positive side point.set(4.1, 0, 1) - self.assertEqual(plane.side(point), Planed.POSITIVE_SIDE) + self.assertEqual(plane.side(point), PlaneSide.POSITIVE_SIDE) def test_side__axis_aligned_box(self): plane = Planed(Vector3d(0, 0, 1), 1) # On the negative side of the plane (below the plane) box = AxisAlignedBox(Vector3d(-.5, -.5, -.5), Vector3d(.5, .5, .5)) - self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) + self.assertEqual(plane.side(box), PlaneSide.NEGATIVE_SIDE) # Still on the negative side of the plane (below the plane) box = AxisAlignedBox(Vector3d(-10, -10, -10), Vector3d(.9, .9, .9)) - self.assertEqual(plane.side(box), Planed.NEGATIVE_SIDE) + self.assertEqual(plane.side(box), PlaneSide.NEGATIVE_SIDE) # Above the plane (positive side) box = AxisAlignedBox(Vector3d(2, 2, 2), Vector3d(3, 3, 3)) - self.assertEqual(plane.side(box), Planed.POSITIVE_SIDE) + self.assertEqual(plane.side(box), PlaneSide.POSITIVE_SIDE) # On both sides the plane box = AxisAlignedBox(Vector3d(0, 0, 0), Vector3d(3, 3, 3)) - self.assertEqual(plane.side(box), Planed.BOTH_SIDE) + self.assertEqual(plane.side(box), PlaneSide.BOTH_SIDE) def test_intersection(self): plane = Planed(Vector3d(0.5, 0, 1), 1) diff --git a/src/python/Sphere_TEST.py b/src/python_pybind11/test/Sphere_TEST.py similarity index 92% rename from src/python/Sphere_TEST.py rename to src/python_pybind11/test/Sphere_TEST.py index 7c44d5de2..14c5df186 100644 --- a/src/python/Sphere_TEST.py +++ b/src/python_pybind11/test/Sphere_TEST.py @@ -16,7 +16,7 @@ import unittest import ignition -from ignition.math import IGN_PI, MassMatrix3d, Material, Planed, Sphered, Vector2d, Vector3d +from ignition.math import MassMatrix3d, Material, Planed, Sphered, Vector2d, Vector3d class TestSphere(unittest.TestCase): @@ -39,28 +39,28 @@ def test_constructor(self): self.assertEqual(sphere, sphere2) # Radius and mat - sphere = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) + sphere = Sphered(1.0, Material(ignition.math.MaterialType.WOOD)) self.assertEqual(1.0, sphere.radius()) - self.assertEqual(Material(ignition.math.MaterialType_WOOD), sphere.material()) + self.assertEqual(Material(ignition.math.MaterialType.WOOD), sphere.material()) - sphere2 = Sphered(1.0, Material(ignition.math.MaterialType_WOOD)) + sphere2 = Sphered(1.0, Material(ignition.math.MaterialType.WOOD)) self.assertEqual(sphere, sphere2) def test_comparison(self): - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + wood = Sphered(0.1, Material(ignition.math.MaterialType.WOOD)) modified = wood self.assertEqual(wood, modified) modified.set_radius(1.0) - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + wood = Sphered(0.1, Material(ignition.math.MaterialType.WOOD)) self.assertNotEqual(wood, modified) modified = wood - wood = Sphered(0.1, Material(ignition.math.MaterialType_WOOD)) + wood = Sphered(0.1, Material(ignition.math.MaterialType.WOOD)) self.assertEqual(wood, modified) - modified.set_material(Material(ignition.math.MaterialType_PINE)) + modified.set_material(Material(ignition.math.MaterialType.PINE)) self.assertNotEqual(wood, modified) def test_mutators(self): @@ -69,15 +69,15 @@ def test_mutators(self): self.assertEqual(Material(), sphere.material()) sphere.set_radius(.123) - sphere.set_material(Material(ignition.math.MaterialType_PINE)) + sphere.set_material(Material(ignition.math.MaterialType.PINE)) self.assertEqual(.123, sphere.radius()) - self.assertEqual(Material(ignition.math.MaterialType_PINE), sphere.material()) + self.assertEqual(Material(ignition.math.MaterialType.PINE), sphere.material()) def test_volume_and_density(self): mass = 1.0 sphere = Sphered(0.001) - expectedVolume = (4.0/3.0) * IGN_PI * math.pow(0.001, 3) + expectedVolume = (4.0/3.0) * math.pi * math.pow(0.001, 3) self.assertEqual(expectedVolume, sphere.volume()) expectedDensity = mass / expectedVolume