refactoring directions

This commit is contained in:
Alexander Nozik 2023-04-04 18:50:17 +03:00
parent f5201b6be0
commit 639a255aaf
3 changed files with 31 additions and 21 deletions

View File

@ -3,6 +3,7 @@
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
*/
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
package space.kscience.kmath.trajectory
import kotlinx.serialization.KSerializer
@ -22,6 +23,19 @@ import kotlin.math.atan2
public interface DubinsPose2D : DoubleVector2D {
public val coordinates: DoubleVector2D
public val bearing: Angle
public companion object {
public fun bearingToVector(bearing: Angle): Vector2D<Double> =
Euclidean2DSpace.vector(cos(bearing), sin(bearing))
public fun vectorToBearing(vector2D: DoubleVector2D): Angle {
require(vector2D.x != 0.0 || vector2D.y != 0.0) { "Can't get bearing of zero vector" }
return atan2(vector2D.y, vector2D.x).radians
}
public fun of(point: DoubleVector2D, direction: DoubleVector2D): DubinsPose2D =
DubinsPose2D(point, vectorToBearing(direction))
}
}
@Serializable

View File

@ -22,7 +22,7 @@ internal data class Tangent(
val endObstacle: Obstacle,
val lineSegment: LineSegment2D,
val startDirection: Trajectory2D.Direction,
val endDirection: Trajectory2D.Direction = startDirection
val endDirection: Trajectory2D.Direction = startDirection,
)
private class TangentPath(val tangents: List<Tangent>) {
@ -465,22 +465,22 @@ private fun TangentPath.toTrajectory(): CompositeTrajectory2D = CompositeTraject
)
internal fun findAllPaths(
startingPoint: DoubleVector2D,
startingDirection: DoubleVector2D,
start: DubinsPose2D,
startingRadius: Double,
finalPoint: DoubleVector2D,
finalDirection: DoubleVector2D,
finish: DubinsPose2D,
finalRadius: Double,
obstacles: List<Obstacle>,
): List<CompositeTrajectory2D> {
fun DubinsPose2D.direction() = vector(cos(bearing),sin(bearing))
val initialCircles = constructTangentCircles(
startingPoint,
startingDirection,
start,
start.direction(),
startingRadius
)
val finalCircles = constructTangentCircles(
finalPoint,
finalDirection,
finish,
finish.direction(),
finalRadius
)
val trajectories = mutableListOf<CompositeTrajectory2D>()
@ -495,7 +495,7 @@ internal fun findAllPaths(
initialCircles[i]!!,
Obstacle(listOf(initialCircles[i]!!)),
Obstacle(listOf(initialCircles[i]!!)),
LineSegment2D(startingPoint, startingPoint),
LineSegment2D(start, start),
i
)
)
@ -596,7 +596,7 @@ internal fun findAllPaths(
end,
Obstacle(end),
Obstacle(end),
LineSegment2D(finalPoint, finalPoint),
LineSegment2D(finish, finish),
startDirection = lastDirection,
endDirection = j
)

View File

@ -29,11 +29,9 @@ class ObstacleTest {
)
val outputTangents = findAllPaths(
startPoint,
startDirection,
DubinsPose2D.of(startPoint, startDirection),
startRadius,
finalPoint,
finalDirection,
DubinsPose2D.of(finalPoint, finalDirection),
finalRadius,
obstacles
)
@ -68,11 +66,9 @@ class ObstacleTest {
)
)
val paths = findAllPaths(
startPoint,
startDirection,
DubinsPose2D.of(startPoint, startDirection),
startRadius,
finalPoint,
finalDirection,
DubinsPose2D.of(finalPoint, finalDirection),
finalRadius,
obstacles
)