CHANGELOG.md

@@ -10,6 +10,7 @@
 - Algebra now has an obligatory `bufferFactory` (#477).
 
 ### Changed
+- Trajectory use type-safe angles
 - Tensor operations switched to prefix notation
 - Row-wise and column-wise ND shapes in the core
 - Shape is read-only

kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/angles.kt

@@ -7,8 +7,9 @@ package space.kscience.kmath.geometry
 
 import kotlin.jvm.JvmInline
 import kotlin.math.PI
+import kotlin.math.floor
 
-public sealed interface Angle {
+public sealed interface Angle : Comparable<Angle> {
     public fun toRadians(): Radians
     public fun toDegrees(): Degrees
 
@@ -17,7 +18,15 @@ public sealed interface Angle {
 
     public operator fun times(other: Number): Angle
     public operator fun div(other: Number): Angle
+    public operator fun div(other: Angle): Double
     public operator fun unaryMinus(): Angle
+
+    public companion object {
+        public val zero: Radians = Radians(0.0)
+        public val pi: Radians = Radians(PI)
+        public val piTimes2: Radians = Radians(PI * 2)
+        public val piDiv2: Radians = Radians(PI / 2)
+    }
 }
 
 /**
@@ -28,12 +37,16 @@ public value class Radians(public val value: Double) : Angle {
     override fun toRadians(): Radians = this
     override fun toDegrees(): Degrees = Degrees(value * 180 / PI)
 
-    public override fun plus(other: Angle): Radians = Radians(value + other.toRadians().value)
-    public override fun minus(other: Angle): Radians = Radians(value - other.toRadians().value)
+    public override fun plus(other: Angle): Radians = Radians(value + other.radians)
+    public override fun minus(other: Angle): Radians = Radians(value - other.radians)
 
-    public override fun times(other: Number): Radians = Radians(value + other.toDouble())
+    public override fun times(other: Number): Radians = Radians(value * other.toDouble())
     public override fun div(other: Number): Radians = Radians(value / other.toDouble())
+    override fun div(other: Angle): Double = value / other.radians
+
     public override fun unaryMinus(): Radians = Radians(-value)
+
+    override fun compareTo(other: Angle): Int = value.compareTo(other.radians)
 }
 
 public fun sin(angle: Angle): Double = kotlin.math.sin(angle.toRadians().value)
@@ -42,6 +55,8 @@ public fun tan(angle: Angle): Double = kotlin.math.tan(angle.toRadians().value)
 
 public val Number.radians: Radians get() = Radians(toDouble())
 
+public val Angle.radians: Double get() = toRadians().value
+
 /**
  * Type safe degrees
  */
@@ -50,30 +65,26 @@ public value class Degrees(public val value: Double) : Angle {
     override fun toRadians(): Radians = Radians(value * PI / 180)
     override fun toDegrees(): Degrees = this
 
-    public override fun plus(other: Angle): Degrees = Degrees(value + other.toDegrees().value)
-    public override fun minus(other: Angle): Degrees = Degrees(value - other.toDegrees().value)
+    public override fun plus(other: Angle): Degrees = Degrees(value + other.degrees)
+    public override fun minus(other: Angle): Degrees = Degrees(value - other.degrees)
 
-    public override fun times(other: Number): Degrees = Degrees(value + other.toDouble())
+    public override fun times(other: Number): Degrees = Degrees(value * other.toDouble())
     public override fun div(other: Number): Degrees = Degrees(value / other.toDouble())
+    override fun div(other: Angle): Double = value / other.degrees
+
     public override fun unaryMinus(): Degrees = Degrees(-value)
+
+    override fun compareTo(other: Angle): Int = value.compareTo(other.degrees)
 }
 
 public val Number.degrees: Degrees get() = Degrees(toDouble())
 
+public val Angle.degrees: Double get() = toDegrees().value
+
 /**
- * A holder class for Pi representation in radians and degrees
+ * Normalized angle 2 PI range symmetric around [center]. By default, uses (0, 2PI) range.
  */
-public object Pi {
-    public val radians: Radians = Radians(PI)
-    public val degrees: Degrees = radians.toDegrees()
-}
+public fun Angle.normalized(center: Angle = Angle.pi): Angle =
+    this - Angle.piTimes2 * floor((radians + PI - center.radians) / PI / 2)
 
-public object PiTimes2 {
-    public val radians: Radians = Radians(2 * PI)
-    public val degrees: Degrees = radians.toDegrees()
-}
-
-public object PiDiv2 {
-    public val radians: Radians = Radians(PI / 2)
-    public val degrees: Degrees = radians.toDegrees()
-}
+public fun abs(angle: Angle): Angle = if (angle < Angle.zero) -angle else angle

kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/AngleTest.kt

@@ -0,0 +1,16 @@
+package space.kscience.kmath.geometry
+
+import kotlin.test.Test
+import kotlin.test.assertEquals
+
+class AngleTest {
+    @Test
+    fun normalization() {
+        assertEquals(30.degrees, 390.degrees.normalized())
+        assertEquals(30.degrees, (-330).degrees.normalized())
+        assertEquals(200.degrees, 200.degrees.normalized())
+        assertEquals(30.degrees, 390.degrees.normalized(Angle.zero))
+        assertEquals(30.degrees, (-330).degrees.normalized(Angle.zero))
+        assertEquals((-160).degrees, 200.degrees.normalized(Angle.zero))
+    }
+}

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt

@@ -5,13 +5,9 @@
 
 package space.kscience.kmath.trajectory
 
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.*
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
-import kotlin.math.PI
 import kotlin.math.acos
-import kotlin.math.cos
-import kotlin.math.sin
 
 internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
 
@@ -65,12 +61,11 @@ private fun innerTangent(
     with(Euclidean2DSpace) {
         val centers = StraightTrajectory2D(base.center, direction.center)
         if (centers.length < base.radius * 2) return null
-        val angle = theta(
-            when (side) {
-                CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length)
-                CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length)
-            }
-        )
+        val angle = when (side) {
+            CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians
+            CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians
+        }.normalized()
+
         val dX = base.radius * sin(angle)
         val dY = base.radius * cos(angle)
         val p1 = vector(base.center.x + dX, base.center.y + dY)
@@ -78,8 +73,6 @@ private fun innerTangent(
         return StraightTrajectory2D(p1, p2)
     }
 
-internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
-
 
 @Suppress("DuplicatedCode")
 public object DubinsPath {
@@ -129,13 +122,13 @@ public object DubinsPath {
             if (centers.length > turningRadius * 4) return null
 
             val firstVariant = run {
-                var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
+                var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
                 var dX = turningRadius * sin(theta)
                 var dY = turningRadius * cos(theta)
                 val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
                 val e = Circle2D(p, turningRadius)
                 val p1 = vector(c1.center.x + dX, c1.center.y + dY)
-                theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
+                theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
@@ -146,13 +139,13 @@ public object DubinsPath {
             }
 
             val secondVariant = run {
-                var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
+                var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
                 var dX = turningRadius * sin(theta)
                 var dY = turningRadius * cos(theta)
                 val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
                 val e = Circle2D(p, turningRadius)
                 val p1 = vector(c1.center.x + dX, c1.center.y + dY)
-                theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
+                theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
@@ -173,13 +166,13 @@ public object DubinsPath {
             if (centers.length > turningRadius * 4) return null
 
             val firstVariant = run {
-                var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
+                var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
                 var dX = turningRadius * sin(theta)
                 var dY = turningRadius * cos(theta)
                 val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
                 val e = Circle2D(p, turningRadius)
                 val p1 = vector(c1.center.x + dX, c1.center.y + dY)
-                theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
+                theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)
@@ -190,13 +183,13 @@ public object DubinsPath {
             }
 
             val secondVariant = run {
-                var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
+                var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
                 var dX = turningRadius * sin(theta)
                 var dY = turningRadius * cos(theta)
                 val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
                 val e = Circle2D(p, turningRadius)
                 val p1 = vector(c1.center.x + dX, c1.center.y + dY)
-                theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
+                theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
                 dX = turningRadius * sin(theta)
                 dY = turningRadius * cos(theta)
                 val p2 = vector(e.center.x + dX, e.center.y + dY)

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt

@@ -12,9 +12,7 @@ import kotlinx.serialization.UseSerializers
 import kotlinx.serialization.descriptors.SerialDescriptor
 import kotlinx.serialization.encoding.Decoder
 import kotlinx.serialization.encoding.Encoder
-import space.kscience.kmath.geometry.DoubleVector2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
-import space.kscience.kmath.geometry.Vector
+import space.kscience.kmath.geometry.*
 import kotlin.math.atan2
 
 /**
@@ -23,7 +21,7 @@ import kotlin.math.atan2
 @Serializable(DubinsPose2DSerializer::class)
 public interface DubinsPose2D : DoubleVector2D {
     public val coordinates: DoubleVector2D
-    public val bearing: Double
+    public val bearing: Angle
 }
 
 @Serializable
@@ -31,14 +29,14 @@ public class PhaseVector2D(
     override val coordinates: DoubleVector2D,
     public val velocity: DoubleVector2D,
 ) : DubinsPose2D, DoubleVector2D by coordinates {
-    override val bearing: Double get() = atan2(velocity.x, velocity.y)
+    override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
 }
 
 @Serializable
 @SerialName("DubinsPose2D")
 private class DubinsPose2DImpl(
     override val coordinates: DoubleVector2D,
-    override val bearing: Double,
+    override val bearing: Angle,
 ) : DubinsPose2D, DoubleVector2D by coordinates{
 
     override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
@@ -60,4 +58,4 @@ public object DubinsPose2DSerializer: KSerializer<DubinsPose2D>{
     }
 }
 
-public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)
+public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)

kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt

@@ -3,15 +3,13 @@
  * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
  */
 @file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
+
 package space.kscience.kmath.trajectory
 
 import kotlinx.serialization.Serializable
 import kotlinx.serialization.UseSerializers
-import space.kscience.kmath.geometry.Circle2D
-import space.kscience.kmath.geometry.DoubleVector2D
-import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.*
 import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
-import kotlin.math.PI
 import kotlin.math.atan2
 
 @Serializable
@@ -29,7 +27,7 @@ public data class StraightTrajectory2D(
 ) : Trajectory2D {
     override val length: Double get() = start.distanceTo(end)
 
-    public val bearing: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
+    public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized()
 }
 
 /**
@@ -49,26 +47,25 @@ public data class CircleTrajectory2D(
     /**
      * Arc length in radians
      */
-    val arcLength: Double
-        get() = theta(
-            if (direction == Direction.LEFT) {
+    val arcLength: Angle
+        get() = if (direction == Direction.LEFT) {
             start.bearing - end.bearing
         } else {
             end.bearing - start.bearing
-            }
-        )
+        }.normalized()
+
 
     override val length: Double by lazy {
-        circle.radius * arcLength
+        circle.radius * arcLength.radians
     }
 
     public val direction: Direction by lazy {
         if (start.y < circle.center.y) {
-            if (start.bearing > PI) Direction.RIGHT else Direction.LEFT
+            if (start.bearing > Angle.pi) Direction.RIGHT else Direction.LEFT
         } else if (start.y > circle.center.y) {
-            if (start.bearing < PI) Direction.RIGHT else Direction.LEFT
+            if (start.bearing < Angle.pi) Direction.RIGHT else Direction.LEFT
         } else {
-            if (start.bearing == 0.0) {
+            if (start.bearing == Angle.zero) {
                 if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
             } else {
                 if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
@@ -85,13 +82,13 @@ public data class CircleTrajectory2D(
         ): CircleTrajectory2D {
             fun calculatePose(
                 vector: DoubleVector2D,
-                theta: Double,
+                theta: Angle,
                 direction: Direction,
             ): DubinsPose2D = DubinsPose2D(
                 vector,
                 when (direction) {
-                    Direction.LEFT -> theta(theta - PI / 2)
-                    Direction.RIGHT -> theta(theta + PI / 2)
+                    Direction.LEFT -> (theta - Angle.piDiv2).normalized()
+                    Direction.RIGHT -> (theta + Angle.piDiv2).normalized()
                 }
             )
 
@@ -113,5 +110,6 @@ public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Tr
     override val length: Double get() = segments.sumOf { it.length }
 }
 
-public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList())
+public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
+    CompositeTrajectory2D(segments.toList())
 

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt

@@ -6,16 +6,18 @@
 package space.kscience.kmath.trajectory
 
 import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.radians
+import space.kscience.kmath.geometry.sin
 import kotlin.math.PI
 import kotlin.math.abs
-import kotlin.math.sin
 
 const val maxFloatDelta = 0.000001
 
 fun Double.radiansToDegrees() = this * 180 / PI
 
 fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
-fun DubinsPose2D.equalsFloat(other: DubinsPose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.equalFloat(other.bearing)
+fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
+    x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.radians.equalFloat(other.bearing.radians)
 
 fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start)
 fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) {

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt

@@ -8,8 +8,8 @@ package space.kscience.kmath.trajectory.segments
 import space.kscience.kmath.geometry.Circle2D
 import space.kscience.kmath.geometry.Euclidean2DSpace
 import space.kscience.kmath.geometry.circumference
+import space.kscience.kmath.geometry.degrees
 import space.kscience.kmath.trajectory.CircleTrajectory2D
-import space.kscience.kmath.trajectory.radiansToDegrees
 import kotlin.test.Test
 import kotlin.test.assertEquals
 
@@ -20,7 +20,7 @@ class ArcTests {
         val circle = Circle2D(vector(0.0, 0.0), 2.0)
         val arc = CircleTrajectory2D.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory2D.Direction.RIGHT)
         assertEquals(circle.circumference / 4, arc.length, 1.0)
-        assertEquals(0.0, arc.start.bearing.radiansToDegrees())
-        assertEquals(90.0, arc.end.bearing.radiansToDegrees())
+        assertEquals(0.0, arc.start.bearing.degrees)
+        assertEquals(90.0, arc.end.bearing.degrees)
     }
 }

kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt

@@ -6,8 +6,8 @@
 package space.kscience.kmath.trajectory.segments
 
 import space.kscience.kmath.geometry.Euclidean2DSpace
+import space.kscience.kmath.geometry.degrees
 import space.kscience.kmath.trajectory.StraightTrajectory2D
-import space.kscience.kmath.trajectory.radiansToDegrees
 import kotlin.math.pow
 import kotlin.math.sqrt
 import kotlin.test.Test
@@ -19,19 +19,19 @@ class LineTests {
     fun lineTest() = with(Euclidean2DSpace){
         val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
         assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
-        assertEquals(45.0, straight.bearing.radiansToDegrees())
+        assertEquals(45.0, straight.bearing.degrees)
     }
 
     @Test
     fun lineAngleTest() = with(Euclidean2DSpace){
         //val zero = Vector2D(0.0, 0.0)
         val north = StraightTrajectory2D(zero, vector(0.0, 2.0))
-        assertEquals(0.0, north.bearing.radiansToDegrees())
+        assertEquals(0.0, north.bearing.degrees)
         val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
-        assertEquals(90.0, east.bearing.radiansToDegrees())
+        assertEquals(90.0, east.bearing.degrees)
         val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
-        assertEquals(180.0, south.bearing.radiansToDegrees())
+        assertEquals(180.0, south.bearing.degrees)
         val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
-        assertEquals(270.0, west.bearing.radiansToDegrees())
+        assertEquals(270.0, west.bearing.degrees)
     }
 }