forked from kscience/kmath
refactoring directions
This commit is contained in:
parent
f5201b6be0
commit
639a255aaf
@ -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
|
||||
@ -37,12 +51,12 @@ public class PhaseVector2D(
|
||||
private class DubinsPose2DImpl(
|
||||
override val coordinates: DoubleVector2D,
|
||||
override val bearing: Angle,
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates{
|
||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||
|
||||
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
|
||||
}
|
||||
|
||||
public object DubinsPose2DSerializer: KSerializer<DubinsPose2D>{
|
||||
public object DubinsPose2DSerializer : KSerializer<DubinsPose2D> {
|
||||
private val proxySerializer = DubinsPose2DImpl.serializer()
|
||||
|
||||
override val descriptor: SerialDescriptor
|
||||
|
@ -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
|
||||
)
|
||||
|
@ -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
|
||||
)
|
||||
|
Loading…
Reference in New Issue
Block a user