From 32769d690683d8b1975f1d3380c2f6f3ffe49972 Mon Sep 17 00:00:00 2001 From: Erik Schouten Date: Fri, 15 Jul 2022 18:13:50 +0200 Subject: [PATCH] Dubins path --- kmath-trajectory/README.md | 32 ++++ kmath-trajectory/build.gradle.kts | 15 ++ .../kmath/trajectory/dubins/DubinsPath.kt | 30 +++ .../trajectory/dubins/DubinsPathFactory.kt | 171 ++++++++++++++++++ .../kscience/kmath/trajectory/segments/Arc.kt | 43 +++++ .../kmath/trajectory/segments/Line.kt | 23 +++ .../kmath/trajectory/segments/Segment.kt | 5 + .../trajectory/segments/components/Circle.kt | 11 ++ .../trajectory/segments/components/Pose2D.kt | 18 ++ .../space/kscience/kmath/trajectory/Math.kt | 27 +++ .../kmath/trajectory/dubins/DubinsTests.kt | 68 +++++++ .../kmath/trajectory/segments/ArcTests.kt | 24 +++ .../kmath/trajectory/segments/LineTests.kt | 33 ++++ settings.gradle.kts | 3 +- 14 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 kmath-trajectory/README.md create mode 100644 kmath-trajectory/build.gradle.kts create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt create mode 100644 kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt create mode 100644 kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt diff --git a/kmath-trajectory/README.md b/kmath-trajectory/README.md new file mode 100644 index 000000000..cb2b6989f --- /dev/null +++ b/kmath-trajectory/README.md @@ -0,0 +1,32 @@ +# Module kmath-trajectory + + + +## Usage + +## Artifact: + +The Maven coordinates of this project are `space.kscience:kmath-trajectory:0.3.0`. + +**Gradle Groovy:** +```groovy +repositories { + maven { url 'https://repo.kotlin.link' } + mavenCentral() +} + +dependencies { + implementation 'space.kscience:kmath-trajectory:0.3.0' +} +``` +**Gradle Kotlin DSL:** +```kotlin +repositories { + maven("https://repo.kotlin.link") + mavenCentral() +} + +dependencies { + implementation("space.kscience:kmath-trajectory:0.3.0") +} +``` diff --git a/kmath-trajectory/build.gradle.kts b/kmath-trajectory/build.gradle.kts new file mode 100644 index 000000000..502867ee3 --- /dev/null +++ b/kmath-trajectory/build.gradle.kts @@ -0,0 +1,15 @@ +plugins { + kotlin("multiplatform") + id("ru.mipt.npm.gradle.common") + id("ru.mipt.npm.gradle.native") +} + +kotlin.sourceSets.commonMain { + dependencies { + api(projects.kmath.kmathGeometry) + } +} + +readme { + maturity = ru.mipt.npm.gradle.Maturity.PROTOTYPE +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt new file mode 100644 index 000000000..005d7fd60 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt @@ -0,0 +1,30 @@ +/* + * Copyright 2018-2021 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +package space.kscience.kmath.trajectory.dubins + +import space.kscience.kmath.trajectory.segments.Arc +import space.kscience.kmath.trajectory.segments.Segment + +public class DubinsPath( + public val a: Arc, + public val b: Segment, + public val c: Arc, +) { + + public val type: TYPE = TYPE.valueOf( + arrayOf( + a.direction.name[0], + if (b is Arc) b.direction.name[0] else 'S', + c.direction.name[0] + ).toCharArray().concatToString() + ) + + public val length: Double = a.length + b.length + c.length + + public enum class TYPE { + RLR, LRL, RSR, LSL, RSL, LSR + } +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt new file mode 100644 index 000000000..98ed8ed32 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt @@ -0,0 +1,171 @@ +/* + * Copyright 2018-2021 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +package space.kscience.kmath.trajectory.dubins + +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.Arc +import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.segments.components.Circle +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.length +import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.acos +import kotlin.math.cos +import kotlin.math.sin + +public class DubinsPathFactory( + private val base: Pose2D, + private val direction: Pose2D, + private val turningRadius: Double +) { + + public val all: List get() = listOfNotNull(rlr, lrl, rsr, lsl, rsl, lsr) + public val shortest: DubinsPath get() = all.minByOrNull { it.length }!! + public operator fun get(type: DubinsPath.TYPE): DubinsPath? = all.find { it.type == type } + + public val rlr: DubinsPath? get () { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val centers = Line2D(c1.center, c2.center) + return if (centers.length < turningRadius * 4) { + var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + var dX = turningRadius * sin(theta) + var dY = turningRadius * cos(theta) + val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val e = Circle(p, turningRadius) + val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + dX = turningRadius * sin(theta) + dY = turningRadius * cos(theta) + val p2 = Vector2D(e.center.x + dX, e.center.y + dY) + val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT) + val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT) + val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT) + DubinsPath(a1, a2, a3) + } else { + null + } + } + + private val lrl: DubinsPath? get () { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val centers = Line2D(c1.center, c2.center) + return if (centers.length < turningRadius * 4) { + var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta + var dX = turningRadius * sin(theta) + var dY = turningRadius * cos(theta) + val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) + val e = Circle(p, turningRadius) + val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) + theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta + dX = turningRadius * sin(theta) + dY = turningRadius * cos(theta) + val p2 = Vector2D(e.center.x + dX, e.center.y + dY) + val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.LEFT) + val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT) + DubinsPath(a1, a2, a3) + } else { + null + } + } + + public val rsr: DubinsPath? get () { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val l = leftOuterTangent(c1, c2) + val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) + return DubinsPath(a1, LineSegment(l), a3) + } + + public val lsl: DubinsPath + get () { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val l = rightOuterTangent(c1, c2) + val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) + val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) + return DubinsPath(a1, LineSegment(l), a3) + } + + public val rsl: DubinsPath? get () { + val c1 = base.getRightCircle(turningRadius) + val c2 = direction.getLeftCircle(turningRadius) + val l = rightInnerTangent(c1, c2) + return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != null) { + val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) + DubinsPath(a1, LineSegment(l), a3) + } else { + null + } + } + + public val lsr: DubinsPath? get () { + val c1 = base.getLeftCircle(turningRadius) + val c2 = direction.getRightCircle(turningRadius) + val l = leftInnerTangent(c1, c2) + return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != null) { + val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) + val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) + DubinsPath(a1, LineSegment(l), a3) + } else { + null + } + } +} + +private enum class SIDE { + LEFT, RIGHT +} + +private fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first +private fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second +private fun Pose2D.getTangentCircles(radius: Double): Pair { + val dX = radius * cos(theta) + val dY = radius * sin(theta) + return Circle(Vector2D(x - dX, y + dY), radius) to Circle(Vector2D(x + dX, y - dY), radius) +} + +private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) +private fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) +private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { + val centers = Line2D(a.center, b.center) + val p1 = when (side) { + SIDE.LEFT -> Vector2D( + a.center.x - a.radius * cos(centers.theta), + a.center.y + a.radius * sin(centers.theta) + ) + SIDE.RIGHT -> Vector2D( + a.center.x + a.radius * cos(centers.theta), + a.center.y - a.radius * sin(centers.theta) + ) + } + return Line2D(p1, Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y))) +} + +private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) +private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) +private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? { + val centers = Line2D(base.center, direction.center) + return if (centers.length > base.radius * 2) { + val angle = when (side) { + SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) + SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) + }.theta + val dX = base.radius * sin(angle) + val dY = base.radius * cos(angle) + val p1 = Vector2D(base.center.x + dX, base.center.y + dY) + val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY) + Line2D(p1, p2) + } else { + null + } +} 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 new file mode 100644 index 000000000..a7b2fe259 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt @@ -0,0 +1,43 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.components.Circle +import space.kscience.kmath.trajectory.segments.components.Pose2D +import kotlin.math.PI + +public class Arc( + center: Vector2D, + radius: Double, + a: Vector2D, + b: Vector2D, + internal val direction: Direction +) : Circle(center, radius), Segment { + + private val l1 = Line2D(center, a) + private val l2 = Line2D(center, b) + + internal val pose1 = calculatePose(a, l1.theta) + internal val pose2 = calculatePose(b, l2.theta) + private val angle = calculateAngle() + override val length: Double = calculateLength() + + public enum class Direction { + LEFT, RIGHT + } + + private fun calculateAngle() = + (if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta).theta + + private fun calculateLength(): Double { + val proportion = angle / (2 * PI) + return circumference * proportion + } + + private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = + if (direction == Direction.LEFT) { + Pose2D(vector.x, vector.y, (theta - PI / 2).theta) + } else { + Pose2D(vector.x, vector.y, (theta + PI / 2).theta) + } +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt new file mode 100644 index 000000000..f63372016 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt @@ -0,0 +1,23 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.operations.DoubleField.pow +import kotlin.math.PI +import kotlin.math.atan2 +import kotlin.math.sqrt + +public class LineSegment( + internal val line: Line2D +) : Segment { + override val length: Double + get() = line.length +} + +internal val Line2D.theta: Double + get() = atan2(direction.x - base.x, direction.y - base.y).theta + +internal val Line2D.length: Double + get() = sqrt((direction.x - base.x).pow(2) + (direction.y - base.y).pow(2)) + +internal val Double.theta: Double + get() = (this + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt new file mode 100644 index 000000000..8a1d086fc --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt @@ -0,0 +1,5 @@ +package space.kscience.kmath.trajectory.segments + +public interface Segment { + public val length: Double +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt new file mode 100644 index 000000000..946dd8c6e --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt @@ -0,0 +1,11 @@ +package space.kscience.kmath.trajectory.segments.components + +import space.kscience.kmath.geometry.Vector2D +import kotlin.math.PI + +public open class Circle( + internal val center: Vector2D, + internal val radius: Double +) { + internal val circumference = radius * 2 * PI +} 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 new file mode 100644 index 000000000..6bcc3d308 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt @@ -0,0 +1,18 @@ +package space.kscience.kmath.trajectory.segments.components + +import space.kscience.kmath.geometry.Vector2D +import kotlin.math.cos +import kotlin.math.sin + +public 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) + + override fun toString(): String { + return "Pose2D(x=$x, y=$y, theta=$theta)" + } +} 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 new file mode 100644 index 000000000..9258e6b4a --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt @@ -0,0 +1,27 @@ +package space.kscience.kmath.trajectory + +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.PI +import kotlin.math.abs +import kotlin.math.sin + +private const val maxFloatDelta = 0.000001 + +fun Double.radiansToDegrees() = this * 180 / PI + +fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta +fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta) + +fun Line2D.inverse() = Line2D(direction, base) +fun Line2D.shift(shift: Int, width: Double): Line2D { + val dX = width * sin(inverse().theta) + val dY = width * sin(theta) + + return Line2D( + Vector2D(base.x - dX * shift, base.y - dY * shift), + Vector2D(direction.x - dX * shift, direction.y - dY * shift) + ) +} 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 new file mode 100644 index 000000000..583e7a4e0 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -0,0 +1,68 @@ +/* + * Copyright 2018-2021 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +package space.kscience.kmath.trajectory.dubins + +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.Arc +import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.equalFloat +import space.kscience.kmath.trajectory.equalsFloat +import space.kscience.kmath.trajectory.inverse +import space.kscience.kmath.trajectory.segments.components.Pose2D +import space.kscience.kmath.trajectory.segments.theta +import space.kscience.kmath.trajectory.shift +import kotlin.test.Test +import kotlin.test.assertNotNull +import kotlin.test.assertTrue + + +class DubinsTests { + + @Test + fun dubinsTest() { + val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val lineP1 = line.shift(1, 10.0).inverse() + + val start = Pose2D(line.direction, line.theta) + val end = Pose2D(lineP1.base, lineP1.theta) + val radius = 2.0 + val dubins = DubinsPathFactory(start, end, radius) + + val absoluteDistance = start.distanceTo(end) + println("Absolute distance: $absoluteDistance") + + val expectedLengths = mapOf( + DubinsPath.TYPE.RLR to 13.067681939031397, + DubinsPath.TYPE.RSR to 12.28318530717957, + DubinsPath.TYPE.LSL to 32.84955592153878, + DubinsPath.TYPE.RSL to 23.37758938854081, + DubinsPath.TYPE.LSR to 23.37758938854081 + ) + + expectedLengths.forEach { + val path = dubins[it.key] + assertNotNull(path, "Path ${it.key} not found") + println("${it.key}: ${path.length}") + assertTrue(it.value.equalFloat(path.length)) + + assertTrue(start.equalsFloat(path.a.pose1)) + assertTrue(end.equalsFloat(path.c.pose2)) + + // Not working, theta double precision inaccuracy + if (path.b is Arc) { + val b = path.b as Arc + assertTrue(path.a.pose2.equalsFloat(b.pose1)) + assertTrue(path.c.pose1.equalsFloat(b.pose2)) + } else if (path.b is LineSegment) { + val b = (path.b as LineSegment).line + assertTrue(path.a.pose2.equalsFloat(Pose2D(b.base, b.theta))) + assertTrue(path.c.pose1.equalsFloat(Pose2D(b.direction, b.theta))) + } + } + } +} 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 new file mode 100644 index 000000000..73b3a1d87 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt @@ -0,0 +1,24 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.radiansToDegrees +import space.kscience.kmath.trajectory.segments.components.Circle +import kotlin.test.Test +import kotlin.test.assertEquals + +class ArcTests { + + @Test + fun arcTest() { + val center = Vector2D(0.0, 0.0) + val radius = 2.0 + val expectedCircumference = 12.56637 + val circle = Circle(center, radius) + assertEquals(expectedCircumference, circle.circumference, 1.0) + + val arc = Arc(center, radius, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) + assertEquals(expectedCircumference / 4, arc.length, 1.0) + assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) + assertEquals(90.0, arc.pose2.theta.radiansToDegrees()) + } +} 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 new file mode 100644 index 000000000..30f5ef6d8 --- /dev/null +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -0,0 +1,33 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.kmath.geometry.Line2D +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.radiansToDegrees +import kotlin.math.pow +import kotlin.math.sqrt +import kotlin.test.Test +import kotlin.test.assertEquals + +class LineTests { + + @Test + fun lineTest() { + val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), line.length) + assertEquals(45.0, line.theta.radiansToDegrees()) + } + + @Test + fun lineAngleTest() { + val zero = Vector2D(0.0, 0.0) + val north = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + assertEquals(0.0, north.theta.radiansToDegrees()) + val east = Line2D(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + assertEquals(90.0, east.theta.radiansToDegrees()) + val south = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + assertEquals(180.0, south.theta.radiansToDegrees()) + val west = Line2D(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + assertEquals(270.0, west.theta.radiansToDegrees()) + } +} diff --git a/settings.gradle.kts b/settings.gradle.kts index e3c621e9a..b564972c7 100644 --- a/settings.gradle.kts +++ b/settings.gradle.kts @@ -44,6 +44,7 @@ include( ":kmath-jupyter", ":kmath-symja", ":kmath-jafama", + ":kmath-trajectory", ":examples", ":benchmarks", -) \ No newline at end of file +)