diff --git a/CHANGELOG.md b/CHANGELOG.md index b8cb0d976..404366a03 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/angles.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/angles.kt index e7b0afcda..45022ad05 100644 --- a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/angles.kt +++ b/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 { 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() -} \ No newline at end of file +public fun abs(angle: Angle): Angle = if (angle < Angle.zero) -angle else angle \ No newline at end of file diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/AngleTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/AngleTest.kt new file mode 100644 index 000000000..b8086eb75 --- /dev/null +++ b/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)) + } +} \ No newline at end of file diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt index 6bbf3e55b..568ef691a 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/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 { @@ -91,8 +84,8 @@ public object DubinsPath { /** * Return Dubins trajectory type or null if trajectory is not a Dubins path */ - public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type?{ - if(trajectory2D.segments.size != 3) return null + public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type? { + if (trajectory2D.segments.size != 3) return null val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null val b = trajectory2D.segments[1] val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null @@ -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) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt index ff2198bbb..8362d0cb5 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt +++ b/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{ } } -public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta) \ No newline at end of file +public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta) \ No newline at end of file diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt index 120bedf90..b8916e748 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/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) { - start.bearing - end.bearing - } else { - end.bearing - start.bearing - } - ) + 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() } ) @@ -100,7 +97,7 @@ public data class CircleTrajectory2D( val pose1 = calculatePose(start, s1.bearing, direction) val pose2 = calculatePose(end, s2.bearing, direction) val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2) - if(trajectory.direction != direction){ + if (trajectory.direction != direction) { error("Trajectory direction mismatch") } return trajectory @@ -113,5 +110,6 @@ public class CompositeTrajectory2D(public val segments: List) : 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()) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt index 4599f30f8..c69ad24f1 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt @@ -6,19 +6,21 @@ 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){ +fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) { val dX = width * sin(inverse().bearing) val dY = width * sin(bearing) diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt index af1444ade..643f9eaaa 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt +++ b/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) } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt index 03320a3df..54deb2193 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ b/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) } }