Refactor Dubins path
This commit is contained in:
parent
ee569b85f8
commit
e24463c58b
@ -14,7 +14,7 @@ allprojects {
|
|||||||
}
|
}
|
||||||
|
|
||||||
group = "space.kscience"
|
group = "space.kscience"
|
||||||
version = "0.3.1-dev-5"
|
version = "0.3.1-dev-6"
|
||||||
}
|
}
|
||||||
|
|
||||||
subprojects {
|
subprojects {
|
||||||
|
@ -77,11 +77,13 @@ public abstract class TensorFlowOutput<T, TT : TType>(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@PerformancePitfall
|
||||||
override fun get(index: IntArray): T = actualTensor[index]
|
override fun get(index: IntArray): T = actualTensor[index]
|
||||||
|
|
||||||
@PerformancePitfall
|
@PerformancePitfall
|
||||||
override fun elements(): Sequence<Pair<IntArray, T>> = actualTensor.elements()
|
override fun elements(): Sequence<Pair<IntArray, T>> = actualTensor.elements()
|
||||||
|
|
||||||
|
@PerformancePitfall
|
||||||
override fun set(index: IntArray, value: T) {
|
override fun set(index: IntArray, value: T) {
|
||||||
actualTensor[index] = value
|
actualTensor[index] = value
|
||||||
}
|
}
|
||||||
@ -101,6 +103,7 @@ public abstract class TensorFlowAlgebra<T, TT : TNumber, A : Ring<T>> internal c
|
|||||||
|
|
||||||
protected abstract fun const(value: T): Constant<TT>
|
protected abstract fun const(value: T): Constant<TT>
|
||||||
|
|
||||||
|
@OptIn(PerformancePitfall::class)
|
||||||
override fun StructureND<T>.valueOrNull(): T? = if (shape contentEquals ShapeND(1))
|
override fun StructureND<T>.valueOrNull(): T? = if (shape contentEquals ShapeND(1))
|
||||||
get(intArrayOf(0)) else null
|
get(intArrayOf(0)) else null
|
||||||
|
|
||||||
|
@ -13,75 +13,80 @@ import kotlin.math.acos
|
|||||||
import kotlin.math.cos
|
import kotlin.math.cos
|
||||||
import kotlin.math.sin
|
import kotlin.math.sin
|
||||||
|
|
||||||
internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
||||||
|
|
||||||
internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
|
internal fun DubinsPose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
|
||||||
|
|
||||||
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
|
internal fun DubinsPose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> = with(Euclidean2DSpace) {
|
||||||
val dX = radius * cos(theta)
|
val dX = radius * cos(bearing)
|
||||||
val dY = radius * sin(theta)
|
val dY = radius * sin(bearing)
|
||||||
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
return Circle2D(vector(x - dX, y + dY), radius) to Circle2D(vector(x + dX, y - dY), radius)
|
||||||
}
|
}
|
||||||
|
|
||||||
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b, CircleTrajectory.Direction.LEFT)
|
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D =
|
||||||
|
outerTangent(a, b, CircleTrajectory2D.Direction.LEFT)
|
||||||
|
|
||||||
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory = outerTangent(a, b,
|
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightTrajectory2D = outerTangent(
|
||||||
CircleTrajectory.Direction.RIGHT
|
a, b,
|
||||||
|
CircleTrajectory2D.Direction.RIGHT
|
||||||
)
|
)
|
||||||
|
|
||||||
private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory = with(Euclidean2DSpace){
|
private fun outerTangent(a: Circle2D, b: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D =
|
||||||
val centers = StraightTrajectory(a.center, b.center)
|
with(Euclidean2DSpace) {
|
||||||
val p1 = when (side) {
|
val centers = StraightTrajectory2D(a.center, b.center)
|
||||||
CircleTrajectory.Direction.LEFT -> vector(
|
val p1 = when (side) {
|
||||||
a.center.x - a.radius * cos(centers.theta),
|
CircleTrajectory2D.Direction.LEFT -> vector(
|
||||||
a.center.y + a.radius * sin(centers.theta)
|
a.center.x - a.radius * cos(centers.bearing),
|
||||||
)
|
a.center.y + a.radius * sin(centers.bearing)
|
||||||
CircleTrajectory.Direction.RIGHT -> vector(
|
)
|
||||||
a.center.x + a.radius * cos(centers.theta),
|
|
||||||
a.center.y - a.radius * sin(centers.theta)
|
CircleTrajectory2D.Direction.RIGHT -> vector(
|
||||||
|
a.center.x + a.radius * cos(centers.bearing),
|
||||||
|
a.center.y - a.radius * sin(centers.bearing)
|
||||||
|
)
|
||||||
|
}
|
||||||
|
return StraightTrajectory2D(
|
||||||
|
p1,
|
||||||
|
vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
return StraightTrajectory(
|
|
||||||
p1,
|
|
||||||
vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
|
|
||||||
)
|
|
||||||
}
|
|
||||||
|
|
||||||
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? =
|
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
|
||||||
innerTangent(base, direction, CircleTrajectory.Direction.LEFT)
|
innerTangent(base, direction, CircleTrajectory2D.Direction.LEFT)
|
||||||
|
|
||||||
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory? =
|
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightTrajectory2D? =
|
||||||
innerTangent(base, direction, CircleTrajectory.Direction.RIGHT)
|
innerTangent(base, direction, CircleTrajectory2D.Direction.RIGHT)
|
||||||
|
|
||||||
private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory.Direction): StraightTrajectory? = with(Euclidean2DSpace){
|
private fun innerTangent(base: Circle2D, direction: Circle2D, side: CircleTrajectory2D.Direction): StraightTrajectory2D? =
|
||||||
val centers = StraightTrajectory(base.center, direction.center)
|
with(Euclidean2DSpace) {
|
||||||
if (centers.length < base.radius * 2) return null
|
val centers = StraightTrajectory2D(base.center, direction.center)
|
||||||
val angle = theta(
|
if (centers.length < base.radius * 2) return null
|
||||||
when (side) {
|
val angle = theta(
|
||||||
CircleTrajectory.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
|
when (side) {
|
||||||
CircleTrajectory.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
|
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length)
|
||||||
}
|
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length)
|
||||||
)
|
}
|
||||||
val dX = base.radius * sin(angle)
|
)
|
||||||
val dY = base.radius * cos(angle)
|
val dX = base.radius * sin(angle)
|
||||||
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
val dY = base.radius * cos(angle)
|
||||||
val p2 = vector(direction.center.x - dX, direction.center.y - dY)
|
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
||||||
return StraightTrajectory(p1, p2)
|
val p2 = vector(direction.center.x - dX, direction.center.y - dY)
|
||||||
}
|
return StraightTrajectory2D(p1, p2)
|
||||||
|
}
|
||||||
|
|
||||||
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
|
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
|
||||||
|
|
||||||
@Suppress("DuplicatedCode")
|
@Suppress("DuplicatedCode")
|
||||||
public class DubinsPath(
|
public class DubinsPath(
|
||||||
public val a: CircleTrajectory,
|
public val a: CircleTrajectory2D,
|
||||||
public val b: Trajectory,
|
public val b: Trajectory2D,
|
||||||
public val c: CircleTrajectory,
|
public val c: CircleTrajectory2D,
|
||||||
) : CompositeTrajectory(listOf(a,b,c)) {
|
) : CompositeTrajectory2D(listOf(a, b, c)) {
|
||||||
|
|
||||||
public val type: TYPE = TYPE.valueOf(
|
public val type: TYPE = TYPE.valueOf(
|
||||||
arrayOf(
|
arrayOf(
|
||||||
a.direction.name[0],
|
a.direction.name[0],
|
||||||
if (b is CircleTrajectory) b.direction.name[0] else 'S',
|
if (b is CircleTrajectory2D) b.direction.name[0] else 'S',
|
||||||
c.direction.name[0]
|
c.direction.name[0]
|
||||||
).toCharArray().concatToString()
|
).toCharArray().concatToString()
|
||||||
)
|
)
|
||||||
@ -92,8 +97,8 @@ public class DubinsPath(
|
|||||||
|
|
||||||
public companion object {
|
public companion object {
|
||||||
public fun all(
|
public fun all(
|
||||||
start: Pose2D,
|
start: DubinsPose2D,
|
||||||
end: Pose2D,
|
end: DubinsPose2D,
|
||||||
turningRadius: Double,
|
turningRadius: Double,
|
||||||
): List<DubinsPath> = listOfNotNull(
|
): List<DubinsPath> = listOfNotNull(
|
||||||
rlr(start, end, turningRadius),
|
rlr(start, end, turningRadius),
|
||||||
@ -104,132 +109,132 @@ public class DubinsPath(
|
|||||||
lsr(start, end, turningRadius)
|
lsr(start, end, turningRadius)
|
||||||
)
|
)
|
||||||
|
|
||||||
public fun shortest(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath =
|
public fun shortest(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath =
|
||||||
all(start, end, turningRadius).minBy { it.length }
|
all(start, end, turningRadius).minBy { it.length }
|
||||||
|
|
||||||
public fun rlr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
|
public fun rlr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
|
||||||
val c1 = start.getRightCircle(turningRadius)
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
val c2 = end.getRightCircle(turningRadius)
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
val centers = StraightTrajectory(c1.center, c2.center)
|
val centers = StraightTrajectory2D(c1.center, c2.center)
|
||||||
if (centers.length > turningRadius * 4) return null
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
val firstVariant = run {
|
val firstVariant = run {
|
||||||
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
||||||
var dX = turningRadius * sin(theta)
|
var dX = turningRadius * sin(theta)
|
||||||
var dY = turningRadius * cos(theta)
|
var dY = turningRadius * cos(theta)
|
||||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
val e = Circle2D(p, turningRadius)
|
val e = Circle2D(p, turningRadius)
|
||||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||||
theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
||||||
DubinsPath(a1, a2, a3)
|
DubinsPath(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
val secondVariant = run {
|
val secondVariant = run {
|
||||||
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
||||||
var dX = turningRadius * sin(theta)
|
var dX = turningRadius * sin(theta)
|
||||||
var dY = turningRadius * cos(theta)
|
var dY = turningRadius * cos(theta)
|
||||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
val e = Circle2D(p, turningRadius)
|
val e = Circle2D(p, turningRadius)
|
||||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||||
theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.LEFT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.RIGHT)
|
||||||
DubinsPath(a1, a2, a3)
|
DubinsPath(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun lrl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
|
public fun lrl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? = with(Euclidean2DSpace) {
|
||||||
val c1 = start.getLeftCircle(turningRadius)
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
val c2 = end.getLeftCircle(turningRadius)
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
val centers = StraightTrajectory(c1.center, c2.center)
|
val centers = StraightTrajectory2D(c1.center, c2.center)
|
||||||
if (centers.length > turningRadius * 4) return null
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
val firstVariant = run {
|
val firstVariant = run {
|
||||||
var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
||||||
var dX = turningRadius * sin(theta)
|
var dX = turningRadius * sin(theta)
|
||||||
var dY = turningRadius * cos(theta)
|
var dY = turningRadius * cos(theta)
|
||||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
val e = Circle2D(p, turningRadius)
|
val e = Circle2D(p, turningRadius)
|
||||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||||
theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
||||||
DubinsPath(a1, a2, a3)
|
DubinsPath(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
val secondVariant = run{
|
val secondVariant = run{
|
||||||
var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
|
var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
||||||
var dX = turningRadius * sin(theta)
|
var dX = turningRadius * sin(theta)
|
||||||
var dY = turningRadius * cos(theta)
|
var dY = turningRadius * cos(theta)
|
||||||
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
|
||||||
val e = Circle2D(p, turningRadius)
|
val e = Circle2D(p, turningRadius)
|
||||||
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
val p1 = vector(c1.center.x + dX, c1.center.y + dY)
|
||||||
theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
|
theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
||||||
dX = turningRadius * sin(theta)
|
dX = turningRadius * sin(theta)
|
||||||
dY = turningRadius * cos(theta)
|
dY = turningRadius * cos(theta)
|
||||||
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
val p2 = vector(e.center.x + dX, e.center.y + dY)
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, p1, CircleTrajectory.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, p1, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a2 = CircleTrajectory.of(e.center, p1, p2, CircleTrajectory.Direction.RIGHT)
|
val a2 = CircleTrajectory2D.of(e.center, p1, p2, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, p2, end, CircleTrajectory.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, p2, end, CircleTrajectory2D.Direction.LEFT)
|
||||||
DubinsPath(a1, a2, a3)
|
DubinsPath(a1, a2, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
return if (firstVariant.length < secondVariant.length) firstVariant else secondVariant
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun rsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
|
public fun rsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): 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 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
||||||
return DubinsPath(a1, s, a3)
|
return DubinsPath(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun lsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath {
|
public fun lsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): 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 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
||||||
return DubinsPath(a1, s, a3)
|
return DubinsPath(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun rsl(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
public fun rsl(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? {
|
||||||
val c1 = start.getRightCircle(turningRadius)
|
val c1 = start.getRightCircle(turningRadius)
|
||||||
val c2 = end.getLeftCircle(turningRadius)
|
val c2 = end.getLeftCircle(turningRadius)
|
||||||
val s = rightInnerTangent(c1, c2)
|
val s = rightInnerTangent(c1, c2)
|
||||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||||
|
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.RIGHT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.RIGHT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.LEFT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.LEFT)
|
||||||
return DubinsPath(a1, s, a3)
|
return DubinsPath(a1, s, a3)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun lsr(start: Pose2D, end: Pose2D, turningRadius: Double): DubinsPath? {
|
public fun lsr(start: DubinsPose2D, end: DubinsPose2D, turningRadius: Double): DubinsPath? {
|
||||||
val c1 = start.getLeftCircle(turningRadius)
|
val c1 = start.getLeftCircle(turningRadius)
|
||||||
val c2 = end.getRightCircle(turningRadius)
|
val c2 = end.getRightCircle(turningRadius)
|
||||||
val s = leftInnerTangent(c1, c2)
|
val s = leftInnerTangent(c1, c2)
|
||||||
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
|
||||||
|
|
||||||
val a1 = CircleTrajectory.of(c1.center, start, s.start, CircleTrajectory.Direction.LEFT)
|
val a1 = CircleTrajectory2D.of(c1.center, start, s.start, CircleTrajectory2D.Direction.LEFT)
|
||||||
val a3 = CircleTrajectory.of(c2.center, s.end, end, CircleTrajectory.Direction.RIGHT)
|
val a3 = CircleTrajectory2D.of(c2.center, s.end, end, CircleTrajectory2D.Direction.RIGHT)
|
||||||
return DubinsPath(a1, s, a3)
|
return DubinsPath(a1, s, a3)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -0,0 +1,36 @@
|
|||||||
|
/*
|
||||||
|
* 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.trajectory
|
||||||
|
|
||||||
|
import space.kscience.kmath.geometry.DoubleVector2D
|
||||||
|
import space.kscience.kmath.geometry.Vector
|
||||||
|
import kotlin.math.atan2
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Combination of [Vector] and its view angle (clockwise from positive y-axis direction)
|
||||||
|
*/
|
||||||
|
public interface DubinsPose2D : DoubleVector2D {
|
||||||
|
public val coordinate: DoubleVector2D
|
||||||
|
public val bearing: Double
|
||||||
|
}
|
||||||
|
|
||||||
|
public class PhaseVector2D(
|
||||||
|
override val coordinate: DoubleVector2D,
|
||||||
|
public val velocity: DoubleVector2D,
|
||||||
|
) : DubinsPose2D, DoubleVector2D by coordinate {
|
||||||
|
override val bearing: Double get() = atan2(velocity.x, velocity.y)
|
||||||
|
}
|
||||||
|
|
||||||
|
internal class Pose2DImpl(
|
||||||
|
override val coordinate: DoubleVector2D,
|
||||||
|
override val bearing: Double,
|
||||||
|
) : DubinsPose2D, DoubleVector2D by coordinate{
|
||||||
|
|
||||||
|
override fun toString(): String = "Pose2D(x=$x, y=$y, bearing=$bearing)"
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public fun Pose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = Pose2DImpl(coordinate, theta)
|
@ -1,33 +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.kmath.trajectory
|
|
||||||
|
|
||||||
import space.kscience.kmath.geometry.DoubleVector2D
|
|
||||||
import space.kscience.kmath.geometry.Vector
|
|
||||||
import kotlin.math.atan2
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Combination of [Vector] and its view angle
|
|
||||||
*/
|
|
||||||
public interface Pose2D: DoubleVector2D{
|
|
||||||
public val coordinate: DoubleVector2D
|
|
||||||
public val theta: Double
|
|
||||||
}
|
|
||||||
|
|
||||||
public class PhaseVector2D(
|
|
||||||
override val coordinate: DoubleVector2D,
|
|
||||||
public val velocity: DoubleVector2D
|
|
||||||
): Pose2D, DoubleVector2D by coordinate{
|
|
||||||
override val theta: Double get() = atan2(velocity.y, velocity.x)
|
|
||||||
}
|
|
||||||
|
|
||||||
internal class Pose2DImpl(
|
|
||||||
override val coordinate: DoubleVector2D,
|
|
||||||
override val theta: Double
|
|
||||||
) : Pose2D, DoubleVector2D by coordinate
|
|
||||||
|
|
||||||
|
|
||||||
public fun Pose2D(coordinate: DoubleVector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta)
|
|
@ -8,58 +8,61 @@ package space.kscience.kmath.trajectory
|
|||||||
import space.kscience.kmath.geometry.Circle2D
|
import space.kscience.kmath.geometry.Circle2D
|
||||||
import space.kscience.kmath.geometry.DoubleVector2D
|
import space.kscience.kmath.geometry.DoubleVector2D
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
import space.kscience.kmath.geometry.circumference
|
|
||||||
import kotlin.math.PI
|
import kotlin.math.PI
|
||||||
import kotlin.math.atan2
|
import kotlin.math.atan2
|
||||||
|
|
||||||
public sealed interface Trajectory {
|
public sealed interface Trajectory2D {
|
||||||
public val length: Double
|
public val length: Double
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Straight path segment. The order of start and end defines the direction
|
* Straight path segment. The order of start and end defines the direction
|
||||||
*/
|
*/
|
||||||
public data class StraightTrajectory(
|
public data class StraightTrajectory2D(
|
||||||
internal val start: DoubleVector2D,
|
internal val start: DoubleVector2D,
|
||||||
internal val end: DoubleVector2D,
|
internal val end: DoubleVector2D,
|
||||||
) : Trajectory {
|
) : Trajectory2D {
|
||||||
override val length: Double get() = start.distanceTo(end)
|
override val length: Double get() = start.distanceTo(end)
|
||||||
|
|
||||||
internal val theta: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
|
internal val bearing: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* An arc segment
|
* An arc segment
|
||||||
*/
|
*/
|
||||||
public data class CircleTrajectory(
|
public data class CircleTrajectory2D(
|
||||||
public val circle: Circle2D,
|
public val circle: Circle2D,
|
||||||
public val start: Pose2D,
|
public val start: DubinsPose2D,
|
||||||
public val end: Pose2D,
|
public val end: DubinsPose2D,
|
||||||
) : Trajectory {
|
) : Trajectory2D {
|
||||||
|
|
||||||
public enum class Direction {
|
public enum class Direction {
|
||||||
LEFT, RIGHT
|
LEFT, RIGHT
|
||||||
}
|
}
|
||||||
|
|
||||||
override val length: Double by lazy {
|
/**
|
||||||
val angle: Double = theta(
|
* Arc length in radians
|
||||||
|
*/
|
||||||
|
val arcLength: Double
|
||||||
|
get() = theta(
|
||||||
if (direction == Direction.LEFT) {
|
if (direction == Direction.LEFT) {
|
||||||
start.theta - end.theta
|
start.bearing - end.bearing
|
||||||
} else {
|
} else {
|
||||||
end.theta - start.theta
|
end.bearing - start.bearing
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
val proportion = angle / (2 * PI)
|
|
||||||
circle.circumference * proportion
|
override val length: Double by lazy {
|
||||||
|
circle.radius * arcLength
|
||||||
}
|
}
|
||||||
|
|
||||||
internal val direction: Direction by lazy {
|
internal val direction: Direction by lazy {
|
||||||
if (start.y < circle.center.y) {
|
if (start.y < circle.center.y) {
|
||||||
if (start.theta > PI) Direction.RIGHT else Direction.LEFT
|
if (start.bearing > PI) Direction.RIGHT else Direction.LEFT
|
||||||
} else if (start.y > circle.center.y) {
|
} else if (start.y > circle.center.y) {
|
||||||
if (start.theta < PI) Direction.RIGHT else Direction.LEFT
|
if (start.bearing < PI) Direction.RIGHT else Direction.LEFT
|
||||||
} else {
|
} else {
|
||||||
if (start.theta == 0.0) {
|
if (start.bearing == 0.0) {
|
||||||
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
|
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||||
} else {
|
} else {
|
||||||
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
|
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||||
@ -68,12 +71,17 @@ public data class CircleTrajectory(
|
|||||||
}
|
}
|
||||||
|
|
||||||
public companion object {
|
public companion object {
|
||||||
public fun of(center: DoubleVector2D, start: DoubleVector2D, end: DoubleVector2D, direction: Direction): CircleTrajectory {
|
public fun of(
|
||||||
|
center: DoubleVector2D,
|
||||||
|
start: DoubleVector2D,
|
||||||
|
end: DoubleVector2D,
|
||||||
|
direction: Direction,
|
||||||
|
): CircleTrajectory2D {
|
||||||
fun calculatePose(
|
fun calculatePose(
|
||||||
vector: DoubleVector2D,
|
vector: DoubleVector2D,
|
||||||
theta: Double,
|
theta: Double,
|
||||||
direction: Direction,
|
direction: Direction,
|
||||||
): Pose2D = Pose2D(
|
): DubinsPose2D = Pose2D(
|
||||||
vector,
|
vector,
|
||||||
when (direction) {
|
when (direction) {
|
||||||
Direction.LEFT -> theta(theta - PI / 2)
|
Direction.LEFT -> theta(theta - PI / 2)
|
||||||
@ -81,16 +89,20 @@ public data class CircleTrajectory(
|
|||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
val s1 = StraightTrajectory(center, start)
|
val s1 = StraightTrajectory2D(center, start)
|
||||||
val s2 = StraightTrajectory(center, end)
|
val s2 = StraightTrajectory2D(center, end)
|
||||||
val pose1 = calculatePose(start, s1.theta, direction)
|
val pose1 = calculatePose(start, s1.bearing, direction)
|
||||||
val pose2 = calculatePose(end, s2.theta, direction)
|
val pose2 = calculatePose(end, s2.bearing, direction)
|
||||||
return CircleTrajectory(Circle2D(center, s1.length), pose1, pose2)
|
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
|
||||||
|
if(trajectory.direction != direction){
|
||||||
|
error("Trajectory direction mismatch")
|
||||||
|
}
|
||||||
|
return trajectory
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public open class CompositeTrajectory(public val segments: Collection<Trajectory>) : Trajectory {
|
public open class CompositeTrajectory2D(public val segments: Collection<Trajectory2D>) : Trajectory2D {
|
||||||
override val length: Double get() = segments.sumOf { it.length }
|
override val length: Double get() = segments.sumOf { it.length }
|
||||||
}
|
}
|
||||||
|
|
@ -1,14 +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.kmath.trajectory
|
|
||||||
|
|
||||||
public fun interface TrajectoryCost {
|
|
||||||
public fun estimate(trajectory: Trajectory): Double
|
|
||||||
|
|
||||||
public companion object{
|
|
||||||
public val length: TrajectoryCost = TrajectoryCost { it.length }
|
|
||||||
}
|
|
||||||
}
|
|
@ -12,5 +12,5 @@ public fun interface MaxCurvature {
|
|||||||
public fun DubinsPath.Companion.shortest(
|
public fun DubinsPath.Companion.shortest(
|
||||||
start: PhaseVector2D,
|
start: PhaseVector2D,
|
||||||
end: PhaseVector2D,
|
end: PhaseVector2D,
|
||||||
computer: MaxCurvature,
|
maxCurvature: MaxCurvature,
|
||||||
): DubinsPath = shortest(start, end, computer.compute(start))
|
): DubinsPath = shortest(start, end, maxCurvature.compute(start))
|
||||||
|
@ -15,14 +15,14 @@ const val maxFloatDelta = 0.000001
|
|||||||
fun Double.radiansToDegrees() = this * 180 / PI
|
fun Double.radiansToDegrees() = this * 180 / PI
|
||||||
|
|
||||||
fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
|
fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta
|
||||||
fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta)
|
fun DubinsPose2D.equalsFloat(other: DubinsPose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.equalFloat(other.bearing)
|
||||||
|
|
||||||
fun StraightTrajectory.inverse() = StraightTrajectory(end, start)
|
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start)
|
||||||
fun StraightTrajectory.shift(shift: Int, width: Double): StraightTrajectory = with(Euclidean2DSpace){
|
fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace){
|
||||||
val dX = width * sin(inverse().theta)
|
val dX = width * sin(inverse().bearing)
|
||||||
val dY = width * sin(theta)
|
val dY = width * sin(bearing)
|
||||||
|
|
||||||
return StraightTrajectory(
|
return StraightTrajectory2D(
|
||||||
vector(start.x - dX * shift, start.y - dY * shift),
|
vector(start.x - dX * shift, start.y - dY * shift),
|
||||||
vector(end.x - dX * shift, end.y - dY * shift)
|
vector(end.x - dX * shift, end.y - dY * shift)
|
||||||
)
|
)
|
||||||
|
@ -16,11 +16,11 @@ class DubinsTests {
|
|||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun dubinsTest() = with(Euclidean2DSpace){
|
fun dubinsTest() = with(Euclidean2DSpace){
|
||||||
val straight = StraightTrajectory(vector(0.0, 0.0), vector(100.0, 100.0))
|
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
|
||||||
val lineP1 = straight.shift(1, 10.0).inverse()
|
val lineP1 = straight.shift(1, 10.0).inverse()
|
||||||
|
|
||||||
val start = Pose2D(straight.end, straight.theta)
|
val start = Pose2D(straight.end, straight.bearing)
|
||||||
val end = Pose2D(lineP1.start, lineP1.theta)
|
val end = Pose2D(lineP1.start, lineP1.bearing)
|
||||||
val radius = 2.0
|
val radius = 2.0
|
||||||
val dubins = DubinsPath.all(start, end, radius)
|
val dubins = DubinsPath.all(start, end, radius)
|
||||||
|
|
||||||
@ -45,14 +45,14 @@ class DubinsTests {
|
|||||||
assertTrue(end.equalsFloat(path.c.end))
|
assertTrue(end.equalsFloat(path.c.end))
|
||||||
|
|
||||||
// Not working, theta double precision inaccuracy
|
// Not working, theta double precision inaccuracy
|
||||||
if (path.b is CircleTrajectory) {
|
if (path.b is CircleTrajectory2D) {
|
||||||
val b = path.b as CircleTrajectory
|
val b = path.b as CircleTrajectory2D
|
||||||
assertTrue(path.a.end.equalsFloat(b.start))
|
assertTrue(path.a.end.equalsFloat(b.start))
|
||||||
assertTrue(path.c.start.equalsFloat(b.end))
|
assertTrue(path.c.start.equalsFloat(b.end))
|
||||||
} else if (path.b is StraightTrajectory) {
|
} else if (path.b is StraightTrajectory2D) {
|
||||||
val b = path.b as StraightTrajectory
|
val b = path.b as StraightTrajectory2D
|
||||||
assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta)))
|
assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.bearing)))
|
||||||
assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta)))
|
assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.bearing)))
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -8,7 +8,7 @@ package space.kscience.kmath.trajectory.segments
|
|||||||
import space.kscience.kmath.geometry.Circle2D
|
import space.kscience.kmath.geometry.Circle2D
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||||
import space.kscience.kmath.geometry.circumference
|
import space.kscience.kmath.geometry.circumference
|
||||||
import space.kscience.kmath.trajectory.CircleTrajectory
|
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
||||||
import space.kscience.kmath.trajectory.radiansToDegrees
|
import space.kscience.kmath.trajectory.radiansToDegrees
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertEquals
|
import kotlin.test.assertEquals
|
||||||
@ -18,9 +18,9 @@ class ArcTests {
|
|||||||
@Test
|
@Test
|
||||||
fun arcTest() = with(Euclidean2DSpace){
|
fun arcTest() = with(Euclidean2DSpace){
|
||||||
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
||||||
val arc = CircleTrajectory.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory.Direction.RIGHT)
|
val arc = CircleTrajectory2D.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory2D.Direction.RIGHT)
|
||||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||||
assertEquals(0.0, arc.start.theta.radiansToDegrees())
|
assertEquals(0.0, arc.start.bearing.radiansToDegrees())
|
||||||
assertEquals(90.0, arc.end.theta.radiansToDegrees())
|
assertEquals(90.0, arc.end.bearing.radiansToDegrees())
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -6,7 +6,7 @@
|
|||||||
package space.kscience.kmath.trajectory.segments
|
package space.kscience.kmath.trajectory.segments
|
||||||
|
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||||
import space.kscience.kmath.trajectory.StraightTrajectory
|
import space.kscience.kmath.trajectory.StraightTrajectory2D
|
||||||
import space.kscience.kmath.trajectory.radiansToDegrees
|
import space.kscience.kmath.trajectory.radiansToDegrees
|
||||||
import kotlin.math.pow
|
import kotlin.math.pow
|
||||||
import kotlin.math.sqrt
|
import kotlin.math.sqrt
|
||||||
@ -17,21 +17,21 @@ class LineTests {
|
|||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun lineTest() = with(Euclidean2DSpace){
|
fun lineTest() = with(Euclidean2DSpace){
|
||||||
val straight = StraightTrajectory(vector(0.0, 0.0), vector(100.0, 100.0))
|
val straight = StraightTrajectory2D(vector(0.0, 0.0), vector(100.0, 100.0))
|
||||||
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
|
assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length)
|
||||||
assertEquals(45.0, straight.theta.radiansToDegrees())
|
assertEquals(45.0, straight.bearing.radiansToDegrees())
|
||||||
}
|
}
|
||||||
|
|
||||||
@Test
|
@Test
|
||||||
fun lineAngleTest() = with(Euclidean2DSpace){
|
fun lineAngleTest() = with(Euclidean2DSpace){
|
||||||
//val zero = Vector2D(0.0, 0.0)
|
//val zero = Vector2D(0.0, 0.0)
|
||||||
val north = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, 2.0))
|
val north = StraightTrajectory2D(zero, vector(0.0, 2.0))
|
||||||
assertEquals(0.0, north.theta.radiansToDegrees())
|
assertEquals(0.0, north.bearing.radiansToDegrees())
|
||||||
val east = StraightTrajectory(Euclidean2DSpace.zero, vector(2.0, 0.0))
|
val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
|
||||||
assertEquals(90.0, east.theta.radiansToDegrees())
|
assertEquals(90.0, east.bearing.radiansToDegrees())
|
||||||
val south = StraightTrajectory(Euclidean2DSpace.zero, vector(0.0, -2.0))
|
val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
|
||||||
assertEquals(180.0, south.theta.radiansToDegrees())
|
assertEquals(180.0, south.bearing.radiansToDegrees())
|
||||||
val west = StraightTrajectory(Euclidean2DSpace.zero, vector(-2.0, 0.0))
|
val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
|
||||||
assertEquals(270.0, west.theta.radiansToDegrees())
|
assertEquals(270.0, west.bearing.radiansToDegrees())
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user