kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

Code simplification for Dubins path

Browse Source
This commit is contained in:
Alexander Nozik 2022-07-26 09:19:04 +03:00
parent 1e8a8cac7e
commit 323e8b6872
No known key found for this signature in database
GPG Key ID: F7FCF2DD25C71357
16 changed files with 225 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Circle2D.kt Normal file
View File

@ -0,0 +1,18 @@
/*
* 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.geometry
import kotlin.math.PI
/**
* A circle in 2D space
*/
public class Circle2D(
public val center: Vector2D,
public val radius: Double
)
public val Circle2D.circumference: Double get() = radius * 2 * PI

4
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Line.kt
View File

@ -5,6 +5,10 @@
package space.kscience.kmath.geometry
/**
* A line formed by [base] vector of start and a [direction] vector. Direction vector is not necessarily normalized,
* but its length does not affect line properties
*/
public data class Line<out V : Vector>(val base: V, val direction: V)
public typealias Line2D = Line<Vector2D>

68
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt
View File

@ -5,40 +5,43 @@
package space.kscience.kmath.trajectory.dubins
import space.kscience.kmath.geometry.Circle2D
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.segments.Segment
import space.kscience.kmath.trajectory.segments.Straight
import space.kscience.kmath.trajectory.segments.components.Circle
import space.kscience.kmath.trajectory.segments.components.Pose2D
import space.kscience.kmath.trajectory.segments.ArcSegment
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import space.kscience.kmath.trajectory.segments.Trajectory
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
public class DubinsPath(
public val a: Arc,
public val b: Segment,
public val c: Arc,
) {
public val a: ArcSegment,
public val b: Trajectory,
public val c: ArcSegment,
) : Trajectory {
public val type: TYPE = TYPE.valueOf(
arrayOf(
a.direction.name[0],
if (b is Arc) b.direction.name[0] else 'S',
if (b is ArcSegment) b.direction.name[0] else 'S',
c.direction.name[0]
).toCharArray().concatToString()
)
public val length: Double = a.length + b.length + c.length
override val length: Double get() = a.length + b.length + c.length
public enum class TYPE {
RLR, LRL, RSR, LSL, RSL, LSR
}
public companion object {
public fun all(start: Pose2D, end: Pose2D, turningRadius: Double): List<DubinsPath> =
listOfNotNull(
public fun all(
start: Pose2D,
end: Pose2D,
turningRadius: Double,
): List<DubinsPath> = listOfNotNull(
rlr(start, end, turningRadius),
lrl(start, end, turningRadius),
rsr(start, end, turningRadius),
@ -46,50 +49,51 @@ public class DubinsPath(
rsl(start, end, turningRadius),
lsr(start, end, turningRadius)
)
public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
all(start, end, turningRadius).minByOrNull { it.length }!!
all(start, end, turningRadius).minBy { it.length }
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val centers = Straight(c1.center, c2.center)
val centers = StraightSegment(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
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 e = Circle2D(p, turningRadius)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.RIGHT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, a2, a3)
}
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val centers = Straight(c1.center, c2.center)
val centers = StraightSegment(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
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 e = Circle2D(p, turningRadius)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
val a1 = ArcSegment.of(c1.center, start, p1, ArcSegment.Direction.LEFT)
val a2 = ArcSegment.of(e.center, p1, p2, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, p2, end, ArcSegment.Direction.LEFT)
return DubinsPath(a1, a2, a3)
}
@ -97,8 +101,8 @@ public class DubinsPath(
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = leftOuterTangent(c1, c2)
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, s, a3)
}
@ -106,8 +110,8 @@ public class DubinsPath(
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = rightOuterTangent(c1, c2)
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
return DubinsPath(a1, s, a3)
}
@ -117,8 +121,8 @@ public class DubinsPath(
val s = rightInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
return DubinsPath(a1, s, a3)
}
@ -128,8 +132,8 @@ public class DubinsPath(
val s = leftInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, s, a3)
}
}

41
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/TrajectoryFunctions.kt → kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
View File

@ -5,10 +5,10 @@
package space.kscience.kmath.trajectory.dubins
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Straight
import space.kscience.kmath.trajectory.segments.components.Circle
import space.kscience.kmath.trajectory.segments.components.Pose2D
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.cos
@ -18,18 +18,19 @@ private enum class SIDE {
LEFT, RIGHT
}
internal fun Pose2D.getLeftCircle(radius: Double): Circle = getTangentCircles(radius).first
internal fun Pose2D.getRightCircle(radius: Double): Circle = getTangentCircles(radius).second
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle, Circle> {
internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
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)
return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
}
internal fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT)
internal fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT)
private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
val centers = Straight(a.center, b.center)
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT)
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT)
private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment {
val centers = StraightSegment(a.center, b.center)
val p1 = when (side) {
SIDE.LEFT -> Vector2D(
a.center.x - a.radius * cos(centers.theta),
@ -40,16 +41,20 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight {
a.center.y - a.radius * sin(centers.theta)
)
}
return Straight(
return StraightSegment(
p1,
Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
)
}
internal fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT)
internal fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT)
private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? {
val centers = Straight(base.center, direction.center)
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, SIDE.LEFT)
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, SIDE.RIGHT)
private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? {
val centers = StraightSegment(base.center, direction.center)
if (centers.length < base.radius * 2) return null
val angle = theta(
when (side) {
@ -61,7 +66,7 @@ private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight?
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)
return Straight(p1, p2)
return StraightSegment(p1, p2)
}
internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)

59
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
View File

@ -1,59 +0,0 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.dubins.theta
import space.kscience.kmath.trajectory.segments.components.Circle
import space.kscience.kmath.trajectory.segments.components.Pose2D
import kotlin.math.PI
public data class Arc(
public val circle: Circle,
public val start: Pose2D,
public val end: Pose2D
) : Segment {
internal companion object {
fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
val s1 = Straight(center, start)
val s2 = Straight(center, end)
val pose1 = calculatePose(start, s1.theta, direction)
val pose2 = calculatePose(end, s2.theta, direction)
return Arc(Circle(center, s1.length), pose1, pose2)
}
private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
Pose2D.of(
vector,
when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
Direction.RIGHT -> theta(theta + PI / 2)
}
)
}
internal enum class Direction {
LEFT, RIGHT
}
override val length: Double
get() {
val angle: Double =
theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
val proportion = angle / (2 * PI)
return circle.circumference * proportion
}
internal val direction: Direction
get() = if (start.y < circle.center.y) {
if (start.theta > PI) Direction.RIGHT else Direction.LEFT
} else if (start.y > circle.center.y) {
if (start.theta < PI) Direction.RIGHT else Direction.LEFT
} else {
if (start.theta == 0.0) {
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
} else {
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
}
}
}

21
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt Normal file
View File

@ -0,0 +1,21 @@
/*
* 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.segments
import space.kscience.kmath.geometry.Vector2D
/**
* A [Vector2D] with view direction
*/
public data class Pose2D(
override val x: Double,
override val y: Double,
public val theta: Double
) : Vector2D {
public companion object {
public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta)
}
}

5
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Segment.kt
View File

@ -1,5 +0,0 @@
package space.kscience.kmath.trajectory.segments
public interface Segment {
public val length: Double
}

18
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt
View File

@ -1,18 +0,0 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.dubins.theta
import kotlin.math.PI
import kotlin.math.atan2
public data class Straight(
internal val start: Vector2D,
internal val end: Vector2D
) : Segment {
override val length: Double
get() = start.distanceTo(end)
internal val theta: Double
get() = theta(atan2(end.x - start.x, end.y - start.y))
}

89
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt Normal file
View File

@ -0,0 +1,89 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.trajectory.dubins.theta
import kotlin.math.PI
import kotlin.math.atan2
public interface Trajectory {
public val length: Double
}
/**
* Straight path segment. The order of start and end defines the direction
*/
public data class StraightSegment(
internal val start: Vector2D,
internal val end: Vector2D,
) : Trajectory {
override val length: Double get() = start.distanceTo(end)
internal val theta: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
}
/**
* An arc segment
*/
public data class ArcSegment(
public val circle: Circle2D,
public val start: Pose2D,
public val end: Pose2D,
) : Trajectory {
public enum class Direction {
LEFT, RIGHT
}
override val length: Double by lazy {
val angle: Double = theta(
if (direction == Direction.LEFT) {
start.theta - end.theta
} else {
end.theta - start.theta
}
)
val proportion = angle / (2 * PI)
circle.circumference * proportion
}
internal val direction: Direction by lazy {
if (start.y < circle.center.y) {
if (start.theta > PI) Direction.RIGHT else Direction.LEFT
} else if (start.y > circle.center.y) {
if (start.theta < PI) Direction.RIGHT else Direction.LEFT
} else {
if (start.theta == 0.0) {
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
} else {
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
}
}
}
public companion object {
public fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): ArcSegment {
fun calculatePose(
vector: Vector2D,
theta: Double,
direction: Direction,
): Pose2D = Pose2D.of(
vector,
when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
Direction.RIGHT -> theta(theta + PI / 2)
}
)
val s1 = StraightSegment(center, start)
val s2 = StraightSegment(center, end)
val pose1 = calculatePose(start, s1.theta, direction)
val pose2 = calculatePose(end, s2.theta, direction)
return ArcSegment(Circle2D(center, s1.length), pose1, pose2)
}
}
}

11
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Circle.kt
View File

@ -1,11 +0,0 @@
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
}

13
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
View File

@ -1,13 +0,0 @@
package space.kscience.kmath.trajectory.segments.components
import space.kscience.kmath.geometry.Vector2D
public data class Pose2D(
override val x: Double,
override val y: Double,
public val theta: Double
) : Vector2D {
internal companion object {
internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
}
}

10
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
View File

@ -1,8 +1,8 @@
package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Straight
import space.kscience.kmath.trajectory.segments.components.Pose2D
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.sin
@ -14,12 +14,12 @@ 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 Straight.inverse() = Straight(end, start)
fun Straight.shift(shift: Int, width: Double): Straight {
fun StraightSegment.inverse() = StraightSegment(end, start)
fun StraightSegment.shift(shift: Int, width: Double): StraightSegment {
val dX = width * sin(inverse().theta)
val dY = width * sin(theta)
return Straight(
return StraightSegment(
Vector2D(start.x - dX * shift, start.y - dY * shift),
Vector2D(end.x - dX * shift, end.y - dY * shift)
)

16
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt
View File

@ -10,9 +10,9 @@ import space.kscience.kmath.geometry.Vector2D
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.segments.ArcSegment
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import space.kscience.kmath.trajectory.shift
import kotlin.test.Test
import kotlin.test.assertNotNull
@ -23,7 +23,7 @@ class DubinsTests {
@Test
fun dubinsTest() {
val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = Pose2D.of(straight.end, straight.theta)
@ -52,12 +52,12 @@ class DubinsTests {
assertTrue(end.equalsFloat(path.c.end))
// Not working, theta double precision inaccuracy
if (path.b is Arc) {
val b = path.b as Arc
if (path.b is ArcSegment) {
val b = path.b as ArcSegment
assertTrue(path.a.end.equalsFloat(b.start))
assertTrue(path.c.start.equalsFloat(b.end))
} else if (path.b is Straight) {
val b = path.b as Straight
} else if (path.b is StraightSegment) {
val b = path.b as StraightSegment
assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
}

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

@ -1,8 +1,9 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.trajectory.radiansToDegrees
import space.kscience.kmath.trajectory.segments.components.Circle
import kotlin.test.Test
import kotlin.test.assertEquals
@ -10,8 +11,8 @@ class ArcTests {
@Test
fun arcTest() {
val circle = Circle(Vector2D(0.0, 0.0), 2.0)
val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
val circle = Circle2D(Vector2D(0.0, 0.0), 2.0)
val arc = ArcSegment.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), ArcSegment.Direction.RIGHT)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.start.theta.radiansToDegrees())
assertEquals(90.0, arc.end.theta.radiansToDegrees())

6
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt → kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/CircleTests.kt
View File

@ -3,9 +3,11 @@
* 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.segments.components
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.trajectory.maxFloatDelta
import kotlin.test.Test
import kotlin.test.assertEquals
@ -17,7 +19,7 @@ class CircleTests {
val center = Vector2D(0.0, 0.0)
val radius = 2.0
val expectedCircumference = 12.56637
val circle = Circle(center, radius)
val circle = Circle2D(center, radius)
assertEquals(expectedCircumference, circle.circumference, maxFloatDelta)
}
}

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

@ -12,21 +12,21 @@ class LineTests {
@Test
fun lineTest() {
val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
assertEquals(45.0, straight.theta.radiansToDegrees())
}
@Test
fun lineAngleTest() {
val zero = Vector2D(0.0, 0.0)
val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
//val zero = Vector2D(0.0, 0.0)
val north = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, 2.0))
assertEquals(0.0, north.theta.radiansToDegrees())
val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
val east = StraightSegment(Euclidean2DSpace.zero, Vector2D(2.0, 0.0))
assertEquals(90.0, east.theta.radiansToDegrees())
val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
val south = StraightSegment(Euclidean2DSpace.zero, Vector2D(0.0, -2.0))
assertEquals(180.0, south.theta.radiansToDegrees())
val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
val west = StraightSegment(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0))
assertEquals(270.0, west.theta.radiansToDegrees())
}
}