Add support for fcl::Ellipsoid/Halfspace/TriangleP

Copied 4b851d6 from kolinguo/MPlib

pybind/collision_detection/fcl/pybind_fcl.cpp

@@ -27,8 +27,11 @@ using Capsule = fcl::Capsule<S>;
 using Cone = fcl::Cone<S>;
 using Convex = fcl::Convex<S>;
 using Cylinder = fcl::Cylinder<S>;
+using Ellipsoid = fcl::Ellipsoid<S>;
+using Halfspace = fcl::Halfspace<S>;
 using Plane = fcl::Plane<S>;
 using Sphere = fcl::Sphere<S>;
+using TriangleP = fcl::TriangleP<S>;
 using BVHModel_OBBRSS = fcl::BVHModel<fcl::OBBRSS<S>>;
 using OcTree = fcl::OcTree<S>;
 
@@ -129,6 +132,29 @@ void build_pyfcl(py::module &m) {
       .def_readwrite("radius", &Cylinder::radius)
       .def_readwrite("lz", &Cylinder::lz);
 
+  auto PyEllipsoid = py::class_<Ellipsoid, std::shared_ptr<Ellipsoid>>(
+      m, "Ellipsoid", PyCollisionGeometry, DOC(fcl, Ellipsoid));
+  PyEllipsoid
+      .def(py::init<S, S, S>(), py::arg("a"), py::arg("b"), py::arg("c"),
+           DOC(fcl, Ellipsoid, Ellipsoid))
+      .def(py::init<const Vector3<S> &>(), py::arg("radii"),
+           DOC(fcl, Ellipsoid, Ellipsoid, 2))
+      .def_readwrite("radii", &Ellipsoid::radii);
+
+  auto PyHalfspace = py::class_<Halfspace, std::shared_ptr<Halfspace>>(
+      m, "Halfspace", PyCollisionGeometry, DOC(fcl, Halfspace));
+  PyHalfspace
+      .def(py::init<const Vector3<S> &, S>(), py::arg("n"), py::arg("d"),
+           DOC(fcl, Halfspace, Halfspace))
+      .def(py::init<S, S, S, S>(), py::arg("a"), py::arg("b"), py::arg("c"),
+           py::arg("d"), DOC(fcl, Halfspace, Halfspace, 2))
+      .def_readwrite("n", &Halfspace::n)
+      .def_readwrite("d", &Halfspace::d)
+      .def("signed_distance", &Halfspace::signedDistance, py::arg("p"),
+           DOC(fcl, Halfspace, signedDistance))
+      .def("distance", &Halfspace::distance, py::arg("p"),
+           DOC(fcl, Halfspace, distance));
+
   auto PyPlane = py::class_<Plane, std::shared_ptr<Plane>>(
       m, "Plane", PyCollisionGeometry, DOC(fcl, Plane));
   PyPlane
@@ -137,13 +163,25 @@ void build_pyfcl(py::module &m) {
       .def(py::init<S, S, S, S>(), py::arg("a"), py::arg("b"), py::arg("c"),
            py::arg("d"), DOC(fcl, Plane, Plane, 2))
       .def_readwrite("n", &Plane::n)
-      .def_readwrite("d", &Plane::d);
+      .def_readwrite("d", &Plane::d)
+      .def("signed_distance", &Plane::signedDistance, py::arg("p"),
+           DOC(fcl, Plane, signedDistance))
+      .def("distance", &Plane::distance, py::arg("p"), DOC(fcl, Plane, distance));
 
   auto PySphere = py::class_<Sphere, std::shared_ptr<Sphere>>(
       m, "Sphere", PyCollisionGeometry, DOC(fcl, Sphere));
   PySphere.def(py::init<S>(), py::arg("radius"), DOC(fcl, Sphere, Sphere))
       .def_readwrite("radius", &Sphere::radius);
 
+  auto PyTriangleP = py::class_<TriangleP, std::shared_ptr<TriangleP>>(
+      m, "TriangleP", PyCollisionGeometry, DOC(fcl, TriangleP));
+  PyTriangleP
+      .def(py::init<const Vector3<S> &, const Vector3<S> &, const Vector3<S> &>(),
+           py::arg("a"), py::arg("b"), py::arg("c"), DOC(fcl, TriangleP, TriangleP))
+      .def_readwrite("a", &TriangleP::a)
+      .def_readwrite("b", &TriangleP::b)
+      .def_readwrite("c", &TriangleP::c);
+
   auto PyBVHModel_OBBRSS =
       py::class_<BVHModel_OBBRSS, std::shared_ptr<BVHModel_OBBRSS>>(
           m, "BVHModel", PyCollisionGeometry, DOC(fcl, BVHModel_OBBRSS));

pybind/docstring/collision_detection/fcl/fcl.h

@@ -157,6 +157,62 @@ Construct a cylinder with given radius and height.
 :param radius: radius of the cylinder
 :param lz: height of the cylinder along z axis)doc";
 
+// ----- fcl::Ellipsoid ----- //
+static const char *__doc_fcl_Ellipsoid =
+R"doc(Ellipsoid collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a ellipsoid
+geometry.)doc";
+
+static const char *__doc_fcl_Ellipsoid_Ellipsoid =
+R"doc(
+Construct a ellipsoid with given parameters.
+
+:param a: length of the ``x`` semi-axis
+:param b: length of the ``y`` semi-axis
+:param c: length of the ``z`` semi-axis)doc";
+
+static const char *__doc_fcl_Ellipsoid_Ellipsoid_2 =
+R"doc(
+Construct a ellipsoid with given parameters.
+
+:param radii: vector of the length of the ``x``, ``y``, and ``z`` semi-axes)doc";
+
+// ----- fcl::Halfspace ----- //
+static const char *__doc_fcl_Halfspace =
+R"doc(Infinite halfspace collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a halfspace geometry.)doc";
+
+static const char *__doc_fcl_Halfspace_Halfspace =
+R"doc(
+Construct a halfspace with given normal direction and offset where ``n * p = d``.
+Points in the negative side of the separation plane ``{p | n * p < d}`` are inside
+the half space (will have collision).
+
+:param n: normal direction of the halfspace
+:param d: offset of the halfspace)doc";
+
+static const char *__doc_fcl_Halfspace_Halfspace_2 =
+R"doc(
+Construct a halfspace with given halfspace parameters where ``ax + by + cz = d``.
+Points in the negative side of the separation plane ``{(x, y, z) | ax + by + cz < d}``
+are inside the half space (will have collision).)doc";
+
+static const char *__doc_fcl_Halfspace_signedDistance =
+R"doc(
+Compute the signed distance of a point to the halfspace as ``n * p - d``.
+
+:param p: a point in 3D space
+:return: signed distance of the point to the halfspace)doc";
+
+static const char *__doc_fcl_Halfspace_distance =
+R"doc(
+Compute the distance of a point to the halfspace as ``abs(n * p - d)``.
+
+:param p: a point in 3D space
+:return: distance of the point to the halfspace)doc";
+
 // ----- fcl::Plane ----- //
 static const char *__doc_fcl_Plane =
 R"doc(Infinite plane collision geometry.
@@ -165,7 +221,7 @@ Inheriting from CollisionGeometry, this class specializes to a plane geometry.)d
 
 static const char *__doc_fcl_Plane_Plane =
 R"doc(
-Construct a plane with given normal direction and offset where ``n * v = d``.
+Construct a plane with given normal direction and offset where ``n * p = d``.
 
 :param n: normal direction of the plane
 :param d: offset of the plane)doc";
@@ -174,6 +230,20 @@ static const char *__doc_fcl_Plane_Plane_2 =
 R"doc(
 Construct a plane with given plane parameters where ``ax + by + cz = d``.)doc";
 
+static const char *__doc_fcl_Plane_signedDistance =
+R"doc(
+Compute the signed distance of a point to the plane as ``n * p - d``.
+
+:param p: a point in 3D space
+:return: signed distance of the point to the plane)doc";
+
+static const char *__doc_fcl_Plane_distance =
+R"doc(
+Compute the distance of a point to the plane as ``abs(n * p - d)``.
+
+:param p: a point in 3D space
+:return: distance of the point to the plane)doc";
+
 // ----- fcl::Sphere ----- //
 static const char *__doc_fcl_Sphere =
 R"doc(Sphere collision geometry.
@@ -186,6 +256,20 @@ Construct a sphere with given radius.
 
 :param radius: radius of the sphere)doc";
 
+// ----- fcl::TriangleP ----- //
+static const char *__doc_fcl_TriangleP =
+R"doc(TriangleP collision geometry.
+
+Inheriting from CollisionGeometry, this class specializes to a triangleP geometry.)doc";
+
+static const char *__doc_fcl_TriangleP_TriangleP =
+R"doc(
+Construct a set of triangles from vectors of point coordinates.
+
+:param a: vector of point ``x`` coordinates
+:param b: vector of point ``y`` coordinates
+:param c: vector of point ``z`` coordinates)doc";
+
 // ----- fcl::BVHModel<fcl::OBBRSS> ----- //
 static const char *__doc_fcl_BVHModel_OBBRSS =
 R"doc(BVHModel collision geometry.