Add Python interface to Triangle3 (#247)

Signed-off-by: Alejandro Hernández <[email protected]>

include/ignition/math/Triangle3.hh

@@ -130,11 +130,12 @@ namespace ignition
       public: bool Contains(const Vector3<T> &_pt) const
       {
         // Make sure the point is on the same plane as the triangle
-        if (Planed(this->Normal()).Side(_pt) == Planed::NO_SIDE)
+        if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2]))
+          == Planed::NO_SIDE)
         {
-          Vector3d v0 = this->pts[2] - this->pts[0];
-          Vector3d v1 = this->pts[1] - this->pts[0];
-          Vector3d v2 = _pt - this->pts[0];
+          Vector3<T> v0 = this->pts[2] - this->pts[0];
+          Vector3<T> v1 = this->pts[1] - this->pts[0];
+          Vector3<T> v2 = _pt - this->pts[0];
 
           double dot00 = v0.Dot(v0);
           double dot01 = v0.Dot(v1);
@@ -150,17 +151,17 @@ namespace ignition
           // Check if point is in triangle
           return (u >= 0) && (v >= 0) && (u + v <= 1);
         }
-        else
-        {
-          return false;
-        }
+        return false;
       }
 
       /// \brief Get the triangle's normal vector.
       /// \return The normal vector for the triangle.
       public: Vector3d Normal() const
       {
-         return Vector3d::Normal(this->pts[0], this->pts[1], this->pts[2]);
+        return math::Vector3d::Normal(
+          Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]),
+          Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]),
+          Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2]));
       }
 
       /// \brief Get whether the given line intersects an edge of this triangle.
@@ -179,22 +180,24 @@ namespace ignition
       /// \param[out] _ipt1 Return value of the first intersection point,
       /// only valid if the return value of the function is true.
       /// \return True if the given line intersects this triangle.
-      public: bool Intersects(const Line3<T> &_line, Vector3<T> &_ipt1) const
+      public: bool Intersects(
+        const Line3<T> &_line, Vector3<T> &_ipt1) const
       {
         // Triangle normal
         Vector3d norm = this->Normal();
 
         // Ray direction to intersect with triangle
-        Vector3d dir = (_line[1] - _line[0]).Normalize();
+        Vector3<T> dir = (_line[1] - _line[0]).Normalize();
 
-        double denom = norm.Dot(dir);
+        double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2]));
 
         // Handle the case when the line is not co-planar with the triangle
         if (!math::equal(denom, 0.0))
         {
           // Distance from line start to triangle intersection
+          Vector3<T> diff = _line[0] - this->pts[0];
           double intersection =
-            -norm.Dot(_line[0] - this->pts[0]) / denom;
+            -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom;
 
           // Make sure the ray intersects the triangle
           if (intersection < 1.0 || intersection > _line.Length())

src/python/CMakeLists.txt

@@ -110,6 +110,7 @@ if (PYTHONLIBS_FOUND)
       Spline_TEST
       Temperature_TEST
       Triangle_TEST
+      Triangle3_TEST
       Vector2_TEST
       Vector3_TEST
       Vector3Stats_TEST

src/python/Triangle3.i

@@ -0,0 +1,67 @@
+/*
+ * 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.
+ *
+*/
+
+%module triangle3
+%{
+#include <ignition/math/config.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Line3.hh>
+#include <set>
+#include <ignition/math/Triangle3.hh>
+#include <ignition/math/Vector3.hh>
+%}
+
+namespace ignition
+{
+namespace math
+{
+    template<typename T>
+    class Triangle3
+    {
+      %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) "";
+      public: Triangle3() = default;
+      public: Triangle3(const ignition::math::Vector3<T> &_pt1,
+                        const ignition::math::Vector3<T> &_pt2,
+                        const ignition::math::Vector3<T> &_pt3);
+      public: void Set(const unsigned int _index, const math::Vector3<T> &_pt);
+      public: void Set(const ignition::math::Vector3<T> &_pt1,
+                       const ignition::math::Vector3<T> &_pt2,
+                       const ignition::math::Vector3<T> &_pt3);
+      public: bool Valid() const;
+      public: Line3<T> Side(const unsigned int _index) const;
+      public: bool Contains(const Line3<T> &_line) const;
+      public: bool Contains(const ignition::math::Vector3<T> &_pt) const;
+      public: ignition::math::Vector3d Normal() const;
+      public: bool Intersects(const Line3<T> &_line,
+                              ignition::math::Vector3<T> &_ipt1) const;
+      public: T Perimeter() const;
+      public: double Area() const;
+    };
+
+    %extend Triangle3
+    {
+      ignition::math::Vector3<T> __getitem__(const unsigned int i) const
+      {
+        return (*$self)[i];
+      }
+    }
+
+    %template(Triangle3i) Triangle3<int>;
+    %template(Triangle3d) Triangle3<double>;
+    %template(Triangle3f) Triangle3<float>;
+}
+}

src/python/Triangle3_TEST.py

@@ -0,0 +1,186 @@
+# 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.
+
+import math
+import unittest
+from ignition.math import Line3d
+from ignition.math import Triangle3d
+from ignition.math import Vector3d
+
+
+class TestTriangle(unittest.TestCase):
+
+    def test_constructor(self):
+        # Constructor
+        tri = Triangle3d()
+        self.assertAlmostEqual(tri[0], Vector3d(0, 0, 0))
+        self.assertAlmostEqual(tri[1], Vector3d(0, 0, 0))
+        self.assertAlmostEqual(tri[2], Vector3d(0, 0, 0))
+
+        # Construct from three points
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertTrue(tri.valid())
+
+        self.assertAlmostEqual(tri[0], Vector3d(0, 0, 0))
+        self.assertAlmostEqual(tri[1], Vector3d(0, 1, 0))
+        self.assertAlmostEqual(tri[2], Vector3d(1, 0, 0))
+        self.assertAlmostEqual(tri[3], tri[2])
+
+        # Construct degenerate from 3 collinear points
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(0, 2, 0))
+
+        # Expect not valid
+        self.assertFalse(tri.valid())
+
+    def test_set(self):
+        tri = Triangle3d()
+
+        tri.set(0, Vector3d(3, 4, 1))
+        tri.set(1, Vector3d(5, 6, 2))
+        tri.set(2, Vector3d(7, 8, -3))
+        self.assertAlmostEqual(tri[0], Vector3d(3, 4, 1))
+        self.assertAlmostEqual(tri[1], Vector3d(5, 6, 2))
+        self.assertAlmostEqual(tri[2], Vector3d(7, 8, -3))
+
+        tri.set(Vector3d(0.1, 0.2, -0.3),
+                Vector3d(0.3, 0.4, 0.5),
+                Vector3d(1.5, 2.6, 3.7))
+        self.assertAlmostEqual(tri[0], Vector3d(0.1, 0.2, -0.3))
+        self.assertAlmostEqual(tri[1], Vector3d(0.3, 0.4, 0.5))
+        self.assertAlmostEqual(tri[2], Vector3d(1.5, 2.6, 3.7))
+
+    def test_side(self):
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 1),
+                         Vector3d(1, 0, 2))
+
+        self.assertTrue(tri.side(0) == Line3d(0, 0, 0, 0, 1, 1))
+        self.assertTrue(tri.side(1) == Line3d(0, 1, 1, 1, 0, 2))
+        self.assertTrue(tri.side(2) == Line3d(1, 0, 2, 0, 0, 0))
+
+    def test_contains_line(self):
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertTrue(tri.contains(tri.side(0)))
+        self.assertTrue(tri.contains(tri.side(1)))
+        self.assertTrue(tri.contains(tri.side(2)))
+
+        self.assertTrue(tri.contains(Line3d(0.1, 0.1, 0.0, 0.5, 0.5, 0.0)))
+
+        self.assertFalse(tri.contains(Line3d(0.1, 0.1, 0.0, 0.6, 0.6, 0.0)))
+        self.assertFalse(tri.contains(Line3d(-0.1, -0.1, 0.0, 0.5, 0.5, 0.0)))
+        self.assertFalse(tri.contains(Line3d(0.1, 0.1, 0.1, 0.5, 0.5, 0.1)))
+        self.assertFalse(tri.contains(Line3d(0.1, 0.1, -0.1, 0.1, 0.1, 0.1)))
+
+    def test_intersects(self):
+        pt1 = Vector3d()
+        pt2 = Vector3d()
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertTrue(tri.intersects(tri.side(0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0, 0, 0))
+
+        self.assertTrue(tri.intersects(tri.side(1), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0, 1, 0))
+
+        self.assertTrue(tri.intersects(tri.side(2), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(1, 0, 0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, 0.0, 0.5, 0.5, 0.0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.1, 0.1, 0.0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, 0.0, 0.6, 0.6, 0.0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.5, 0.5, 0.0))
+
+        self.assertTrue(tri.intersects(Line3d(0.6, 0.6, 0, 0.1, 0.1, 0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.5, 0.5, 0.0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, 0, -0.6, 0.1, 0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.0, 0.1, 0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, 0, 0.1, -0.1, 0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.1, 0.0, 0))
+
+        self.assertTrue(tri.intersects(Line3d(-0.1, -0.1, 0, 0.5, 0.5, 0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.0, 0.0, 0))
+
+        self.assertTrue(tri.intersects(Line3d(-2, -2, 0, 0.2, 0.2, 0), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.0, 0.0, 0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, -1, 0.1, 0.1, 1), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.1, 0.1, 0))
+
+        self.assertTrue(tri.intersects(Line3d(0.1, 0.1, -1, 0.3, 0.3, 1), pt1))
+        self.assertAlmostEqual(pt1, Vector3d(0.2, 0.2, 0))
+
+        self.assertFalse(tri.intersects(Line3d(-0.1, 0, 0, -0.1, 1, 0), pt1))
+
+    def test_contains_pt(self):
+        tri = Triangle3d(Vector3d(0, 0, 0.0),
+                         Vector3d(0, 1, 0.0),
+                         Vector3d(1, 0, 0.0))
+
+        self.assertTrue(tri.contains(tri[0]))
+        self.assertTrue(tri.contains(tri[1]))
+        self.assertTrue(tri.contains(tri[2]))
+
+        self.assertTrue(tri.contains(Vector3d(0.1, 0.1, 0.0)))
+        self.assertTrue(tri.contains(Vector3d(0.0, 0.5, 0.0)))
+        self.assertTrue(tri.contains(Vector3d(0.5, 0.0, 0.0)))
+        self.assertTrue(tri.contains(Vector3d(0.5, 0.5, 0.0)))
+
+        self.assertFalse(tri.contains(Vector3d(-0.01, -0.01, 0.0)))
+        self.assertFalse(tri.contains(Vector3d(1.01, 0.0, 0.0)))
+        self.assertFalse(tri.contains(Vector3d(0, 1.01, 0.0)))
+        self.assertFalse(tri.contains(Vector3d(0.1, 0.1, 0.1)))
+        self.assertFalse(tri.contains(Vector3d(0.1, 0.1, -0.1)))
+
+    def test_perimeter(self):
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertAlmostEqual(tri.perimeter(), 2.0 + math.sqrt(2.0))
+
+        tri = Triangle3d(Vector3d(0, 0, 1),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertAlmostEqual(tri.perimeter(), 3.0 * math.sqrt(2.0))
+
+    def test_area(self):
+        tri = Triangle3d(Vector3d(0, 0, 0),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertAlmostEqual(tri.area(), 0.5, delta=1e-6)
+
+        tri = Triangle3d(Vector3d(0, 0, 1),
+                         Vector3d(0, 1, 0),
+                         Vector3d(1, 0, 0))
+
+        self.assertAlmostEqual(tri.area(), (math.sqrt(2.0) * math.sqrt(1.5))*0.5, delta=1e-6)
+
+
+if __name__ == '__main__':
+    unittest.main()

src/python/python.i

@@ -24,5 +24,6 @@
 %include MaterialType.i
 %include Material.i
 %include Triangle.i
+%include Triangle3.i
 %include Kmeans.i
 %include Vector3Stats.i