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

Arc contains circle, circle direction is computed from poses, factory function can create Arc based on Vector points and provided direction

Browse Source
This commit is contained in:
Erik Schouten 2022-07-17 15:47:05 +02:00
parent 8faa312424
commit 86fce7ec68
4 changed files with 61 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,9 +65,9 @@ public class DubinsPath(
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -87,9 +87,9 @@ public class DubinsPath(
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -97,8 +97,8 @@ public class DubinsPath(
val c1 = start.getRightCircle(turningRadius) val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius)
val s = leftOuterTangent(c1, c2) val s = leftOuterTangent(c1, c2)
val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -106,8 +106,8 @@ public class DubinsPath(
val c1 = start.getLeftCircle(turningRadius) val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val s = rightOuterTangent(c1, c2) val s = rightOuterTangent(c1, c2)
val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -117,8 +117,8 @@ public class DubinsPath(
val s = rightInnerTangent(c1, c2) val s = rightInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -128,8 +128,8 @@ public class DubinsPath(
val s = leftInnerTangent(c1, c2) val s = leftInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
} }

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

@ -1,39 +1,27 @@
package space.kscience.kmath.trajectory.segments package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.dubins.theta import space.kscience.kmath.trajectory.dubins.theta
import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Circle
import space.kscience.kmath.trajectory.segments.components.Pose2D import space.kscience.kmath.trajectory.segments.components.Pose2D
import kotlin.math.PI import kotlin.math.PI
public class Arc( public data class Arc(
center: Vector2D, public val circle: Circle,
a: Vector2D, public val start: Pose2D,
b: Vector2D, public val end: Pose2D
internal val direction: Direction ) : Segment {
) : Circle(center, center.distanceTo(a)), Segment {
private val s1 = Straight(center, a) internal companion object {
private val s2 = Straight(center, b) fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
val s1 = Straight(center, start)
internal val pose1 = calculatePose(a, s1.theta) val s2 = Straight(center, end)
internal val pose2 = calculatePose(b, s2.theta) val pose1 = calculatePose(start, s1.theta, direction)
private val angle = calculateAngle() val pose2 = calculatePose(end, s2.theta, direction)
override val length: Double = calculateLength() return Arc(Circle(center, s1.length), pose1, pose2)
public enum class Direction {
LEFT, RIGHT
} }
private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta) private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
private fun calculateLength(): Double {
val proportion = angle / (2 * PI)
return circumference * proportion
}
private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
Pose2D.of( Pose2D.of(
vector, vector,
when (direction) { when (direction) {
@ -42,3 +30,27 @@ public class Arc(
} }
) )
} }
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 = 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
}
}
}

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

@ -48,18 +48,18 @@ class DubinsTests {
println("${it.key}: ${path.length}") println("${it.key}: ${path.length}")
assertTrue(it.value.equalFloat(path.length)) assertTrue(it.value.equalFloat(path.length))
assertTrue(start.equalsFloat(path.a.pose1)) assertTrue(start.equalsFloat(path.a.start))
assertTrue(end.equalsFloat(path.c.pose2)) assertTrue(end.equalsFloat(path.c.end))
// Not working, theta double precision inaccuracy // Not working, theta double precision inaccuracy
if (path.b is Arc) { if (path.b is Arc) {
val b = path.b as Arc val b = path.b as Arc
assertTrue(path.a.pose2.equalsFloat(b.pose1)) assertTrue(path.a.end.equalsFloat(b.start))
assertTrue(path.c.pose1.equalsFloat(b.pose2)) assertTrue(path.c.start.equalsFloat(b.end))
} else if (path.b is Straight) { } else if (path.b is Straight) {
val b = path.b as Straight val b = path.b as Straight
assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta))) assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta))) assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
} }
} }
} }

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

@ -11,9 +11,9 @@ class ArcTests {
@Test @Test
fun arcTest() { fun arcTest() {
val circle = Circle(Vector2D(0.0, 0.0), 2.0) val circle = Circle(Vector2D(0.0, 0.0), 2.0)
val arc = Arc(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT) val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
assertEquals(circle.circumference / 4, arc.length, 1.0) assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) assertEquals(0.0, arc.start.theta.radiansToDegrees())
assertEquals(90.0, arc.pose2.theta.radiansToDegrees()) assertEquals(90.0, arc.end.theta.radiansToDegrees())
} }
} }