From 3260c3d17146226f465aa98a5fc6f8a7cb4262cd Mon Sep 17 00:00:00 2001
From: Erik Schouten <Erik@ErikSchouten.com>
Date: Sun, 17 Jul 2022 14:39:43 +0200
Subject: [PATCH] Pose2D facrtory function

---
 .../space/kscience/kmath/trajectory/segments/Arc.kt  | 12 +++++++-----
 .../kmath/trajectory/segments/components/Pose2D.kt   |  6 +++---
 .../kscience/kmath/trajectory/dubins/DubinsTests.kt  | 10 +++++-----
 3 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
index b5e091db7..fe101f359 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
@@ -34,9 +34,11 @@ public class Arc(
     }
 
     private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
-        if (direction == Direction.LEFT) {
-            Pose2D(vector.x, vector.y, theta(theta - PI / 2))
-        } else {
-            Pose2D(vector.x, vector.y, theta(theta + PI / 2))
-        }
+        Pose2D.of(
+            vector,
+            when (direction) {
+                Direction.LEFT -> theta(theta - PI / 2)
+                Direction.RIGHT -> theta(theta + PI / 2)
+            }
+        )
 }
diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
index d00dfbd96..c49da3187 100644
--- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
+++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
@@ -1,13 +1,13 @@
 package space.kscience.kmath.trajectory.segments.components
 
 import space.kscience.kmath.geometry.Vector2D
-import kotlin.math.cos
-import kotlin.math.sin
 
 public data class Pose2D(
     override val x: Double,
     override val y: Double,
     public val theta: Double
 ) : Vector2D {
-    internal constructor(vector: Vector2D, theta: Double) : this(vector.x, vector.y, theta)
+    internal companion object {
+        internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
+    }
 }
diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
index efe35e5d8..025aab2b7 100644
--- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
+++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
@@ -7,10 +7,10 @@ package space.kscience.kmath.trajectory.dubins
 
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
 import space.kscience.kmath.geometry.Vector2D
-import space.kscience.kmath.trajectory.segments.Arc
 import space.kscience.kmath.trajectory.equalFloat
 import space.kscience.kmath.trajectory.equalsFloat
 import space.kscience.kmath.trajectory.inverse
+import space.kscience.kmath.trajectory.segments.Arc
 import space.kscience.kmath.trajectory.segments.Straight
 import space.kscience.kmath.trajectory.segments.components.Pose2D
 import space.kscience.kmath.trajectory.shift
@@ -26,8 +26,8 @@ class DubinsTests {
         val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
         val lineP1 = straight.shift(1, 10.0).inverse()
 
-        val start = Pose2D(straight.end, straight.theta)
-        val end = Pose2D(lineP1.start, lineP1.theta)
+        val start = Pose2D.of(straight.end, straight.theta)
+        val end = Pose2D.of(lineP1.start, lineP1.theta)
         val radius = 2.0
         val dubins = DubinsPathFactory(start, end, radius)
 
@@ -58,8 +58,8 @@ class DubinsTests {
                 assertTrue(path.c.pose1.equalsFloat(b.pose2))
             } else if (path.b is Straight) {
                 val b = path.b as Straight
-                assertTrue(path.a.pose2.equalsFloat(Pose2D(b.start, b.theta)))
-                assertTrue(path.c.pose1.equalsFloat(Pose2D(b.end, b.theta)))
+                assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta)))
+                assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta)))
             }
         }
     }