[WIP] full rewrite of obstacle avoidance
This commit is contained in:
parent
b06fc5c87a
commit
8bc1987acf
@ -0,0 +1,49 @@
|
||||
package space.kscience.kmath.geometry
|
||||
|
||||
import space.kscience.kmath.operations.DoubleField.pow
|
||||
import kotlin.math.sign
|
||||
|
||||
public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
|
||||
Circle2D(vector(x, y), radius = radius.toDouble())
|
||||
|
||||
public fun Euclidean2DSpace.segment(begin: DoubleVector2D, end: DoubleVector2D): LineSegment2D =
|
||||
LineSegment(begin, end)
|
||||
|
||||
public fun Euclidean2DSpace.segment(x1: Number, y1: Number, x2: Number, y2: Number): LineSegment2D =
|
||||
LineSegment(vector(x1, y1), vector(x2, y2))
|
||||
|
||||
public fun Euclidean2DSpace.intersectsOrInside(circle1: Circle2D, circle2: Circle2D): Boolean {
|
||||
val distance = norm(circle2.center - circle1.center)
|
||||
return distance <= circle1.radius + circle2.radius
|
||||
}
|
||||
|
||||
/**
|
||||
* https://mathworld.wolfram.com/Circle-LineIntersection.html
|
||||
*/
|
||||
public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D): Boolean {
|
||||
val begin = segment.begin
|
||||
val end = segment.end
|
||||
val d = begin.distanceTo(end)
|
||||
val det = (begin.x - circle.center.x) * (end.y - circle.center.y) -
|
||||
(end.x - circle.center.x) * (begin.y - circle.center.y)
|
||||
|
||||
val incidence = circle.radius.pow(2) * d.pow(2) - det.pow(2)
|
||||
|
||||
return incidence >= 0
|
||||
}
|
||||
|
||||
public fun Euclidean2DSpace.intersects(circle: Circle2D, segment: LineSegment2D): Boolean =
|
||||
intersects(segment, circle)
|
||||
|
||||
public fun Euclidean2DSpace.intersects(segment1: LineSegment2D, segment2: LineSegment2D): Boolean {
|
||||
infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
|
||||
infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
|
||||
|
||||
return with(Euclidean2DSpace) {
|
||||
(segment2.begin - segment1.begin) crossSign (segment2.end - segment1.begin) !=
|
||||
(segment2.begin - segment1.end) crossSign (segment2.end - segment1.end) &&
|
||||
(segment1.begin - segment2.begin) crossSign (segment1.end - segment2.begin) !=
|
||||
(segment1.begin - segment2.end) crossSign (segment1.end - segment2.end)
|
||||
}
|
||||
}
|
||||
|
||||
@ -0,0 +1,10 @@
|
||||
package space.kscience.kmath.geometry
|
||||
|
||||
import space.kscience.kmath.misc.zipWithNextCircular
|
||||
|
||||
public fun Euclidean2DSpace.polygon(points: List<DoubleVector2D>): Polygon<Double> = object : Polygon<Double> {
|
||||
override val points: List<Vector2D<Double>> get() = points
|
||||
}
|
||||
|
||||
public fun Euclidean2DSpace.intersects(polygon: Polygon<Double>, segment: LineSegment2D): Boolean =
|
||||
polygon.points.zipWithNextCircular { l, r -> segment(l, r) }.any { intersects(it, segment) }
|
||||
@ -74,6 +74,8 @@ public object DubinsPath {
|
||||
|
||||
override fun toString(): String = "${first}${second}${third}"
|
||||
|
||||
public val last: Direction get() = third
|
||||
|
||||
public companion object {
|
||||
public val RLR: Type = Type(R, L, R)
|
||||
public val LRL: Type = Type(L, R, L)
|
||||
|
||||
@ -24,6 +24,11 @@ public interface DubinsPose2D : DoubleVector2D {
|
||||
public val coordinates: DoubleVector2D
|
||||
public val bearing: Angle
|
||||
|
||||
/**
|
||||
* Reverse the direction of this pose to the opposite, keeping other parameters the same
|
||||
*/
|
||||
public fun reversed(): DubinsPose2D
|
||||
|
||||
public companion object {
|
||||
public fun bearingToVector(bearing: Angle): Vector2D<Double> =
|
||||
Euclidean2DSpace.vector(cos(bearing), sin(bearing))
|
||||
@ -35,12 +40,15 @@ public interface DubinsPose2D : DoubleVector2D {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@Serializable
|
||||
public class PhaseVector2D(
|
||||
override val coordinates: DoubleVector2D,
|
||||
public val velocity: DoubleVector2D,
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||
override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
|
||||
|
||||
override fun reversed(): DubinsPose2D = with(Euclidean2DSpace) { PhaseVector2D(coordinates, -velocity) }
|
||||
}
|
||||
|
||||
@Serializable
|
||||
@ -50,7 +58,9 @@ private class DubinsPose2DImpl(
|
||||
override val bearing: Angle,
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||
|
||||
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
|
||||
override fun reversed(): DubinsPose2D = DubinsPose2DImpl(coordinates, bearing.plus(Angle.pi).normalized())
|
||||
|
||||
override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
|
||||
}
|
||||
|
||||
public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
|
||||
@ -69,7 +79,8 @@ public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
|
||||
}
|
||||
}
|
||||
|
||||
public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, bearing)
|
||||
public fun DubinsPose2D(coordinate: DoubleVector2D, bearing: Angle): DubinsPose2D =
|
||||
DubinsPose2DImpl(coordinate, bearing)
|
||||
|
||||
public fun DubinsPose2D(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
|
||||
DubinsPose2D(point, DubinsPose2D.vectorToBearing(direction))
|
||||
|
||||
@ -5,96 +5,109 @@
|
||||
|
||||
package space.kscience.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Angle
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.LineSegment2D
|
||||
import space.kscience.kmath.geometry.Polygon
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.Vector2D
|
||||
import space.kscience.kmath.misc.zipWithNextCircular
|
||||
|
||||
|
||||
public interface Obstacle {
|
||||
public val circles: List<Circle2D>
|
||||
public val center: Vector2D<Double>
|
||||
|
||||
public fun intersects(segment: LineSegment2D): Boolean
|
||||
public val circumvention: CompositeTrajectory2D
|
||||
|
||||
/**
|
||||
* Check if obstacle has intersection with given [Trajectory2D]
|
||||
*/
|
||||
public fun intersects(trajectory: Trajectory2D): Boolean =
|
||||
Euclidean2DSpace.trajectoryIntersects(circumvention, trajectory)
|
||||
|
||||
public fun intersects(circle: Circle2D): Boolean
|
||||
|
||||
public companion object {
|
||||
public fun avoidObstacles(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
vararg obstacles: Obstacle,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||
ObstacleShell(polygon.circles)
|
||||
}
|
||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||
}
|
||||
|
||||
public fun avoidPolygons(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
vararg obstacles: Polygon<Double>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||
ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
|
||||
}
|
||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||
}
|
||||
|
||||
public fun avoidObstacles(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
obstacles: Collection<Obstacle>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||
ObstacleShell(polygon.circles)
|
||||
}
|
||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||
}
|
||||
|
||||
public fun avoidPolygons(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
obstacles: Collection<Polygon<Double>>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
val obstacleShells: List<ObstacleShell> = obstacles.map { polygon ->
|
||||
ObstacleShell(polygon.points.map { Circle2D(it, trajectoryRadius) })
|
||||
}
|
||||
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacleShells)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public fun Obstacle.intersectsTrajectory(trajectory: Trajectory2D): Boolean = when (trajectory) {
|
||||
is CircleTrajectory2D -> intersects(trajectory.circle)
|
||||
is StraightTrajectory2D -> intersects(trajectory)
|
||||
is CompositeTrajectory2D -> trajectory.segments.any { intersectsTrajectory(it) }
|
||||
private class ObstacleImpl(override val circles: List<Circle2D>) : Obstacle {
|
||||
override val center: Vector2D<Double> by lazy {
|
||||
Euclidean2DSpace.vector(
|
||||
circles.sumOf { it.center.x } / circles.size,
|
||||
circles.sumOf { it.center.y } / circles.size
|
||||
)
|
||||
}
|
||||
|
||||
override val circumvention: CompositeTrajectory2D by lazy {
|
||||
with(Euclidean2DSpace) {
|
||||
/**
|
||||
* A closed right-handed circuit minimal path circumvention of an obstacle.
|
||||
* @return null if number of distinct circles in the obstacle is less than
|
||||
*/
|
||||
require(circles.isNotEmpty()) { "Can't create circumvention for an empty obstacle" }
|
||||
|
||||
if (circles.size == 1) {
|
||||
// a circumvention consisting of a single circle, starting on top
|
||||
val circle = circles.first()
|
||||
val top = vector(circle.center.x + circle.radius, circle.center.y)
|
||||
val startEnd = DubinsPose2D(
|
||||
top,
|
||||
Angle.piDiv2
|
||||
)
|
||||
return@lazy CompositeTrajectory2D(
|
||||
CircleTrajectory2D(circle, startEnd, startEnd)
|
||||
)
|
||||
}
|
||||
|
||||
//TODO use convex hull
|
||||
//distinct and sorted in right-handed direction
|
||||
val circles = circles.distinct().sortedBy {
|
||||
(it.center - center).bearing
|
||||
}
|
||||
|
||||
val tangents = circles.zipWithNextCircular { a: Circle2D, b: Circle2D ->
|
||||
tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR]
|
||||
?: error("Can't find right handed circumvention")
|
||||
}
|
||||
|
||||
val trajectory: List<Trajectory2D> = buildList {
|
||||
for (i in 0 until tangents.lastIndex) {
|
||||
add(tangents[i])
|
||||
add(CircleTrajectory2D(circles[i + 1], tangents[i].endPose, tangents[i + 1].beginPose))
|
||||
}
|
||||
add(tangents.last())
|
||||
add(CircleTrajectory2D(circles[0], tangents.last().endPose, tangents.first().beginPose))
|
||||
}
|
||||
|
||||
return@lazy CompositeTrajectory2D(trajectory)
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (this === other) return true
|
||||
if (other == null || this::class != other::class) return false
|
||||
|
||||
other as ObstacleImpl
|
||||
|
||||
return circles == other.circles
|
||||
}
|
||||
|
||||
override fun hashCode(): Int {
|
||||
return circles.hashCode()
|
||||
}
|
||||
|
||||
override fun toString(): String {
|
||||
return "Obstacle(circles=$circles)"
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleShell(listOf(*circles))
|
||||
public fun Obstacle(vararg circles: Circle2D): Obstacle = ObstacleImpl(listOf(*circles))
|
||||
|
||||
public fun Obstacle(points: List<Vector2D<Double>>, radius: Double): Obstacle =
|
||||
ObstacleShell(points.map { Circle2D(it, radius) })
|
||||
|
||||
//public fun Trajectory2D.intersects(
|
||||
// polygon: Polygon<Double>,
|
||||
// radius: Double,
|
||||
//): Boolean {
|
||||
// val obstacle = Obstacle(polygon.points.map { point -> Circle2D(point, radius) })
|
||||
// return when (this) {
|
||||
// is CircleTrajectory2D -> {
|
||||
// val nearestCircle = obstacle.circles.minBy { it.center.distanceTo(circle.center) }
|
||||
//
|
||||
// }
|
||||
// is StraightTrajectory2D -> obstacle.intersects(this)
|
||||
// is CompositeTrajectory2D -> segments.any { it.intersects(polygon, radius) }
|
||||
// }
|
||||
//}
|
||||
ObstacleImpl(points.map { Circle2D(it, radius) })
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,389 @@
|
||||
package space.kscience.trajectory
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//private class LR<T>(val l: T, val r: T) {
|
||||
// operator fun get(direction: Trajectory2D.Direction) = when (direction) {
|
||||
// Trajectory2D.L -> l
|
||||
// Trajectory2D.R -> r
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//private class TangentPath(val tangents: List<ObstacleTangent>) {
|
||||
// fun last() = tangents.last()
|
||||
//}
|
||||
//
|
||||
//private fun TangentPath(vararg tangents: ObstacleTangent) = TangentPath(listOf(*tangents))
|
||||
//
|
||||
//private fun Circle2D.isInside(other: Circle2D): Boolean {
|
||||
// return center.distanceTo(other.center) + radius <= other.radius
|
||||
//}
|
||||
//
|
||||
//
|
||||
//internal class ObstacleShell(
|
||||
// nodes: List<Circle2D>,
|
||||
//) : Obstacle {
|
||||
// override val circles: List<Circle2D>
|
||||
// override val center: Vector2D<Double>
|
||||
// private val shell: List<LineSegment2D>
|
||||
// private val shellDirection: Trajectory2D.Direction
|
||||
//
|
||||
// init {
|
||||
// this.center = Euclidean2DSpace.vector(
|
||||
// nodes.sumOf { it.center.x } / nodes.size,
|
||||
// nodes.sumOf { it.center.y } / nodes.size
|
||||
// )
|
||||
//
|
||||
//// this.circles = nodes.filter { node ->
|
||||
//// //filter nodes inside other nodes
|
||||
//// nodes.none{ node !== it && node.isInside(it) }
|
||||
//// }
|
||||
//
|
||||
// this.circles = nodes.distinct()
|
||||
//
|
||||
// if (nodes.size < 2) {
|
||||
// shell = emptyList()
|
||||
// shellDirection = Trajectory2D.R
|
||||
// } else {
|
||||
//
|
||||
// //ignore cases when one circle is inside another one
|
||||
// val lslTangents = circles.zipWithNextCircular { a, b ->
|
||||
// tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL] ?: error("Intersecting circles")
|
||||
// }
|
||||
//
|
||||
// val rsrTangents = circles.zipWithNextCircular { a, b ->
|
||||
// tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] ?: error("Intersecting circles")
|
||||
// }
|
||||
//
|
||||
//
|
||||
// val lslToCenter = lslTangents.sumOf { it.begin.distanceTo(center) } +
|
||||
// lslTangents.sumOf { it.end.distanceTo(center) }
|
||||
// val rsrToCenter = rsrTangents.sumOf { it.begin.distanceTo(center) } +
|
||||
// rsrTangents.sumOf { it.end.distanceTo(center) }
|
||||
//
|
||||
// if (rsrToCenter >= lslToCenter) {
|
||||
// this.shell = rsrTangents
|
||||
// this.shellDirection = Trajectory2D.R
|
||||
// } else {
|
||||
// this.shell = lslTangents
|
||||
// this.shellDirection = Trajectory2D.L
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// constructor(obstacle: Obstacle) : this(obstacle.circles)
|
||||
//
|
||||
// /**
|
||||
// * Check if segment has any intersections with this obstacle
|
||||
// */
|
||||
// override fun intersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) {
|
||||
// shell.any { tangent -> intersects(segment, tangent) }
|
||||
// || circles.any { circle -> intersects(segment, circle) }
|
||||
// }
|
||||
//
|
||||
// internal fun innerIntersects(segment: LineSegment2D): Boolean = with(Euclidean2DSpace) {
|
||||
// intersects(polygon(circles.map { it.center }), segment)
|
||||
// }
|
||||
//
|
||||
// override fun intersects(circle: Circle2D): Boolean = with(Euclidean2DSpace) {
|
||||
// shell.any { tangent -> intersects(tangent, circle) }
|
||||
// || circles.any { c2 -> intersectsOrInside(circle, c2) }
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Tangent to next obstacle node in given direction
|
||||
// */
|
||||
// fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent {
|
||||
// if (circleIndex == -1) error("Circle does not belong to this tangent")
|
||||
//
|
||||
// val nextCircleIndex = if (direction == this.shellDirection) {
|
||||
// if (circleIndex == circles.lastIndex) 0 else circleIndex + 1
|
||||
// } else {
|
||||
// if (circleIndex == 0) circles.lastIndex else circleIndex - 1
|
||||
// }
|
||||
//
|
||||
// return ObstacleTangent(
|
||||
// LineSegment(
|
||||
// shell[nextCircleIndex].end,
|
||||
// shell[nextCircleIndex].begin
|
||||
// ),
|
||||
// ObstacleConnection(this, circleIndex, direction),
|
||||
// ObstacleConnection(this, nextCircleIndex, direction),
|
||||
// )
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * All tangents in given direction
|
||||
// */
|
||||
// internal fun tangentsAlong(
|
||||
// initialCircleIndex: Int,
|
||||
// finalCircleIndex: Int,
|
||||
// direction: Trajectory2D.Direction,
|
||||
// ): List<ObstacleTangent> {
|
||||
// return buildList {
|
||||
// var currentIndex = initialCircleIndex
|
||||
// do {
|
||||
// val tangent = nextTangent(currentIndex, direction)
|
||||
// add(tangent)
|
||||
// currentIndex = tangent.endNode.nodeIndex
|
||||
// } while (currentIndex != finalCircleIndex)
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// override fun equals(other: Any?): Boolean {
|
||||
// if (other == null || other !is ObstacleShell) return false
|
||||
// return circles == other.circles
|
||||
// }
|
||||
//
|
||||
// override fun hashCode(): Int {
|
||||
// return circles.hashCode()
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleShell(listOf(*circles))
|
||||
//
|
||||
//
|
||||
//private fun arcLength(
|
||||
// circle: Circle2D,
|
||||
// point1: DoubleVector2D,
|
||||
// point2: DoubleVector2D,
|
||||
// direction: Trajectory2D.Direction,
|
||||
//): Double {
|
||||
// val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
|
||||
// val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
|
||||
// var angle = 0.0
|
||||
// when (direction) {
|
||||
// Trajectory2D.L -> {
|
||||
// angle = if (phi2 >= phi1) {
|
||||
// phi2 - phi1
|
||||
// } else {
|
||||
// 2 * PI + phi2 - phi1
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// Trajectory2D.R -> {
|
||||
// angle = if (phi2 >= phi1) {
|
||||
// 2 * PI - (phi2 - phi1)
|
||||
// } else {
|
||||
// -(phi2 - phi1)
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// return circle.radius * angle
|
||||
//}
|
||||
//
|
||||
//private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
|
||||
// return Pair(
|
||||
// r * Euclidean2DSpace.vector(v.y / Euclidean2DSpace.norm(v), -v.x / Euclidean2DSpace.norm(v)),
|
||||
// r * Euclidean2DSpace.vector(-v.y / Euclidean2DSpace.norm(v), v.x / Euclidean2DSpace.norm(v))
|
||||
// )
|
||||
//}
|
||||
//
|
||||
//
|
||||
//private fun constructTangentCircles(
|
||||
// point: DoubleVector2D,
|
||||
// direction: DoubleVector2D,
|
||||
// r: Double,
|
||||
//): LR<Circle2D> {
|
||||
// val center1 = point + normalVectors(direction, r).first
|
||||
// val center2 = point + normalVectors(direction, r).second
|
||||
// val p1 = center1 - point
|
||||
// return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
|
||||
// LR(
|
||||
// Circle2D(center1, r),
|
||||
// Circle2D(center2, r)
|
||||
// )
|
||||
// } else {
|
||||
// LR(
|
||||
// Circle2D(center2, r),
|
||||
// Circle2D(center1, r)
|
||||
// )
|
||||
// }
|
||||
//}
|
||||
//
|
||||
//private fun sortedObstacles(
|
||||
// currentObstacle: Obstacle,
|
||||
// obstacles: List<Obstacle>,
|
||||
//): List<Obstacle> {
|
||||
// return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) }
|
||||
//}
|
||||
//
|
||||
///**
|
||||
// * Check if all proposed paths have ended at [finalObstacle]
|
||||
// */
|
||||
//private fun allFinished(
|
||||
// paths: List<TangentPath>,
|
||||
// finalObstacle: Obstacle,
|
||||
//): Boolean = paths.all { it.last().endNode.obstacle === finalObstacle }
|
||||
//
|
||||
//private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
|
||||
//
|
||||
//
|
||||
//private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
|
||||
// buildList {
|
||||
// tangents.zipWithNext().forEach { (left, right: ObstacleTangent) ->
|
||||
// add(left.lineSegment.toTrajectory())
|
||||
// add(
|
||||
// CircleTrajectory2D.of(
|
||||
// right.startCircle.center,
|
||||
// left.lineSegment.end,
|
||||
// right.lineSegment.begin,
|
||||
// right.startDirection
|
||||
// )
|
||||
// )
|
||||
// }
|
||||
//
|
||||
// add(tangents.last().lineSegment.toTrajectory())
|
||||
// }
|
||||
//)
|
||||
//
|
||||
//internal fun findAllPaths(
|
||||
// start: DubinsPose2D,
|
||||
// startingRadius: Double,
|
||||
// finish: DubinsPose2D,
|
||||
// finalRadius: Double,
|
||||
// obstacles: List<ObstacleShell>,
|
||||
//): List<CompositeTrajectory2D> {
|
||||
// fun DubinsPose2D.direction() = Euclidean2DSpace.vector(cos(bearing), sin(bearing))
|
||||
//
|
||||
// // two circles for the initial point
|
||||
// val initialCircles = constructTangentCircles(
|
||||
// start,
|
||||
// start.direction(),
|
||||
// startingRadius
|
||||
// )
|
||||
//
|
||||
// //two circles for the final point
|
||||
// val finalCircles = constructTangentCircles(
|
||||
// finish,
|
||||
// finish.direction(),
|
||||
// finalRadius
|
||||
// )
|
||||
//
|
||||
// //all valid trajectories
|
||||
// val trajectories = mutableListOf<CompositeTrajectory2D>()
|
||||
//
|
||||
// for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
|
||||
// for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
|
||||
// //Using obstacle to minimize code bloat
|
||||
// val initialObstacle = ObstacleShell(initialCircles[i])
|
||||
// val finalObstacle = ObstacleShell(finalCircles[j])
|
||||
//
|
||||
// var currentPaths: List<TangentPath> = listOf(
|
||||
// TangentPath(
|
||||
// //We need only the direction of the final segment from this
|
||||
// ObstacleTangent(
|
||||
// LineSegment(start, start),
|
||||
// ObstacleConnection(initialObstacle, 0, i),
|
||||
// ObstacleConnection(initialObstacle, 0, i),
|
||||
// )
|
||||
// )
|
||||
// )
|
||||
// while (!allFinished(currentPaths, finalObstacle)) {
|
||||
// // paths after next obstacle iteration
|
||||
// val newPaths = mutableListOf<TangentPath>()
|
||||
// // for each path propagate it one obstacle further
|
||||
// for (tangentPath: TangentPath in currentPaths) {
|
||||
// val currentNode = tangentPath.last().endNode
|
||||
// val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
// val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle)
|
||||
//
|
||||
// // If path is finished, ignore it
|
||||
// // TODO avoid returning to ignored obstacle on the next cycle
|
||||
// if (currentObstacle == finalObstacle) {
|
||||
// newPaths.add(tangentPath)
|
||||
// } else {
|
||||
// val tangentToFinal: ObstacleTangent =
|
||||
// outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
// currentDirection,
|
||||
// Trajectory2D.S,
|
||||
// j
|
||||
// )] ?: break
|
||||
//
|
||||
// // searching for the nearest obstacle that intersects with the direct path
|
||||
// val nextObstacle = obstacles.filter { obstacle ->
|
||||
// obstacle.intersects(tangentToFinal)
|
||||
// }.minByOrNull { currentObstacle.center.distanceTo(it.center) } ?: finalObstacle
|
||||
//
|
||||
// //TODO add break check for end of path
|
||||
//
|
||||
// // All valid tangents from current obstacle to the next one
|
||||
// val nextTangents: Collection<ObstacleTangent> = outerTangents(
|
||||
// currentObstacle,
|
||||
// nextObstacle
|
||||
// ).filter { (key, tangent) ->
|
||||
//// obstacles.none { obstacle ->
|
||||
//// obstacle === currentObstacle
|
||||
//// || obstacle === nextObstacle
|
||||
//// || obstacle.innerIntersects(tangent)
|
||||
//// } && // does not intersect other obstacles
|
||||
// key.first == currentDirection && // initial direction is the same as end of previous segment direction
|
||||
// (nextObstacle != finalObstacle || key.third == j) // if it is the last, it should be the same as the one we are searching for
|
||||
// }.values
|
||||
//
|
||||
// for (tangent in nextTangents) {
|
||||
// val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) {
|
||||
// //if the previous segment last circle is the same as first circle of the next segment
|
||||
//
|
||||
// //If obstacle consists of single circle, do not walk around
|
||||
// if (currentObstacle.circles.size < 2) {
|
||||
// emptyList()
|
||||
// } else {
|
||||
// val lengthMaxPossible = arcLength(
|
||||
// tangent.startCircle,
|
||||
// tangentPath.last().lineSegment.end,
|
||||
// currentObstacle.nextTangent(
|
||||
// tangent.beginNode.nodeIndex,
|
||||
// currentDirection
|
||||
// ).lineSegment.begin,
|
||||
// currentDirection
|
||||
// )
|
||||
//
|
||||
// val lengthCalculated = arcLength(
|
||||
// tangent.startCircle,
|
||||
// tangentPath.last().lineSegment.end,
|
||||
// tangent.lineSegment.begin,
|
||||
// currentDirection
|
||||
// )
|
||||
// // ensure that path does not go inside the obstacle
|
||||
// if (lengthCalculated > lengthMaxPossible) {
|
||||
// currentObstacle.tangentsAlong(
|
||||
// currentNode.nodeIndex,
|
||||
// tangent.beginNode.nodeIndex,
|
||||
// currentDirection,
|
||||
// )
|
||||
// } else {
|
||||
// emptyList()
|
||||
// }
|
||||
// }
|
||||
// } else {
|
||||
// currentObstacle.tangentsAlong(
|
||||
// currentNode.nodeIndex,
|
||||
// tangent.beginNode.nodeIndex,
|
||||
// currentDirection,
|
||||
// )
|
||||
// }
|
||||
// newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// currentPaths = newPaths
|
||||
// }
|
||||
//
|
||||
// trajectories += currentPaths.map { tangentPath ->
|
||||
//// val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
// val end = Obstacle(finalCircles[j])
|
||||
// TangentPath(
|
||||
// tangentPath.tangents +
|
||||
// ObstacleTangent(
|
||||
// LineSegment(finish, finish),
|
||||
// ObstacleConnection(end, 0, j),
|
||||
// ObstacleConnection(end, 0, j)
|
||||
// )
|
||||
// )
|
||||
// }.map { it.toTrajectory() }
|
||||
// }
|
||||
// }
|
||||
// return trajectories
|
||||
//}
|
||||
@ -0,0 +1,363 @@
|
||||
package space.kscience.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import kotlin.collections.component1
|
||||
import kotlin.collections.component2
|
||||
import kotlin.math.PI
|
||||
import kotlin.math.atan2
|
||||
|
||||
|
||||
public class Obstacles(public val obstacles: List<Obstacle>) {
|
||||
|
||||
private inner class ObstacleConnection(
|
||||
val obstacleIndex: Int,
|
||||
val nodeIndex: Int,
|
||||
val direction: Trajectory2D.Direction,
|
||||
) {
|
||||
val obstacle: Obstacle get() = obstacles[obstacleIndex]
|
||||
val circle: Circle2D get() = obstacle.circles[nodeIndex]
|
||||
}
|
||||
|
||||
private inner class ObstacleTangent(
|
||||
val tangentTrajectory: StraightTrajectory2D,
|
||||
val from: ObstacleConnection?,
|
||||
val to: ObstacleConnection?,
|
||||
) {
|
||||
/**
|
||||
* If false this tangent intersects another obstacle
|
||||
*/
|
||||
val isValid by lazy {
|
||||
obstacles.indices.none {
|
||||
it != from?.obstacleIndex && it != to?.obstacleIndex && obstacles[it].intersects(tangentTrajectory)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* All tangents between two obstacles
|
||||
*
|
||||
* In general generates 4 paths.
|
||||
* TODO check intersections.
|
||||
*/
|
||||
private fun tangentsBetween(
|
||||
firstIndex: Int,
|
||||
secondIndex: Int,
|
||||
): Map<DubinsPath.Type, ObstacleTangent> = with(Euclidean2DSpace) {
|
||||
val first = obstacles[firstIndex]
|
||||
val second = obstacles[secondIndex]
|
||||
val firstPolygon = polygon(first.circles.map { it.center })
|
||||
val secondPolygon = polygon(second.circles.map { it.center })
|
||||
buildMap {
|
||||
for (firstCircleIndex in first.circles.indices) {
|
||||
val firstCircle = first.circles[firstCircleIndex]
|
||||
for (secondCircleIndex in second.circles.indices) {
|
||||
val secondCircle = second.circles[secondCircleIndex]
|
||||
for ((pathType, segment) in tangentsBetweenCircles(
|
||||
firstCircle,
|
||||
secondCircle
|
||||
)) {
|
||||
if (!intersects(firstPolygon, segment) && !intersects(secondPolygon, segment)) {
|
||||
put(
|
||||
pathType,
|
||||
ObstacleTangent(
|
||||
segment,
|
||||
ObstacleConnection(firstIndex, firstCircleIndex, pathType.first),
|
||||
ObstacleConnection(secondIndex, secondCircleIndex, pathType.third)
|
||||
)
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private fun tangentsFromCircle(
|
||||
circle: Circle2D,
|
||||
direction: Trajectory2D.Direction,
|
||||
obstacleIndex: Int,
|
||||
): Map<DubinsPath.Type, ObstacleTangent> = with(Euclidean2DSpace) {
|
||||
val obstacle = obstacles[obstacleIndex]
|
||||
val polygon = polygon(obstacle.circles.map { it.center })
|
||||
buildMap {
|
||||
for (circleIndex in obstacle.circles.indices) {
|
||||
val obstacleCircle = obstacle.circles[circleIndex]
|
||||
for ((pathType, segment) in tangentsBetweenCircles(
|
||||
circle,
|
||||
obstacleCircle
|
||||
)) {
|
||||
if (pathType.first == direction && !intersects(polygon, segment)) {
|
||||
put(
|
||||
pathType,
|
||||
ObstacleTangent(
|
||||
segment,
|
||||
null,
|
||||
ObstacleConnection(obstacleIndex, circleIndex, pathType.third)
|
||||
)
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private fun tangentToCircle(
|
||||
obstacleIndex: Int,
|
||||
obstacleDirection: Trajectory2D.Direction,
|
||||
circle: Circle2D,
|
||||
direction: Trajectory2D.Direction,
|
||||
): ObstacleTangent? = with(Euclidean2DSpace) {
|
||||
val obstacle = obstacles[obstacleIndex]
|
||||
val polygon = polygon(obstacle.circles.map { it.center })
|
||||
for (circleIndex in obstacle.circles.indices) {
|
||||
val obstacleCircle = obstacle.circles[circleIndex]
|
||||
tangentsBetweenCircles(
|
||||
obstacleCircle,
|
||||
circle
|
||||
).get(DubinsPath.Type(obstacleDirection, Trajectory2D.S, direction))?.takeIf {
|
||||
!intersects(polygon, it)
|
||||
}?.let {
|
||||
return ObstacleTangent(
|
||||
it,
|
||||
ObstacleConnection(obstacleIndex, circleIndex, obstacleDirection),
|
||||
null,
|
||||
)
|
||||
}
|
||||
}
|
||||
return null
|
||||
}
|
||||
|
||||
|
||||
private val tangentsCache = hashMapOf<Pair<Int, Int>, Map<DubinsPath.Type, ObstacleTangent>>()
|
||||
|
||||
private fun getAllTangents(i: Int, j: Int): Map<DubinsPath.Type, ObstacleTangent> =
|
||||
tangentsCache.getOrPut(i to j) {
|
||||
tangentsBetween(i, j)
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Circumvention trajectory alongside obstacle. Replacing first and last arcs with appropriate cuts
|
||||
*/
|
||||
private fun trajectoryBetween(tangent1: ObstacleTangent, tangent2: ObstacleTangent): CompositeTrajectory2D {
|
||||
require(tangent1.to != null)
|
||||
require(tangent2.from != null)
|
||||
|
||||
require(tangent1.to.obstacleIndex == tangent2.from.obstacleIndex)
|
||||
require(tangent1.to.direction == tangent2.from.direction)
|
||||
|
||||
val circumvention = tangent1.to.obstacle.circumvention(
|
||||
tangent1.to.direction,
|
||||
tangent1.to.nodeIndex,
|
||||
tangent2.from.nodeIndex
|
||||
).segments.toMutableList()
|
||||
|
||||
//cutting first and last arcs to accommodate connection points
|
||||
val first = circumvention.first() as CircleTrajectory2D
|
||||
val last = circumvention.last() as CircleTrajectory2D
|
||||
circumvention[0] = CircleTrajectory2D(
|
||||
first.circle,
|
||||
tangent1.tangentTrajectory.endPose,
|
||||
first.end
|
||||
)
|
||||
circumvention[circumvention.lastIndex] = CircleTrajectory2D(
|
||||
last.circle,
|
||||
last.begin,
|
||||
tangent2.tangentTrajectory.beginPose
|
||||
)
|
||||
return CompositeTrajectory2D(circumvention)
|
||||
}
|
||||
|
||||
private inner class TangentPath(val tangents: List<ObstacleTangent>) {
|
||||
val isFinished get() = tangents.last().to == null
|
||||
|
||||
fun toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
|
||||
buildList<Trajectory2D> {
|
||||
add(tangents.first().tangentTrajectory)
|
||||
tangents.zipWithNext().forEach { (l, r) ->
|
||||
addAll(trajectoryBetween(l, r).segments)
|
||||
add(r.tangentTrajectory)
|
||||
}
|
||||
}
|
||||
)
|
||||
}
|
||||
|
||||
public fun allTrajectoriesAvoiding(
|
||||
startCircle: Circle2D,
|
||||
startDirection: Trajectory2D.Direction,
|
||||
endCircle: Circle2D,
|
||||
endDirection: Trajectory2D.Direction,
|
||||
): Collection<Trajectory2D> {
|
||||
val directTangent: StraightTrajectory2D = tangentsBetweenCircles(startCircle, endCircle).get(
|
||||
DubinsPath.Type(startDirection, Trajectory2D.S, endDirection)
|
||||
) ?: return emptySet()
|
||||
|
||||
//fast return if no obstacles intersect direct path
|
||||
if (obstacles.none { it.intersects(directTangent) }) return listOf(directTangent)
|
||||
|
||||
/**
|
||||
* Continue current tangent to final point or to the next obstacle
|
||||
*/
|
||||
fun TangentPath.nextSteps(): Collection<TangentPath> {
|
||||
val connection = tangents.last().to
|
||||
require(connection != null)
|
||||
|
||||
//indices of obstacles that are not on previous path
|
||||
val remainingObstacleIndices = obstacles.indices - tangents.mapNotNull { it.to?.obstacleIndex }
|
||||
|
||||
//a tangent to end point, null if tangent could not be constructed
|
||||
val tangentToEnd: ObstacleTangent = tangentToCircle(
|
||||
connection.obstacleIndex,
|
||||
connection.direction,
|
||||
endCircle,
|
||||
endDirection
|
||||
) ?: return emptySet()
|
||||
|
||||
if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
|
||||
TangentPath(tangents + tangentToEnd)
|
||||
)
|
||||
|
||||
// tangents to other obstacles
|
||||
return remainingObstacleIndices.sortedWith(
|
||||
compareByDescending<Int> { obstacles[it].intersects(tangentToEnd.tangentTrajectory) } //take intersecting obstacles
|
||||
.thenBy { connection.circle.center.distanceTo(obstacles[it].center) } //then nearest
|
||||
).firstNotNullOf { nextObstacleIndex ->
|
||||
//all tangents from cache
|
||||
getAllTangents(connection.obstacleIndex, nextObstacleIndex).filter {
|
||||
//filtered by direction
|
||||
it.key.first == connection.direction
|
||||
}.values.takeIf { it.isNotEmpty() } // skip if empty
|
||||
}.map {
|
||||
TangentPath(tangents + it)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//find nearest obstacle that has valid tangents to
|
||||
val tangentsToFirstObstacle: Collection<ObstacleTangent> = obstacles.indices.sortedWith(
|
||||
compareByDescending<Int> { obstacles[it].intersects(directTangent) } //take intersecting obstacles
|
||||
.thenBy { startCircle.center.distanceTo(obstacles[it].center) } //then nearest
|
||||
).firstNotNullOf { obstacleIndex ->
|
||||
tangentsFromCircle(startCircle, startDirection, obstacleIndex).values
|
||||
.filter { it.isValid }.takeIf { it.isNotEmpty() }
|
||||
}
|
||||
|
||||
var paths = tangentsToFirstObstacle.map { TangentPath(listOf(it)) }
|
||||
|
||||
while (!paths.all { it.isFinished }) {
|
||||
paths = paths.flatMap { it.nextSteps() }
|
||||
}
|
||||
return paths.map { it.toTrajectory() }
|
||||
}
|
||||
|
||||
|
||||
public companion object {
|
||||
private data class LR<T>(val l: T, val r: T) {
|
||||
operator fun get(direction: Trajectory2D.Direction) = when (direction) {
|
||||
Trajectory2D.L -> l
|
||||
Trajectory2D.R -> r
|
||||
}
|
||||
}
|
||||
|
||||
private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> =
|
||||
with(Euclidean2DSpace) {
|
||||
Pair(
|
||||
r * vector(v.y / norm(v), -v.x / norm(v)),
|
||||
r * vector(-v.y / norm(v), v.x / norm(v))
|
||||
)
|
||||
}
|
||||
|
||||
private fun constructTangentCircles(
|
||||
pose: DubinsPose2D,
|
||||
r: Double,
|
||||
): LR<Circle2D> = with(Euclidean2DSpace) {
|
||||
val direction = DubinsPose2D.bearingToVector(pose.bearing)
|
||||
//TODO optimize to use bearing
|
||||
val center1 = pose + normalVectors(direction, r).first
|
||||
val center2 = pose + normalVectors(direction, r).second
|
||||
val p1 = center1 - pose
|
||||
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
|
||||
LR(
|
||||
Circle2D(center1, r),
|
||||
Circle2D(center2, r)
|
||||
)
|
||||
} else {
|
||||
LR(
|
||||
Circle2D(center2, r),
|
||||
Circle2D(center1, r)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
public fun avoidObstacles(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
startingRadius: Double,
|
||||
obstacleList: List<Obstacle>,
|
||||
finalRadius: Double = startingRadius,
|
||||
): List<Trajectory2D> {
|
||||
val obstacles = Obstacles(obstacleList)
|
||||
val initialCircles = constructTangentCircles(
|
||||
start,
|
||||
startingRadius
|
||||
)
|
||||
|
||||
//two circles for the final point
|
||||
val finalCircles = constructTangentCircles(
|
||||
finish,
|
||||
finalRadius
|
||||
)
|
||||
val lr = listOf(Trajectory2D.L, Trajectory2D.R)
|
||||
return buildList {
|
||||
lr.forEach { beginDirection ->
|
||||
lr.forEach { endDirection ->
|
||||
addAll(
|
||||
obstacles.allTrajectoriesAvoiding(
|
||||
initialCircles[beginDirection],
|
||||
beginDirection,
|
||||
finalCircles[endDirection],
|
||||
endDirection
|
||||
)
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public fun avoidObstacles(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
vararg obstacles: Obstacle,
|
||||
): List<Trajectory2D> = avoidObstacles(start, finish, trajectoryRadius, obstacles.toList())
|
||||
|
||||
public fun avoidPolygons(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
vararg polygons: Polygon<Double>,
|
||||
): List<Trajectory2D> {
|
||||
val obstacles: List<Obstacle> = polygons.map { polygon ->
|
||||
Obstacle(polygon.points, trajectoryRadius)
|
||||
}
|
||||
return avoidObstacles(start, finish, trajectoryRadius, obstacles)
|
||||
}
|
||||
|
||||
|
||||
public fun avoidPolygons(
|
||||
start: DubinsPose2D,
|
||||
finish: DubinsPose2D,
|
||||
trajectoryRadius: Double,
|
||||
polygons: Collection<Polygon<Double>>,
|
||||
): List<Trajectory2D> {
|
||||
val obstacles: List<Obstacle> = polygons.map { polygon ->
|
||||
Obstacle(polygon.points, trajectoryRadius)
|
||||
}
|
||||
return avoidObstacles(start, finish, trajectoryRadius, obstacles)
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -11,16 +11,24 @@ import kotlinx.serialization.Serializable
|
||||
import kotlinx.serialization.UseSerializers
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.minus
|
||||
import kotlin.math.atan2
|
||||
|
||||
@Serializable
|
||||
public sealed interface Trajectory2D {
|
||||
public val length: Double
|
||||
|
||||
public val beginPose: DubinsPose2D
|
||||
public val endPose: DubinsPose2D
|
||||
|
||||
/**
|
||||
* Produce a trajectory with reversed order of points
|
||||
*/
|
||||
public fun reversed(): Trajectory2D
|
||||
|
||||
public sealed interface Type
|
||||
|
||||
public sealed interface Direction: Type
|
||||
public sealed interface Direction : Type
|
||||
|
||||
public object R : Direction {
|
||||
override fun toString(): String = "R"
|
||||
@ -35,6 +43,9 @@ public sealed interface Trajectory2D {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public val DoubleVector2D.bearing: Angle get() = (atan2(x, y).radians).normalized()
|
||||
|
||||
/**
|
||||
* Straight path segment. The order of start and end defines the direction
|
||||
*/
|
||||
@ -47,49 +58,60 @@ public data class StraightTrajectory2D(
|
||||
|
||||
override val length: Double get() = begin.distanceTo(end)
|
||||
|
||||
public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized()
|
||||
public val bearing: Angle get() = (end - begin).bearing
|
||||
|
||||
override val beginPose: DubinsPose2D get() = DubinsPose2D(begin, bearing)
|
||||
override val endPose: DubinsPose2D get() = DubinsPose2D(end, bearing)
|
||||
|
||||
override fun reversed(): StraightTrajectory2D = StraightTrajectory2D(end, begin)
|
||||
}
|
||||
|
||||
public fun StraightTrajectory2D(segment: LineSegment2D): StraightTrajectory2D =
|
||||
StraightTrajectory2D(segment.begin, segment.end)
|
||||
|
||||
/**
|
||||
* An arc segment
|
||||
*/
|
||||
@Serializable
|
||||
@SerialName("arc")
|
||||
public data class CircleTrajectory2D(
|
||||
public data class CircleTrajectory2D (
|
||||
public val circle: Circle2D,
|
||||
public val start: DubinsPose2D,
|
||||
public val begin: DubinsPose2D,
|
||||
public val end: DubinsPose2D,
|
||||
) : Trajectory2D {
|
||||
|
||||
override val beginPose: DubinsPose2D get() = begin
|
||||
override val endPose: DubinsPose2D get() = end
|
||||
|
||||
/**
|
||||
* Arc length in radians
|
||||
*/
|
||||
val arcLength: Angle
|
||||
val arcAngle: Angle
|
||||
get() = if (direction == Trajectory2D.L) {
|
||||
start.bearing - end.bearing
|
||||
begin.bearing - end.bearing
|
||||
} else {
|
||||
end.bearing - start.bearing
|
||||
end.bearing - begin.bearing
|
||||
}.normalized()
|
||||
|
||||
|
||||
override val length: Double by lazy {
|
||||
circle.radius * arcLength.radians
|
||||
circle.radius * arcAngle.radians
|
||||
}
|
||||
|
||||
public val direction: Trajectory2D.Direction by lazy {
|
||||
if (start.y < circle.center.y) {
|
||||
if (start.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
} else if (start.y > circle.center.y) {
|
||||
if (start.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
when {
|
||||
begin.y < circle.center.y -> if (begin.bearing > Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
begin.y > circle.center.y -> if (begin.bearing < Angle.pi) Trajectory2D.R else Trajectory2D.L
|
||||
else -> if (begin.bearing == Angle.zero) {
|
||||
if (begin.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
} else {
|
||||
if (start.bearing == Angle.zero) {
|
||||
if (start.x < circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
} else {
|
||||
if (start.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
if (begin.x > circle.center.x) Trajectory2D.R else Trajectory2D.L
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
override fun reversed(): CircleTrajectory2D = CircleTrajectory2D(circle, end.reversed(), begin.reversed())
|
||||
|
||||
public companion object {
|
||||
public fun of(
|
||||
center: DoubleVector2D,
|
||||
@ -124,8 +146,28 @@ public data class CircleTrajectory2D(
|
||||
@SerialName("composite")
|
||||
public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Trajectory2D {
|
||||
override val length: Double get() = segments.sumOf { it.length }
|
||||
|
||||
override val beginPose: DubinsPose2D get() = segments.first().beginPose
|
||||
override val endPose: DubinsPose2D get() = segments.last().endPose
|
||||
|
||||
override fun reversed(): CompositeTrajectory2D = CompositeTrajectory2D(segments.map { it.reversed() }.reversed())
|
||||
}
|
||||
|
||||
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
|
||||
CompositeTrajectory2D(segments.toList())
|
||||
|
||||
public fun Euclidean2DSpace.trajectoryIntersects(a: Trajectory2D, b: Trajectory2D): Boolean = when (a) {
|
||||
is CircleTrajectory2D -> when (b) {
|
||||
is CircleTrajectory2D -> intersectsOrInside(a.circle, b.circle)
|
||||
is StraightTrajectory2D -> intersects(a.circle, b)
|
||||
is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) }
|
||||
}
|
||||
|
||||
is StraightTrajectory2D -> when (b) {
|
||||
is CircleTrajectory2D -> intersects(a, b.circle)
|
||||
is StraightTrajectory2D -> intersects(a, b)
|
||||
is CompositeTrajectory2D -> b.segments.any { trajectoryIntersects(it, b) }
|
||||
}
|
||||
|
||||
is CompositeTrajectory2D -> a.segments.any { trajectoryIntersects(it, b) }
|
||||
}
|
||||
|
||||
@ -0,0 +1,120 @@
|
||||
package space.kscience.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.trajectory.DubinsPath.Type
|
||||
import kotlin.math.*
|
||||
|
||||
|
||||
/**
|
||||
* Create inner and outer tangents between two circles.
|
||||
* This method returns a map of segments using [DubinsPath] connection type notation.
|
||||
*/
|
||||
internal fun tangentsBetweenCircles(
|
||||
first: Circle2D,
|
||||
second: Circle2D,
|
||||
): Map<Type, StraightTrajectory2D> = with(Euclidean2DSpace) {
|
||||
// Distance between centers
|
||||
val distanceBetweenCenters: Double = first.center.distanceTo(second.center)
|
||||
|
||||
// return empty map if one circle is inside another
|
||||
val minRadius = min(first.radius, second.radius)
|
||||
val maxRadius = max(first.radius, second.radius)
|
||||
|
||||
val listOfTangents = when {
|
||||
// one circle inside another, no tangents
|
||||
distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap()
|
||||
// circles intersect, only outer tangents
|
||||
distanceBetweenCenters - minRadius <= maxRadius -> listOf(Type.RSR, Type.LSL)
|
||||
// no intersections all tangents
|
||||
else -> listOf(Type.RSR, Type.RSL, Type.LSR, Type.LSL)
|
||||
}
|
||||
|
||||
val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y)
|
||||
|
||||
return listOfTangents.associateWith { route ->
|
||||
val r1 = when (route.first) {
|
||||
Trajectory2D.L -> -first.radius
|
||||
Trajectory2D.R -> first.radius
|
||||
}
|
||||
val r2 = when (route.third) {
|
||||
Trajectory2D.L -> -second.radius
|
||||
Trajectory2D.R -> second.radius
|
||||
}
|
||||
val r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
|
||||
val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r)
|
||||
val angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
|
||||
StraightTrajectory2D(
|
||||
first.center + w * r1,
|
||||
second.center + w * r2
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex]
|
||||
*/
|
||||
public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: Int): CompositeTrajectory2D {
|
||||
require(fromIndex in circles.indices) { "$fromIndex is not in ${circles.indices}" }
|
||||
val startCircle = circles[fromIndex]
|
||||
val segments = buildList {
|
||||
val reserve = mutableListOf<Trajectory2D>()
|
||||
|
||||
val sourceSegments = when (direction) {
|
||||
Trajectory2D.L -> circumvention.reversed().segments
|
||||
Trajectory2D.R -> circumvention.segments
|
||||
}
|
||||
|
||||
var i = 0
|
||||
while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) {
|
||||
//put all segments before target circle on the reserve
|
||||
reserve.add(sourceSegments[i])
|
||||
i++
|
||||
}
|
||||
|
||||
while (i < sourceSegments.size) {
|
||||
// put required segments on result list
|
||||
add(sourceSegments[i])
|
||||
i++
|
||||
}
|
||||
//add remaining segments from reserve
|
||||
addAll(reserve)
|
||||
check(i == sourceSegments.size)
|
||||
}
|
||||
return CompositeTrajectory2D(segments)
|
||||
}
|
||||
|
||||
/**
|
||||
* Create an obstacle circumvention in given [direction] starting (including) from obstacle node with given [fromIndex]
|
||||
* and ending (including) at obstacle node with given [toIndex]
|
||||
*/
|
||||
public fun Obstacle.circumvention(
|
||||
direction: Trajectory2D.Direction,
|
||||
fromIndex: Int,
|
||||
toIndex: Int,
|
||||
): CompositeTrajectory2D {
|
||||
require(toIndex in circles.indices) { "$toIndex is not in ${circles.indices}" }
|
||||
val toCircle = circles[toIndex]
|
||||
val fullCircumvention = circumvention(direction, fromIndex).segments
|
||||
return CompositeTrajectory2D(
|
||||
buildList {
|
||||
var i = 0
|
||||
do {
|
||||
val segment = fullCircumvention[i]
|
||||
add(segment)
|
||||
i++
|
||||
} while ((segment as? CircleTrajectory2D)?.circle != toCircle)
|
||||
}
|
||||
)
|
||||
}
|
||||
@ -1,536 +0,0 @@
|
||||
package space.kscience.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.*
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.minus
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.times
|
||||
import space.kscience.kmath.misc.zipWithNextCircular
|
||||
import space.kscience.kmath.operations.DoubleField.pow
|
||||
import kotlin.math.*
|
||||
|
||||
|
||||
internal data class ObstacleConnection(
|
||||
val obstacle: Obstacle,
|
||||
val nodeIndex: Int,
|
||||
val direction: Trajectory2D.Direction,
|
||||
) {
|
||||
val circle get() = obstacle.circles[nodeIndex]
|
||||
}
|
||||
|
||||
internal data class ObstacleTangent(
|
||||
val lineSegment: LineSegment2D,
|
||||
val beginNode: ObstacleConnection,
|
||||
val endNode: ObstacleConnection,
|
||||
) : LineSegment2D by lineSegment {
|
||||
val startCircle get() = beginNode.circle
|
||||
val startDirection get() = beginNode.direction
|
||||
val endCircle get() = endNode.circle
|
||||
val endDirection get() = endNode.direction
|
||||
}
|
||||
|
||||
|
||||
private class LR<T>(val l: T, val r: T) {
|
||||
operator fun get(direction: Trajectory2D.Direction) = when (direction) {
|
||||
Trajectory2D.L -> l
|
||||
Trajectory2D.R -> r
|
||||
}
|
||||
}
|
||||
|
||||
private class TangentPath(val tangents: List<ObstacleTangent>) {
|
||||
fun last() = tangents.last()
|
||||
}
|
||||
|
||||
private fun TangentPath(vararg tangents: ObstacleTangent) = TangentPath(listOf(*tangents))
|
||||
|
||||
/**
|
||||
* Create inner and outer tangents between two circles.
|
||||
* This method returns a map of segments using [DubinsPath] connection type notation.
|
||||
*/
|
||||
internal fun tangentsBetweenCircles(
|
||||
first: Circle2D,
|
||||
second: Circle2D,
|
||||
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) {
|
||||
// Distance between centers
|
||||
val distanceBetweenCenters: Double = first.center.distanceTo(second.center)
|
||||
|
||||
// return empty map if one circle is inside another
|
||||
val minRadius = min(first.radius, second.radius)
|
||||
val maxRadius = max(first.radius, second.radius)
|
||||
|
||||
val listOfTangents = when {
|
||||
// one circle inside another, no tangents
|
||||
distanceBetweenCenters + minRadius <= maxRadius -> return emptyMap()
|
||||
// circles intersect, only outer tangents
|
||||
distanceBetweenCenters - minRadius <= maxRadius -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.LSL)
|
||||
// no intersections all tangents
|
||||
else -> listOf(DubinsPath.Type.RSR, DubinsPath.Type.RSL, DubinsPath.Type.LSR, DubinsPath.Type.LSL)
|
||||
}
|
||||
|
||||
val angle1 = atan2(second.center.x - first.center.x, second.center.y - first.center.y)
|
||||
|
||||
return listOfTangents.associateWith { route ->
|
||||
val r1 = when (route.first) {
|
||||
Trajectory2D.L -> -first.radius
|
||||
Trajectory2D.R -> first.radius
|
||||
}
|
||||
val r2 = when (route.third) {
|
||||
Trajectory2D.L -> -second.radius
|
||||
Trajectory2D.R -> second.radius
|
||||
}
|
||||
val r = if (r1.sign == r2.sign) {
|
||||
r1.absoluteValue - r2.absoluteValue
|
||||
} else {
|
||||
r1.absoluteValue + r2.absoluteValue
|
||||
}
|
||||
|
||||
val l = sqrt(distanceBetweenCenters * distanceBetweenCenters - r * r)
|
||||
val angle2 = if (r1.absoluteValue > r2.absoluteValue) {
|
||||
angle1 + r1.sign * atan2(r.absoluteValue, l)
|
||||
} else {
|
||||
angle1 - r2.sign * atan2(r.absoluteValue, l)
|
||||
}
|
||||
val w = vector(-cos(angle2), sin(angle2))
|
||||
|
||||
LineSegment(
|
||||
first.center + w * r1,
|
||||
second.center + w * r2
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
private fun Circle2D.isInside(other: Circle2D): Boolean {
|
||||
return center.distanceTo(other.center) + radius <= other.radius
|
||||
}
|
||||
|
||||
|
||||
internal class ObstacleShell(
|
||||
nodes: List<Circle2D>,
|
||||
) : Obstacle {
|
||||
override val circles: List<Circle2D>
|
||||
override val center: Vector2D<Double>
|
||||
private val shell: List<LineSegment2D>
|
||||
private val shellDirection: Trajectory2D.Direction
|
||||
|
||||
init {
|
||||
this.center = Euclidean2DSpace.vector(
|
||||
nodes.sumOf { it.center.x } / nodes.size,
|
||||
nodes.sumOf { it.center.y } / nodes.size
|
||||
)
|
||||
|
||||
// this.circles = nodes.filter { node ->
|
||||
// //filter nodes inside other nodes
|
||||
// nodes.none{ node !== it && node.isInside(it) }
|
||||
// }
|
||||
|
||||
this.circles = nodes.distinct()
|
||||
|
||||
if (nodes.size < 2) {
|
||||
shell = emptyList()
|
||||
shellDirection = Trajectory2D.R
|
||||
} else {
|
||||
|
||||
//ignore cases when one circle is inside another one
|
||||
val lslTangents = circles.zipWithNextCircular { a, b ->
|
||||
tangentsBetweenCircles(a, b)[DubinsPath.Type.LSL] ?: error("Intersecting circles")
|
||||
}
|
||||
|
||||
val rsrTangents = circles.zipWithNextCircular { a, b ->
|
||||
tangentsBetweenCircles(a, b)[DubinsPath.Type.RSR] ?: error("Intersecting circles")
|
||||
}
|
||||
|
||||
|
||||
val lslToCenter = lslTangents.sumOf { it.begin.distanceTo(center) } +
|
||||
lslTangents.sumOf { it.end.distanceTo(center) }
|
||||
val rsrToCenter = rsrTangents.sumOf { it.begin.distanceTo(center) } +
|
||||
rsrTangents.sumOf { it.end.distanceTo(center) }
|
||||
|
||||
if (rsrToCenter >= lslToCenter) {
|
||||
this.shell = rsrTangents
|
||||
this.shellDirection = Trajectory2D.R
|
||||
} else {
|
||||
this.shell = lslTangents
|
||||
this.shellDirection = Trajectory2D.L
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
constructor(obstacle: Obstacle) : this(obstacle.circles)
|
||||
|
||||
/**
|
||||
* Check if segment has any intersections with this obstacle
|
||||
*/
|
||||
override fun intersects(segment: LineSegment2D): Boolean =
|
||||
shell.any { tangent -> segment.intersectsSegment(tangent) }
|
||||
|| circles.any { circle -> segment.intersectsCircle(circle) }
|
||||
|
||||
override fun intersects(circle: Circle2D): Boolean =
|
||||
shell.any { tangent -> tangent.intersectsCircle(circle) }
|
||||
|
||||
/**
|
||||
* Tangent to next obstacle node in given direction
|
||||
*/
|
||||
fun nextTangent(circleIndex: Int, direction: Trajectory2D.Direction): ObstacleTangent {
|
||||
if (circleIndex == -1) error("Circle does not belong to this tangent")
|
||||
|
||||
val nextCircleIndex = if (direction == this.shellDirection) {
|
||||
if (circleIndex == circles.lastIndex) 0 else circleIndex + 1
|
||||
} else {
|
||||
if (circleIndex == 0) circles.lastIndex else circleIndex - 1
|
||||
}
|
||||
|
||||
return ObstacleTangent(
|
||||
LineSegment(
|
||||
shell[nextCircleIndex].end,
|
||||
shell[nextCircleIndex].begin
|
||||
),
|
||||
ObstacleConnection(this, circleIndex, direction),
|
||||
ObstacleConnection(this, nextCircleIndex, direction),
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* All tangents in given direction
|
||||
*/
|
||||
internal fun tangentsAlong(
|
||||
initialCircleIndex: Int,
|
||||
finalCircleIndex: Int,
|
||||
direction: Trajectory2D.Direction,
|
||||
): List<ObstacleTangent> {
|
||||
return buildList {
|
||||
var currentIndex = initialCircleIndex
|
||||
do {
|
||||
val tangent = nextTangent(currentIndex, direction)
|
||||
add(tangent)
|
||||
currentIndex = tangent.endNode.nodeIndex
|
||||
} while (currentIndex != finalCircleIndex)
|
||||
}
|
||||
}
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (other == null || other !is ObstacleShell) return false
|
||||
return circles == other.circles
|
||||
}
|
||||
|
||||
override fun hashCode(): Int {
|
||||
return circles.hashCode()
|
||||
}
|
||||
}
|
||||
|
||||
internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleShell(listOf(*circles))
|
||||
|
||||
|
||||
private fun LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean {
|
||||
infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
|
||||
infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
|
||||
|
||||
return with(Euclidean2DSpace) {
|
||||
(other.begin - begin) crossSign (other.end - begin) !=
|
||||
(other.begin - end) crossSign (other.end - end) &&
|
||||
(begin - other.begin) crossSign (end - other.begin) !=
|
||||
(begin - other.end) crossSign (end - other.end)
|
||||
}
|
||||
}
|
||||
|
||||
private fun LineSegment2D.intersectsCircle(circle: Circle2D): Boolean {
|
||||
val a = (begin.x - end.x).pow(2.0) + (begin.y - end.y).pow(2.0)
|
||||
val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
|
||||
(begin.y - end.y) * (end.y - circle.center.y))
|
||||
val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
|
||||
circle.radius.pow(2.0)
|
||||
|
||||
val aNormalized = a / (a * a + b * b + c * c)
|
||||
val bNormalized = b / (a * a + b * b + c * c)
|
||||
val cNormalized = c / (a * a + b * b + c * c)
|
||||
|
||||
val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
|
||||
if (d < 1e-6) {
|
||||
return false
|
||||
} else {
|
||||
val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
|
||||
val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
|
||||
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* All tangents between two obstacles
|
||||
*
|
||||
* In general generates 4 paths.
|
||||
* TODO check intersections.
|
||||
*/
|
||||
private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Type, ObstacleTangent> = buildMap {
|
||||
|
||||
for (firstCircleIndex in first.circles.indices) {
|
||||
for (secondCircleIndex in second.circles.indices) {
|
||||
for ((pathType, segment) in tangentsBetweenCircles(
|
||||
first.circles[firstCircleIndex],
|
||||
second.circles[secondCircleIndex]
|
||||
)) {
|
||||
val tangent = ObstacleTangent(
|
||||
segment,
|
||||
ObstacleConnection(first, firstCircleIndex, pathType.first),
|
||||
ObstacleConnection(second, secondCircleIndex, pathType.third)
|
||||
)
|
||||
|
||||
if (!(first.intersects(tangent)) && !(second.intersects(tangent))) {
|
||||
put(
|
||||
pathType,
|
||||
tangent
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private fun arcLength(
|
||||
circle: Circle2D,
|
||||
point1: DoubleVector2D,
|
||||
point2: DoubleVector2D,
|
||||
direction: Trajectory2D.Direction,
|
||||
): Double {
|
||||
val phi1 = atan2(point1.y - circle.center.y, point1.x - circle.center.x)
|
||||
val phi2 = atan2(point2.y - circle.center.y, point2.x - circle.center.x)
|
||||
var angle = 0.0
|
||||
when (direction) {
|
||||
Trajectory2D.L -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
phi2 - phi1
|
||||
} else {
|
||||
2 * PI + phi2 - phi1
|
||||
}
|
||||
}
|
||||
|
||||
Trajectory2D.R -> {
|
||||
angle = if (phi2 >= phi1) {
|
||||
2 * PI - (phi2 - phi1)
|
||||
} else {
|
||||
-(phi2 - phi1)
|
||||
}
|
||||
}
|
||||
}
|
||||
return circle.radius * angle
|
||||
}
|
||||
|
||||
private fun normalVectors(v: DoubleVector2D, r: Double): Pair<DoubleVector2D, DoubleVector2D> {
|
||||
return Pair(
|
||||
r * Euclidean2DSpace.vector(v.y / Euclidean2DSpace.norm(v), -v.x / Euclidean2DSpace.norm(v)),
|
||||
r * Euclidean2DSpace.vector(-v.y / Euclidean2DSpace.norm(v), v.x / Euclidean2DSpace.norm(v))
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
private fun constructTangentCircles(
|
||||
point: DoubleVector2D,
|
||||
direction: DoubleVector2D,
|
||||
r: Double,
|
||||
): LR<Circle2D> {
|
||||
val center1 = point + normalVectors(direction, r).first
|
||||
val center2 = point + normalVectors(direction, r).second
|
||||
val p1 = center1 - point
|
||||
return if (atan2(p1.y, p1.x) - atan2(direction.y, direction.x) in listOf(PI / 2, -3 * PI / 2)) {
|
||||
LR(
|
||||
Circle2D(center1, r),
|
||||
Circle2D(center2, r)
|
||||
)
|
||||
} else {
|
||||
LR(
|
||||
Circle2D(center2, r),
|
||||
Circle2D(center1, r)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
private fun sortedObstacles(
|
||||
currentObstacle: Obstacle,
|
||||
obstacles: List<Obstacle>,
|
||||
): List<Obstacle> {
|
||||
return obstacles.sortedBy { Euclidean2DSpace.norm(it.center - currentObstacle.center) }
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if all proposed paths have ended at [finalObstacle]
|
||||
*/
|
||||
private fun allFinished(
|
||||
paths: List<TangentPath>,
|
||||
finalObstacle: Obstacle,
|
||||
): Boolean {
|
||||
for (path in paths) {
|
||||
if (path.last().endNode.obstacle !== finalObstacle) {
|
||||
return false
|
||||
}
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
private fun LineSegment2D.toTrajectory() = StraightTrajectory2D(begin, end)
|
||||
|
||||
|
||||
private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTrajectory2D(
|
||||
buildList {
|
||||
tangents.zipWithNext().forEach { (left, right: ObstacleTangent) ->
|
||||
add(left.lineSegment.toTrajectory())
|
||||
add(
|
||||
CircleTrajectory2D.of(
|
||||
right.startCircle.center,
|
||||
left.lineSegment.end,
|
||||
right.lineSegment.begin,
|
||||
right.startDirection
|
||||
)
|
||||
)
|
||||
}
|
||||
|
||||
add(tangents.last().lineSegment.toTrajectory())
|
||||
}
|
||||
)
|
||||
|
||||
internal fun findAllPaths(
|
||||
start: DubinsPose2D,
|
||||
startingRadius: Double,
|
||||
finish: DubinsPose2D,
|
||||
finalRadius: Double,
|
||||
obstacles: List<ObstacleShell>,
|
||||
): List<CompositeTrajectory2D> {
|
||||
fun DubinsPose2D.direction() = Euclidean2DSpace.vector(cos(bearing), sin(bearing))
|
||||
|
||||
// two circles for the initial point
|
||||
val initialCircles = constructTangentCircles(
|
||||
start,
|
||||
start.direction(),
|
||||
startingRadius
|
||||
)
|
||||
|
||||
//two circles for the final point
|
||||
val finalCircles = constructTangentCircles(
|
||||
finish,
|
||||
finish.direction(),
|
||||
finalRadius
|
||||
)
|
||||
|
||||
//all valid trajectories
|
||||
val trajectories = mutableListOf<CompositeTrajectory2D>()
|
||||
|
||||
for (i in listOf(Trajectory2D.L, Trajectory2D.R)) {
|
||||
for (j in listOf(Trajectory2D.L, Trajectory2D.R)) {
|
||||
//Using obstacle to minimize code bloat
|
||||
val initialObstacle = ObstacleShell(initialCircles[i])
|
||||
val finalObstacle = ObstacleShell(finalCircles[j])
|
||||
|
||||
var currentPaths: List<TangentPath> = listOf(
|
||||
TangentPath(
|
||||
//We need only the direction of the final segment from this
|
||||
ObstacleTangent(
|
||||
LineSegment(start, start),
|
||||
ObstacleConnection(initialObstacle, 0, i),
|
||||
ObstacleConnection(initialObstacle, 0, i),
|
||||
)
|
||||
)
|
||||
)
|
||||
while (!allFinished(currentPaths, finalObstacle)) {
|
||||
// paths after next obstacle iteration
|
||||
val newPaths = mutableListOf<TangentPath>()
|
||||
// for each path propagate it one obstacle further
|
||||
for (tangentPath: TangentPath in currentPaths) {
|
||||
val currentNode = tangentPath.last().endNode
|
||||
val currentDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
val currentObstacle: ObstacleShell = ObstacleShell(tangentPath.last().endNode.obstacle)
|
||||
|
||||
// If path is finished, ignore it
|
||||
// TODO avoid returning to ignored obstacle on the next cycle
|
||||
if (currentObstacle == finalObstacle) {
|
||||
newPaths.add(tangentPath)
|
||||
} else {
|
||||
val tangentToFinal: ObstacleTangent =
|
||||
outerTangents(currentObstacle, finalObstacle)[DubinsPath.Type(
|
||||
currentDirection,
|
||||
Trajectory2D.S,
|
||||
j
|
||||
)] ?: break
|
||||
|
||||
// searching for the nearest obstacle that intersects with the direct path
|
||||
val nextObstacle = sortedObstacles(currentObstacle, obstacles).find { obstacle ->
|
||||
obstacle.intersects(tangentToFinal)
|
||||
} ?: finalObstacle
|
||||
|
||||
//TODO add break check for end of path
|
||||
|
||||
// All valid tangents from current obstacle to the next one
|
||||
val nextTangents: Collection<ObstacleTangent> = outerTangents(
|
||||
currentObstacle,
|
||||
nextObstacle
|
||||
).filter { (key, tangent) ->
|
||||
obstacles.none { obstacle -> obstacle.intersects(tangent) } && // does not intersect other obstacles
|
||||
key.first == currentDirection && // initial direction is the same as end of previous segment direction
|
||||
(nextObstacle != finalObstacle || key.third == j) // if it is the last, it should be the same as the one we are searching for
|
||||
}.values
|
||||
|
||||
for (tangent in nextTangents) {
|
||||
val tangentsAlong = if (tangent.startCircle === tangentPath.last().endCircle) {
|
||||
//if the previous segment last circle is the same as first circle of the next segment
|
||||
|
||||
//If obstacle consists of single circle, do not walk around
|
||||
if (currentObstacle.circles.size < 2) {
|
||||
emptyList()
|
||||
} else {
|
||||
val lengthMaxPossible = arcLength(
|
||||
tangent.startCircle,
|
||||
tangentPath.last().lineSegment.end,
|
||||
currentObstacle.nextTangent(
|
||||
tangent.beginNode.nodeIndex,
|
||||
currentDirection
|
||||
).lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
|
||||
val lengthCalculated = arcLength(
|
||||
tangent.startCircle,
|
||||
tangentPath.last().lineSegment.end,
|
||||
tangent.lineSegment.begin,
|
||||
currentDirection
|
||||
)
|
||||
// ensure that path does not go inside the obstacle
|
||||
if (lengthCalculated > lengthMaxPossible) {
|
||||
currentObstacle.tangentsAlong(
|
||||
currentNode.nodeIndex,
|
||||
tangent.beginNode.nodeIndex,
|
||||
currentDirection,
|
||||
)
|
||||
} else {
|
||||
emptyList()
|
||||
}
|
||||
}
|
||||
} else {
|
||||
currentObstacle.tangentsAlong(
|
||||
currentNode.nodeIndex,
|
||||
tangent.beginNode.nodeIndex,
|
||||
currentDirection,
|
||||
)
|
||||
}
|
||||
newPaths.add(TangentPath(tangentPath.tangents + tangentsAlong + tangent))
|
||||
}
|
||||
}
|
||||
}
|
||||
currentPaths = newPaths
|
||||
}
|
||||
|
||||
trajectories += currentPaths.map { tangentPath ->
|
||||
// val lastDirection: Trajectory2D.Direction = tangentPath.last().endDirection
|
||||
val end = Obstacle(finalCircles[j])
|
||||
TangentPath(
|
||||
tangentPath.tangents +
|
||||
ObstacleTangent(
|
||||
LineSegment(finish, finish),
|
||||
ObstacleConnection(end, 0, j),
|
||||
ObstacleConnection(end, 0, j)
|
||||
)
|
||||
)
|
||||
}.map { it.toTrajectory() }
|
||||
}
|
||||
}
|
||||
return trajectories
|
||||
}
|
||||
@ -3,21 +3,19 @@
|
||||
* 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.trajectory.segments
|
||||
package space.kscience.kmath.geometry
|
||||
|
||||
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.trajectory.CircleTrajectory2D
|
||||
import space.kscience.trajectory.DubinsPose2D
|
||||
import space.kscience.trajectory.Trajectory2D
|
||||
import kotlin.math.PI
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
class ArcTests {
|
||||
|
||||
@Test
|
||||
fun arcTest() = with(Euclidean2DSpace){
|
||||
fun arc() = with(Euclidean2DSpace) {
|
||||
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
||||
val arc = CircleTrajectory2D.of(
|
||||
circle.center,
|
||||
@ -26,7 +24,19 @@ class ArcTests {
|
||||
Trajectory2D.R
|
||||
)
|
||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||
assertEquals(0.0, arc.start.bearing.degrees)
|
||||
assertEquals(0.0, arc.begin.bearing.degrees)
|
||||
assertEquals(90.0, arc.end.bearing.degrees)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun quarter() = with(Euclidean2DSpace) {
|
||||
val circle = circle(1, 0, 1)
|
||||
val arc = CircleTrajectory2D(
|
||||
circle,
|
||||
DubinsPose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians),
|
||||
DubinsPose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians)
|
||||
)
|
||||
assertEquals(Trajectory2D.R, arc.direction)
|
||||
assertEquals(PI / 2, arc.length, 1e-4)
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.pow
|
||||
import kotlin.math.sqrt
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
import kotlin.test.assertFalse
|
||||
import kotlin.test.assertTrue
|
||||
|
||||
class CircleTests {
|
||||
|
||||
@Test
|
||||
fun circle() {
|
||||
val center = Euclidean2DSpace.vector(0.0, 0.0)
|
||||
val radius = 2.0
|
||||
val expectedCircumference = 12.56637
|
||||
val circle = Circle2D(center, radius)
|
||||
assertEquals(expectedCircumference, circle.circumference, 1e-4)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun circleIntersection() = with(Euclidean2DSpace) {
|
||||
assertTrue {
|
||||
intersectsOrInside(
|
||||
circle(0.0, 0.0, 1.0),
|
||||
circle(0.0, 0.0, 2.0)
|
||||
)
|
||||
}
|
||||
assertTrue {
|
||||
intersectsOrInside(
|
||||
circle(0.0, 1.0, 1.0),
|
||||
circle(0.0, 0.0, 1.0)
|
||||
)
|
||||
}
|
||||
assertFalse {
|
||||
intersectsOrInside(
|
||||
circle(0.0, 1.0, 1.0),
|
||||
circle(0.0, -1.1, 1.0)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
fun circleLineIntersection() = with(Euclidean2DSpace) {
|
||||
assertTrue {
|
||||
intersects(circle(0, 0, 1), segment(1, 1, -1, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
intersects(circle(1, 1, sqrt(2.0)/2), segment(1, 0, 0, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
intersects(circle(1, 1, 1), segment(2, 2, 0, 2))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
intersects(circle(0, 0, 1), segment(1, -1, 1, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
intersects(circle(0, 0, 1), segment(1, 0, -1, 0))
|
||||
}
|
||||
|
||||
assertFalse {
|
||||
intersects(circle(0, 0, 1), segment(1, 2, -1, 2))
|
||||
}
|
||||
|
||||
assertFalse {
|
||||
intersects(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0))
|
||||
}
|
||||
}
|
||||
|
||||
private fun Euclidean2DSpace.oldIntersect(circle: Circle2D, segment: LineSegment2D): Boolean{
|
||||
val begin = segment.begin
|
||||
val end = segment.end
|
||||
val lengthSquared = (begin.x - end.x).pow(2) + (begin.y - end.y).pow(2)
|
||||
val b = 2 * ((begin.x - end.x) * (end.x - circle.center.x) +
|
||||
(begin.y - end.y) * (end.y - circle.center.y))
|
||||
val c = (end.x - circle.center.x).pow(2) + (end.y - circle.center.y).pow(2) -
|
||||
circle.radius.pow(2)
|
||||
|
||||
val aNormalized = lengthSquared / (lengthSquared * lengthSquared + b * b + c * c)
|
||||
val bNormalized = b / (lengthSquared * lengthSquared + b * b + c * c)
|
||||
val cNormalized = c / (lengthSquared * lengthSquared + b * b + c * c)
|
||||
|
||||
val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
|
||||
if (d < 1e-6) {
|
||||
return false
|
||||
} else {
|
||||
val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
|
||||
val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
|
||||
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
@Test
|
||||
fun oldCircleLineIntersection() = with(Euclidean2DSpace){
|
||||
assertTrue {
|
||||
oldIntersect(circle(0, 0, 1.1), segment(1, 1, -1, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
oldIntersect(circle(1, 1, sqrt(2.0)/2+0.01), segment(1, 0, 0, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
oldIntersect(circle(1, 1, 1.01), segment(2, 2, 0, 2))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
oldIntersect(circle(0, 0, 1.01), segment(1, -1, 1, 1))
|
||||
}
|
||||
|
||||
assertTrue {
|
||||
oldIntersect(circle(0, 0, 1.0), segment(2, 0, -2, 0))
|
||||
}
|
||||
|
||||
assertFalse {
|
||||
oldIntersect(circle(0, 0, 1), segment(1, 2, -1, 2))
|
||||
}
|
||||
|
||||
assertFalse {
|
||||
oldIntersect(circle(-1, 0, 1), segment(0, 1.05, -2, 1.0))
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -3,10 +3,8 @@
|
||||
* 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.trajectory.segments
|
||||
package space.kscience.kmath.geometry
|
||||
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.degrees
|
||||
import space.kscience.trajectory.StraightTrajectory2D
|
||||
import kotlin.math.pow
|
||||
import kotlin.math.sqrt
|
||||
@ -45,16 +45,16 @@ class DubinsTests {
|
||||
val b = path.segments[1]
|
||||
val c = path.segments[2] as CircleTrajectory2D
|
||||
|
||||
assertTrue(start.equalsFloat(a.start))
|
||||
assertTrue(end.equalsFloat(c.end))
|
||||
assertEquals(start, a.begin)
|
||||
assertEquals(end, c.end)
|
||||
|
||||
// Not working, theta double precision inaccuracy
|
||||
if (b is CircleTrajectory2D) {
|
||||
assertTrue(a.end.equalsFloat(b.start))
|
||||
assertTrue(c.start.equalsFloat(b.end))
|
||||
assertEquals(a.end, b.begin)
|
||||
assertEquals(c.begin, b.end)
|
||||
} else if (b is StraightTrajectory2D) {
|
||||
assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing)))
|
||||
assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing)))
|
||||
assertEquals(a.end, DubinsPose2D(b.begin, b.bearing))
|
||||
assertEquals(c.begin, DubinsPose2D(b.end, b.bearing))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -8,38 +8,51 @@ package space.kscience.trajectory
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace.vector
|
||||
import space.kscience.kmath.geometry.degrees
|
||||
import kotlin.math.PI
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
import kotlin.test.assertTrue
|
||||
|
||||
class ObstacleTest {
|
||||
|
||||
@Test
|
||||
fun firstPath() {
|
||||
fun equalObstacles() {
|
||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
assertEquals(circle1, circle2)
|
||||
val obstacle1 = Obstacle(circle1)
|
||||
val obstacle2 = Obstacle(circle2)
|
||||
assertEquals(obstacle1, obstacle2)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun singeObstacle() {
|
||||
val startPoint = vector(-5.0, -1.0)
|
||||
val startDirection = vector(1.0, 1.0)
|
||||
val startRadius = 0.5
|
||||
val finalPoint = vector(20.0, 4.0)
|
||||
val finalDirection = vector(1.0, -1.0)
|
||||
|
||||
val outputTangents = Obstacle.avoidObstacles(
|
||||
val outputTangents = Obstacles.avoidObstacles(
|
||||
DubinsPose2D(startPoint, startDirection),
|
||||
DubinsPose2D(finalPoint, finalDirection),
|
||||
startRadius,
|
||||
Obstacle(Circle2D(vector(7.0, 1.0), 5.0))
|
||||
)
|
||||
assertTrue { outputTangents.isNotEmpty() }
|
||||
val length = outputTangents.minOf { it.length }
|
||||
assertEquals(27.2113183, length, 1e-6)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun secondPath() {
|
||||
fun twoObstacles() {
|
||||
val startPoint = vector(-5.0, -1.0)
|
||||
val startDirection = vector(1.0, 1.0)
|
||||
val radius = 0.5
|
||||
val finalPoint = vector(20.0, 4.0)
|
||||
val finalDirection = vector(1.0, -1.0)
|
||||
|
||||
val paths = Obstacle.avoidObstacles(
|
||||
val paths = Obstacles.avoidObstacles(
|
||||
DubinsPose2D(startPoint, startDirection),
|
||||
DubinsPose2D(finalPoint, finalDirection),
|
||||
radius,
|
||||
@ -55,19 +68,36 @@ class ObstacleTest {
|
||||
Circle2D(vector(9.0, 4.0), 0.5)
|
||||
)
|
||||
)
|
||||
assertTrue { paths.isNotEmpty() }
|
||||
val length = paths.minOf { it.length }
|
||||
assertEquals(28.9678224, length, 1e-6)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun nearPoints() {
|
||||
fun circumvention(){
|
||||
val obstacle = Obstacle(
|
||||
Circle2D(vector(0.0, 0.0), 1.0),
|
||||
Circle2D(vector(0.0, 1.0), 1.0),
|
||||
Circle2D(vector(1.0, 1.0), 1.0),
|
||||
Circle2D(vector(1.0, 0.0), 1.0)
|
||||
)
|
||||
|
||||
val circumvention = obstacle.circumvention
|
||||
|
||||
assertEquals(4, circumvention.segments.count { it is CircleTrajectory2D })
|
||||
|
||||
assertEquals(4 + 2* PI, circumvention.length, 1e-4)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun closePoints() {
|
||||
val startPoint = vector(-1.0, -1.0)
|
||||
val startDirection = vector(0.0, 1.0)
|
||||
val startRadius = 1.0
|
||||
val finalPoint = vector(-1, -1)
|
||||
val finalDirection = vector(1.0, 0)
|
||||
|
||||
val paths = Obstacle.avoidObstacles(
|
||||
val paths = Obstacles.avoidObstacles(
|
||||
DubinsPose2D(startPoint, startDirection),
|
||||
DubinsPose2D(finalPoint, finalDirection),
|
||||
startRadius,
|
||||
@ -78,14 +108,15 @@ class ObstacleTest {
|
||||
Circle2D(vector(1.0, 0.0), 1.0)
|
||||
)
|
||||
)
|
||||
assertTrue { paths.isNotEmpty() }
|
||||
val length = paths.minOf { it.length }
|
||||
println(length)
|
||||
//assertEquals(28.9678224, length, 1e-6)
|
||||
}
|
||||
|
||||
@Test
|
||||
fun fromMap() {
|
||||
val paths = Obstacle.avoidObstacles(
|
||||
fun largeCoordinates() {
|
||||
val paths = Obstacles.avoidObstacles(
|
||||
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
|
||||
DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
|
||||
5000.0,
|
||||
@ -98,34 +129,6 @@ class ObstacleTest {
|
||||
)
|
||||
)
|
||||
assertTrue { paths.isNotEmpty() }
|
||||
val length = paths.minOf { it.length }
|
||||
}
|
||||
@Test
|
||||
fun fromMapLess() {
|
||||
|
||||
val paths = Obstacle.avoidObstacles(
|
||||
DubinsPose2D(x = 48.4149535516561, y = 299.50862534208703, bearing = 3.401475378237137.degrees),
|
||||
DubinsPose2D(x = 45.66638489126448, y = 283.00541087567504, bearing = 325.32183928982727.degrees),
|
||||
0.5,
|
||||
Obstacle(
|
||||
Circle2D(vector(x=44.60882236175772, y=289.52640759535935), radius=0.5),
|
||||
Circle2D(vector(x=45.558751549431164, y=289.71165594902174), radius=0.5),
|
||||
Circle2D(vector(x=46.590308440141426, y=289.3897500160981), radius=0.5),
|
||||
Circle2D(vector(x=46.242119397653354, y=287.94964842121634), radius=0.5),
|
||||
Circle2D(vector(x=44.92318047505464, y=288.0132403305273), radius=0.5)
|
||||
)
|
||||
)
|
||||
val length = paths.minOf { it.length }
|
||||
}
|
||||
|
||||
@Test
|
||||
fun equalObstacles() {
|
||||
val circle1 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
val circle2 = Circle2D(vector(1.0, 6.5), 0.5)
|
||||
assertEquals(circle1, circle2)
|
||||
val obstacle1 = ObstacleShell(listOf(circle1))
|
||||
val obstacle2 = ObstacleShell(listOf(circle2))
|
||||
assertEquals(obstacle1, obstacle2)
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/*
|
||||
* Copyright 2018-2022 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.trajectory.segments
|
||||
|
||||
import space.kscience.kmath.geometry.Circle2D
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.circumference
|
||||
import kotlin.test.Test
|
||||
import kotlin.test.assertEquals
|
||||
|
||||
class CircleTests {
|
||||
|
||||
@Test
|
||||
fun arcTest() {
|
||||
val center = Euclidean2DSpace.vector(0.0, 0.0)
|
||||
val radius = 2.0
|
||||
val expectedCircumference = 12.56637
|
||||
val circle = Circle2D(center, radius)
|
||||
assertEquals(expectedCircumference, circle.circumference, 1e-4)
|
||||
}
|
||||
}
|
||||
@ -6,13 +6,15 @@
|
||||
package space.kscience.trajectory
|
||||
|
||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||
import space.kscience.kmath.geometry.equalsFloat
|
||||
import space.kscience.kmath.geometry.radians
|
||||
import space.kscience.kmath.geometry.sin
|
||||
|
||||
|
||||
fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
|
||||
x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians)
|
||||
fun assertEquals(expected: DubinsPose2D, actual: DubinsPose2D, precision: Double = 1e-6){
|
||||
kotlin.test.assertEquals(expected.x, actual.x, precision)
|
||||
kotlin.test.assertEquals(expected.y, actual.y, precision)
|
||||
kotlin.test.assertEquals(expected.bearing.radians, actual.bearing.radians, precision)
|
||||
}
|
||||
|
||||
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user