added handling of vertex duplicates for fan triangulation

rtron-math/src/main/kotlin/io/rtron/math/processing/triangulation/FanTriangulationAlgorithm.kt

@@ -17,6 +17,8 @@
 package io.rtron.math.processing.triangulation
 
 import arrow.core.Either
+import arrow.core.NonEmptyList
+import arrow.core.left
 import arrow.core.nonEmptyListOf
 import arrow.core.right
 import io.rtron.math.geometry.euclidean.threed.point.Vector3D
@@ -29,10 +31,14 @@ import io.rtron.math.geometry.euclidean.threed.surface.Polygon3D
  */
 class FanTriangulationAlgorithm : TriangulationAlgorithm() {
 
-    override fun triangulate(vertices: List<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> {
-        val polygons = vertices.filterIndexed { index, _ -> index != 0 }
+    override fun triangulate(vertices: NonEmptyList<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> {
+        if (vertices.tail.any { vertices.head.fuzzyEquals(it, tolerance) }) {
+            return TriangulatorException.FirstVertexDuplicated().left()
+        }
+
+        val polygons = vertices.tail
             .zipWithNext()
-            .map { Polygon3D(nonEmptyListOf(vertices.first(), it.first, it.second), tolerance) }
+            .map { Polygon3D(nonEmptyListOf(vertices.head, it.first, it.second), tolerance) }
 
         return polygons.right()
     }

rtron-math/src/main/kotlin/io/rtron/math/processing/triangulation/Poly2TriTriangulationAlgorithm.kt

@@ -37,7 +37,7 @@ import org.poly2tri.geometry.polygon.PolygonPoint as P2TPolygonPoint
  */
 class Poly2TriTriangulationAlgorithm : TriangulationAlgorithm() {
 
-    override fun triangulate(vertices: List<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> = either.eager {
+    override fun triangulate(vertices: NonEmptyList<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> = either.eager {
         val polygon = P2TPolygon(vertices.map { P2TPolygonPoint(it.x, it.y, it.z) })
 
         poly2TriTriangulation(polygon).bind()

rtron-math/src/main/kotlin/io/rtron/math/processing/triangulation/ProjectedTriangulationAlgorithm.kt

@@ -17,6 +17,7 @@
 package io.rtron.math.processing.triangulation
 
 import arrow.core.Either
+import arrow.core.NonEmptyList
 import arrow.core.continuations.either
 import arrow.core.left
 import io.rtron.math.geometry.euclidean.threed.point.Vector3D
@@ -36,7 +37,7 @@ class ProjectedTriangulationAlgorithm(
     private val triangulationAlgorithm: TriangulationAlgorithm
 ) : TriangulationAlgorithm() {
 
-    override fun triangulate(vertices: List<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> = either.eager {
+    override fun triangulate(vertices: NonEmptyList<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>> = either.eager {
         val projectedVertices = projectVertices(vertices, tolerance)
         val projectedPolygonsTriangulated = triangulationAlgorithm
             .triangulate(projectedVertices, tolerance).bind()
@@ -47,7 +48,7 @@ class ProjectedTriangulationAlgorithm(
     /**
      * Projects the [vertices] into a best fitting plane.
      */
-    private fun projectVertices(vertices: List<Vector3D>, tolerance: Double): List<Vector3D> {
+    private fun projectVertices(vertices: NonEmptyList<Vector3D>, tolerance: Double): NonEmptyList<Vector3D> {
         val affine = run {
             val plane = vertices.calculateBestFittingPlane(tolerance)
             val affineTranslation = Affine3D.of(plane.point)

rtron-math/src/main/kotlin/io/rtron/math/processing/triangulation/TriangulationAlgorithm.kt

@@ -53,5 +53,5 @@ abstract class TriangulationAlgorithm {
      * @param vertices list of vertices representing the outline to be triangulated
      * @return list of triangulated [Polygon3D]
      */
-    internal abstract fun triangulate(vertices: List<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>>
+    internal abstract fun triangulate(vertices: NonEmptyList<Vector3D>, tolerance: Double): Either<TriangulatorException, List<Polygon3D>>
 }

rtron-math/src/main/kotlin/io/rtron/math/processing/triangulation/TriangulatorException.kt

@@ -20,6 +20,5 @@ sealed class TriangulatorException(val message: String) {
     data class Poly2TriException(val reason: String) : TriangulatorException("Poly2Tri-Triangulation failure: $reason")
     data class DifferentVertices(val suffix: String = "") : TriangulatorException("Triangulation algorithm produced different vertices.")
     data class ColinearVertices(val suffix: String = "") : TriangulatorException("Triangulation failure (colinear vertices).")
+    data class FirstVertexDuplicated(val suffix: String = "") : TriangulatorException("First vertex has duplicate vertices.")
 }
-
-fun TriangulatorException.toIllegalStateException() = IllegalStateException(this.message)

rtron-math/src/main/kotlin/io/rtron/math/transform/Affine3D.kt

@@ -16,6 +16,7 @@
 
 package io.rtron.math.transform
 
+import arrow.core.NonEmptyList
 import io.rtron.math.geometry.euclidean.threed.Pose3D
 import io.rtron.math.geometry.euclidean.threed.Rotation3D
 import io.rtron.math.geometry.euclidean.threed.point.Vector3D
@@ -58,6 +59,9 @@ class Affine3D(
     @JvmName("transformOfListVector3D")
     fun transform(points: List<Vector3D>): List<Vector3D> = points.map { transform(it) }
 
+    @JvmName("transformOfNonEmptyListVector3D")
+    fun transform(points: NonEmptyList<Vector3D>): NonEmptyList<Vector3D> = points.map { transform(it) }
+
     @JvmName("inverseTransformOfListVector3D")
     fun inverseTransform(points: List<Vector3D>) = points.map { inverseTransform(it) }