forked from kscience/kmath
Update type-safe angles
This commit is contained in:
parent
0366a69123
commit
2c6d1e89c5
@ -10,6 +10,7 @@
|
|||||||
- Algebra now has an obligatory `bufferFactory` (#477).
|
- Algebra now has an obligatory `bufferFactory` (#477).
|
||||||
|
|
||||||
### Changed
|
### Changed
|
||||||
|
- Trajectory use type-safe angles
|
||||||
- Tensor operations switched to prefix notation
|
- Tensor operations switched to prefix notation
|
||||||
- Row-wise and column-wise ND shapes in the core
|
- Row-wise and column-wise ND shapes in the core
|
||||||
- Shape is read-only
|
- Shape is read-only
|
||||||
|
@ -7,8 +7,9 @@ package space.kscience.kmath.geometry
|
|||||||
|
|
||||||
import kotlin.jvm.JvmInline
|
import kotlin.jvm.JvmInline
|
||||||
import kotlin.math.PI
|
import kotlin.math.PI
|
||||||
|
import kotlin.math.floor
|
||||||
|
|
||||||
public sealed interface Angle {
|
public sealed interface Angle : Comparable<Angle> {
|
||||||
public fun toRadians(): Radians
|
public fun toRadians(): Radians
|
||||||
public fun toDegrees(): Degrees
|
public fun toDegrees(): Degrees
|
||||||
|
|
||||||
@ -17,7 +18,15 @@ public sealed interface Angle {
|
|||||||
|
|
||||||
public operator fun times(other: Number): Angle
|
public operator fun times(other: Number): Angle
|
||||||
public operator fun div(other: Number): Angle
|
public operator fun div(other: Number): Angle
|
||||||
|
public operator fun div(other: Angle): Double
|
||||||
public operator fun unaryMinus(): Angle
|
public operator fun unaryMinus(): Angle
|
||||||
|
|
||||||
|
public companion object {
|
||||||
|
public val zero: Radians = Radians(0.0)
|
||||||
|
public val pi: Radians = Radians(PI)
|
||||||
|
public val piTimes2: Radians = Radians(PI * 2)
|
||||||
|
public val piDiv2: Radians = Radians(PI / 2)
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -28,12 +37,16 @@ public value class Radians(public val value: Double) : Angle {
|
|||||||
override fun toRadians(): Radians = this
|
override fun toRadians(): Radians = this
|
||||||
override fun toDegrees(): Degrees = Degrees(value * 180 / PI)
|
override fun toDegrees(): Degrees = Degrees(value * 180 / PI)
|
||||||
|
|
||||||
public override fun plus(other: Angle): Radians = Radians(value + other.toRadians().value)
|
public override fun plus(other: Angle): Radians = Radians(value + other.radians)
|
||||||
public override fun minus(other: Angle): Radians = Radians(value - other.toRadians().value)
|
public override fun minus(other: Angle): Radians = Radians(value - other.radians)
|
||||||
|
|
||||||
public override fun times(other: Number): Radians = Radians(value + other.toDouble())
|
public override fun times(other: Number): Radians = Radians(value * other.toDouble())
|
||||||
public override fun div(other: Number): Radians = Radians(value / other.toDouble())
|
public override fun div(other: Number): Radians = Radians(value / other.toDouble())
|
||||||
|
override fun div(other: Angle): Double = value / other.radians
|
||||||
|
|
||||||
public override fun unaryMinus(): Radians = Radians(-value)
|
public override fun unaryMinus(): Radians = Radians(-value)
|
||||||
|
|
||||||
|
override fun compareTo(other: Angle): Int = value.compareTo(other.radians)
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun sin(angle: Angle): Double = kotlin.math.sin(angle.toRadians().value)
|
public fun sin(angle: Angle): Double = kotlin.math.sin(angle.toRadians().value)
|
||||||
@ -42,6 +55,8 @@ public fun tan(angle: Angle): Double = kotlin.math.tan(angle.toRadians().value)
|
|||||||
|
|
||||||
public val Number.radians: Radians get() = Radians(toDouble())
|
public val Number.radians: Radians get() = Radians(toDouble())
|
||||||
|
|
||||||
|
public val Angle.radians: Double get() = toRadians().value
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Type safe degrees
|
* Type safe degrees
|
||||||
*/
|
*/
|
||||||
@ -50,30 +65,26 @@ public value class Degrees(public val value: Double) : Angle {
|
|||||||
override fun toRadians(): Radians = Radians(value * PI / 180)
|
override fun toRadians(): Radians = Radians(value * PI / 180)
|
||||||
override fun toDegrees(): Degrees = this
|
override fun toDegrees(): Degrees = this
|
||||||
|
|
||||||
public override fun plus(other: Angle): Degrees = Degrees(value + other.toDegrees().value)
|
public override fun plus(other: Angle): Degrees = Degrees(value + other.degrees)
|
||||||
public override fun minus(other: Angle): Degrees = Degrees(value - other.toDegrees().value)
|
public override fun minus(other: Angle): Degrees = Degrees(value - other.degrees)
|
||||||
|
|
||||||
public override fun times(other: Number): Degrees = Degrees(value + other.toDouble())
|
public override fun times(other: Number): Degrees = Degrees(value * other.toDouble())
|
||||||
public override fun div(other: Number): Degrees = Degrees(value / other.toDouble())
|
public override fun div(other: Number): Degrees = Degrees(value / other.toDouble())
|
||||||
|
override fun div(other: Angle): Double = value / other.degrees
|
||||||
|
|
||||||
public override fun unaryMinus(): Degrees = Degrees(-value)
|
public override fun unaryMinus(): Degrees = Degrees(-value)
|
||||||
|
|
||||||
|
override fun compareTo(other: Angle): Int = value.compareTo(other.degrees)
|
||||||
}
|
}
|
||||||
|
|
||||||
public val Number.degrees: Degrees get() = Degrees(toDouble())
|
public val Number.degrees: Degrees get() = Degrees(toDouble())
|
||||||
|
|
||||||
|
public val Angle.degrees: Double get() = toDegrees().value
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A holder class for Pi representation in radians and degrees
|
* Normalized angle 2 PI range symmetric around [center]. By default, uses (0, 2PI) range.
|
||||||
*/
|
*/
|
||||||
public object Pi {
|
public fun Angle.normalized(center: Angle = Angle.pi): Angle =
|
||||||
public val radians: Radians = Radians(PI)
|
this - Angle.piTimes2 * floor((radians + PI - center.radians) / PI / 2)
|
||||||
public val degrees: Degrees = radians.toDegrees()
|
|
||||||
}
|
|
||||||
|
|
||||||
public object PiTimes2 {
|
public fun abs(angle: Angle): Angle = if (angle < Angle.zero) -angle else angle
|
||||||
public val radians: Radians = Radians(2 * PI)
|
|
||||||
public val degrees: Degrees = radians.toDegrees()
|
|
||||||
}
|
|
||||||
|
|
||||||
public object PiDiv2 {
|
|
||||||
public val radians: Radians = Radians(PI / 2)
|
|
||||||
public val degrees: Degrees = radians.toDegrees()
|
|
||||||
}
|
|
@ -0,0 +1,16 @@
|
|||||||
|
package space.kscience.kmath.geometry
|
||||||
|
|
||||||
|
import kotlin.test.Test
|
||||||
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
|
class AngleTest {
|
||||||
|
@Test
|
||||||
|
fun normalization() {
|
||||||
|
assertEquals(30.degrees, 390.degrees.normalized())
|
||||||
|
assertEquals(30.degrees, (-330).degrees.normalized())
|
||||||
|
assertEquals(200.degrees, 200.degrees.normalized())
|
||||||
|
assertEquals(30.degrees, 390.degrees.normalized(Angle.zero))
|
||||||
|
assertEquals(30.degrees, (-330).degrees.normalized(Angle.zero))
|
||||||
|
assertEquals((-160).degrees, 200.degrees.normalized(Angle.zero))
|
||||||
|
}
|
||||||
|
}
|
@ -5,13 +5,9 @@
|
|||||||
|
|
||||||
package space.kscience.kmath.trajectory
|
package space.kscience.kmath.trajectory
|
||||||
|
|
||||||
import space.kscience.kmath.geometry.Circle2D
|
import space.kscience.kmath.geometry.*
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
import kotlin.math.PI
|
|
||||||
import kotlin.math.acos
|
import kotlin.math.acos
|
||||||
import kotlin.math.cos
|
|
||||||
import kotlin.math.sin
|
|
||||||
|
|
||||||
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
|
||||||
|
|
||||||
@ -65,12 +61,11 @@ private fun innerTangent(
|
|||||||
with(Euclidean2DSpace) {
|
with(Euclidean2DSpace) {
|
||||||
val centers = StraightTrajectory2D(base.center, direction.center)
|
val centers = StraightTrajectory2D(base.center, direction.center)
|
||||||
if (centers.length < base.radius * 2) return null
|
if (centers.length < base.radius * 2) return null
|
||||||
val angle = theta(
|
val angle = when (side) {
|
||||||
when (side) {
|
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians
|
||||||
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length)
|
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians
|
||||||
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length)
|
}.normalized()
|
||||||
}
|
|
||||||
)
|
|
||||||
val dX = base.radius * sin(angle)
|
val dX = base.radius * sin(angle)
|
||||||
val dY = base.radius * cos(angle)
|
val dY = base.radius * cos(angle)
|
||||||
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
val p1 = vector(base.center.x + dX, base.center.y + dY)
|
||||||
@ -78,8 +73,6 @@ private fun innerTangent(
|
|||||||
return StraightTrajectory2D(p1, p2)
|
return StraightTrajectory2D(p1, p2)
|
||||||
}
|
}
|
||||||
|
|
||||||
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
|
|
||||||
|
|
||||||
|
|
||||||
@Suppress("DuplicatedCode")
|
@Suppress("DuplicatedCode")
|
||||||
public object DubinsPath {
|
public object DubinsPath {
|
||||||
@ -91,8 +84,8 @@ public object DubinsPath {
|
|||||||
/**
|
/**
|
||||||
* Return Dubins trajectory type or null if trajectory is not a Dubins path
|
* Return Dubins trajectory type or null if trajectory is not a Dubins path
|
||||||
*/
|
*/
|
||||||
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type?{
|
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type? {
|
||||||
if(trajectory2D.segments.size != 3) return null
|
if (trajectory2D.segments.size != 3) return null
|
||||||
val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
|
val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
|
||||||
val b = trajectory2D.segments[1]
|
val b = trajectory2D.segments[1]
|
||||||
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
|
||||||
@ -129,13 +122,13 @@ public object DubinsPath {
|
|||||||
if (centers.length > turningRadius * 4) return null
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
val firstVariant = run {
|
val firstVariant = run {
|
||||||
var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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.bearing + acos(centers.length / (turningRadius * 4)))
|
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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)
|
||||||
@ -146,13 +139,13 @@ public object DubinsPath {
|
|||||||
}
|
}
|
||||||
|
|
||||||
val secondVariant = run {
|
val secondVariant = run {
|
||||||
var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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.bearing - acos(centers.length / (turningRadius * 4)))
|
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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)
|
||||||
@ -173,13 +166,13 @@ public object DubinsPath {
|
|||||||
if (centers.length > turningRadius * 4) return null
|
if (centers.length > turningRadius * 4) return null
|
||||||
|
|
||||||
val firstVariant = run {
|
val firstVariant = run {
|
||||||
var theta = theta(centers.bearing + acos(centers.length / (turningRadius * 4)))
|
var theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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.bearing - acos(centers.length / (turningRadius * 4)))
|
theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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)
|
||||||
@ -190,13 +183,13 @@ public object DubinsPath {
|
|||||||
}
|
}
|
||||||
|
|
||||||
val secondVariant = run {
|
val secondVariant = run {
|
||||||
var theta = theta(centers.bearing - acos(centers.length / (turningRadius * 4)))
|
var theta = (centers.bearing - acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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.bearing + acos(centers.length / (turningRadius * 4)))
|
theta = (centers.bearing + acos(centers.length / (turningRadius * 4)).radians).normalized()
|
||||||
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)
|
||||||
|
@ -12,9 +12,7 @@ import kotlinx.serialization.UseSerializers
|
|||||||
import kotlinx.serialization.descriptors.SerialDescriptor
|
import kotlinx.serialization.descriptors.SerialDescriptor
|
||||||
import kotlinx.serialization.encoding.Decoder
|
import kotlinx.serialization.encoding.Decoder
|
||||||
import kotlinx.serialization.encoding.Encoder
|
import kotlinx.serialization.encoding.Encoder
|
||||||
import space.kscience.kmath.geometry.DoubleVector2D
|
import space.kscience.kmath.geometry.*
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
|
||||||
import space.kscience.kmath.geometry.Vector
|
|
||||||
import kotlin.math.atan2
|
import kotlin.math.atan2
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -23,7 +21,7 @@ import kotlin.math.atan2
|
|||||||
@Serializable(DubinsPose2DSerializer::class)
|
@Serializable(DubinsPose2DSerializer::class)
|
||||||
public interface DubinsPose2D : DoubleVector2D {
|
public interface DubinsPose2D : DoubleVector2D {
|
||||||
public val coordinates: DoubleVector2D
|
public val coordinates: DoubleVector2D
|
||||||
public val bearing: Double
|
public val bearing: Angle
|
||||||
}
|
}
|
||||||
|
|
||||||
@Serializable
|
@Serializable
|
||||||
@ -31,14 +29,14 @@ public class PhaseVector2D(
|
|||||||
override val coordinates: DoubleVector2D,
|
override val coordinates: DoubleVector2D,
|
||||||
public val velocity: DoubleVector2D,
|
public val velocity: DoubleVector2D,
|
||||||
) : DubinsPose2D, DoubleVector2D by coordinates {
|
) : DubinsPose2D, DoubleVector2D by coordinates {
|
||||||
override val bearing: Double get() = atan2(velocity.x, velocity.y)
|
override val bearing: Angle get() = atan2(velocity.x, velocity.y).radians
|
||||||
}
|
}
|
||||||
|
|
||||||
@Serializable
|
@Serializable
|
||||||
@SerialName("DubinsPose2D")
|
@SerialName("DubinsPose2D")
|
||||||
private class DubinsPose2DImpl(
|
private class DubinsPose2DImpl(
|
||||||
override val coordinates: DoubleVector2D,
|
override val coordinates: DoubleVector2D,
|
||||||
override val bearing: Double,
|
override val bearing: Angle,
|
||||||
) : DubinsPose2D, DoubleVector2D by coordinates{
|
) : DubinsPose2D, DoubleVector2D by coordinates{
|
||||||
|
|
||||||
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
|
override fun toString(): String = "DubinsPose2D(x=$x, y=$y, bearing=$bearing)"
|
||||||
@ -60,4 +58,4 @@ public object DubinsPose2DSerializer: KSerializer<DubinsPose2D>{
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Double): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)
|
public fun DubinsPose2D(coordinate: DoubleVector2D, theta: Angle): DubinsPose2D = DubinsPose2DImpl(coordinate, theta)
|
@ -3,15 +3,13 @@
|
|||||||
* Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file.
|
* 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)
|
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
|
||||||
|
|
||||||
package space.kscience.kmath.trajectory
|
package space.kscience.kmath.trajectory
|
||||||
|
|
||||||
import kotlinx.serialization.Serializable
|
import kotlinx.serialization.Serializable
|
||||||
import kotlinx.serialization.UseSerializers
|
import kotlinx.serialization.UseSerializers
|
||||||
import space.kscience.kmath.geometry.Circle2D
|
import space.kscience.kmath.geometry.*
|
||||||
import space.kscience.kmath.geometry.DoubleVector2D
|
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
|
||||||
import kotlin.math.PI
|
|
||||||
import kotlin.math.atan2
|
import kotlin.math.atan2
|
||||||
|
|
||||||
@Serializable
|
@Serializable
|
||||||
@ -29,7 +27,7 @@ public data class StraightTrajectory2D(
|
|||||||
) : Trajectory2D {
|
) : Trajectory2D {
|
||||||
override val length: Double get() = start.distanceTo(end)
|
override val length: Double get() = start.distanceTo(end)
|
||||||
|
|
||||||
public val bearing: Double get() = theta(atan2(end.x - start.x, end.y - start.y))
|
public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized()
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -49,26 +47,25 @@ public data class CircleTrajectory2D(
|
|||||||
/**
|
/**
|
||||||
* Arc length in radians
|
* Arc length in radians
|
||||||
*/
|
*/
|
||||||
val arcLength: Double
|
val arcLength: Angle
|
||||||
get() = theta(
|
get() = if (direction == Direction.LEFT) {
|
||||||
if (direction == Direction.LEFT) {
|
start.bearing - end.bearing
|
||||||
start.bearing - end.bearing
|
} else {
|
||||||
} else {
|
end.bearing - start.bearing
|
||||||
end.bearing - start.bearing
|
}.normalized()
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
override val length: Double by lazy {
|
override val length: Double by lazy {
|
||||||
circle.radius * arcLength
|
circle.radius * arcLength.radians
|
||||||
}
|
}
|
||||||
|
|
||||||
public val direction: Direction by lazy {
|
public val direction: Direction by lazy {
|
||||||
if (start.y < circle.center.y) {
|
if (start.y < circle.center.y) {
|
||||||
if (start.bearing > PI) Direction.RIGHT else Direction.LEFT
|
if (start.bearing > Angle.pi) Direction.RIGHT else Direction.LEFT
|
||||||
} else if (start.y > circle.center.y) {
|
} else if (start.y > circle.center.y) {
|
||||||
if (start.bearing < PI) Direction.RIGHT else Direction.LEFT
|
if (start.bearing < Angle.pi) Direction.RIGHT else Direction.LEFT
|
||||||
} else {
|
} else {
|
||||||
if (start.bearing == 0.0) {
|
if (start.bearing == Angle.zero) {
|
||||||
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
|
||||||
@ -85,13 +82,13 @@ public data class CircleTrajectory2D(
|
|||||||
): CircleTrajectory2D {
|
): CircleTrajectory2D {
|
||||||
fun calculatePose(
|
fun calculatePose(
|
||||||
vector: DoubleVector2D,
|
vector: DoubleVector2D,
|
||||||
theta: Double,
|
theta: Angle,
|
||||||
direction: Direction,
|
direction: Direction,
|
||||||
): DubinsPose2D = DubinsPose2D(
|
): DubinsPose2D = DubinsPose2D(
|
||||||
vector,
|
vector,
|
||||||
when (direction) {
|
when (direction) {
|
||||||
Direction.LEFT -> theta(theta - PI / 2)
|
Direction.LEFT -> (theta - Angle.piDiv2).normalized()
|
||||||
Direction.RIGHT -> theta(theta + PI / 2)
|
Direction.RIGHT -> (theta + Angle.piDiv2).normalized()
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|
||||||
@ -100,7 +97,7 @@ public data class CircleTrajectory2D(
|
|||||||
val pose1 = calculatePose(start, s1.bearing, direction)
|
val pose1 = calculatePose(start, s1.bearing, direction)
|
||||||
val pose2 = calculatePose(end, s2.bearing, direction)
|
val pose2 = calculatePose(end, s2.bearing, direction)
|
||||||
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
|
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
|
||||||
if(trajectory.direction != direction){
|
if (trajectory.direction != direction) {
|
||||||
error("Trajectory direction mismatch")
|
error("Trajectory direction mismatch")
|
||||||
}
|
}
|
||||||
return trajectory
|
return trajectory
|
||||||
@ -113,5 +110,6 @@ public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Tr
|
|||||||
override val length: Double get() = segments.sumOf { it.length }
|
override val length: Double get() = segments.sumOf { it.length }
|
||||||
}
|
}
|
||||||
|
|
||||||
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D = CompositeTrajectory2D(segments.toList())
|
public fun CompositeTrajectory2D(vararg segments: Trajectory2D): CompositeTrajectory2D =
|
||||||
|
CompositeTrajectory2D(segments.toList())
|
||||||
|
|
||||||
|
@ -6,19 +6,21 @@
|
|||||||
package space.kscience.kmath.trajectory
|
package space.kscience.kmath.trajectory
|
||||||
|
|
||||||
import space.kscience.kmath.geometry.Euclidean2DSpace
|
import space.kscience.kmath.geometry.Euclidean2DSpace
|
||||||
|
import space.kscience.kmath.geometry.radians
|
||||||
|
import space.kscience.kmath.geometry.sin
|
||||||
import kotlin.math.PI
|
import kotlin.math.PI
|
||||||
import kotlin.math.abs
|
import kotlin.math.abs
|
||||||
import kotlin.math.sin
|
|
||||||
|
|
||||||
const val maxFloatDelta = 0.000001
|
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 DubinsPose2D.equalsFloat(other: DubinsPose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.equalFloat(other.bearing)
|
fun DubinsPose2D.equalsFloat(other: DubinsPose2D) =
|
||||||
|
x.equalFloat(other.x) && y.equalFloat(other.y) && bearing.radians.equalFloat(other.bearing.radians)
|
||||||
|
|
||||||
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start)
|
fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start)
|
||||||
fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace){
|
fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) {
|
||||||
val dX = width * sin(inverse().bearing)
|
val dX = width * sin(inverse().bearing)
|
||||||
val dY = width * sin(bearing)
|
val dY = width * sin(bearing)
|
||||||
|
|
||||||
|
@ -8,8 +8,8 @@ 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.geometry.degrees
|
||||||
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
import space.kscience.kmath.trajectory.CircleTrajectory2D
|
||||||
import space.kscience.kmath.trajectory.radiansToDegrees
|
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
import kotlin.test.assertEquals
|
import kotlin.test.assertEquals
|
||||||
|
|
||||||
@ -20,7 +20,7 @@ class ArcTests {
|
|||||||
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
val circle = Circle2D(vector(0.0, 0.0), 2.0)
|
||||||
val arc = CircleTrajectory2D.of(circle.center, vector(-2.0, 0.0), vector(0.0, 2.0), CircleTrajectory2D.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.bearing.radiansToDegrees())
|
assertEquals(0.0, arc.start.bearing.degrees)
|
||||||
assertEquals(90.0, arc.end.bearing.radiansToDegrees())
|
assertEquals(90.0, arc.end.bearing.degrees)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -6,8 +6,8 @@
|
|||||||
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.geometry.degrees
|
||||||
import space.kscience.kmath.trajectory.StraightTrajectory2D
|
import space.kscience.kmath.trajectory.StraightTrajectory2D
|
||||||
import space.kscience.kmath.trajectory.radiansToDegrees
|
|
||||||
import kotlin.math.pow
|
import kotlin.math.pow
|
||||||
import kotlin.math.sqrt
|
import kotlin.math.sqrt
|
||||||
import kotlin.test.Test
|
import kotlin.test.Test
|
||||||
@ -19,19 +19,19 @@ class LineTests {
|
|||||||
fun lineTest() = with(Euclidean2DSpace){
|
fun lineTest() = with(Euclidean2DSpace){
|
||||||
val straight = StraightTrajectory2D(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.bearing.radiansToDegrees())
|
assertEquals(45.0, straight.bearing.degrees)
|
||||||
}
|
}
|
||||||
|
|
||||||
@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 = StraightTrajectory2D(zero, vector(0.0, 2.0))
|
val north = StraightTrajectory2D(zero, vector(0.0, 2.0))
|
||||||
assertEquals(0.0, north.bearing.radiansToDegrees())
|
assertEquals(0.0, north.bearing.degrees)
|
||||||
val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
|
val east = StraightTrajectory2D(zero, vector(2.0, 0.0))
|
||||||
assertEquals(90.0, east.bearing.radiansToDegrees())
|
assertEquals(90.0, east.bearing.degrees)
|
||||||
val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
|
val south = StraightTrajectory2D(zero, vector(0.0, -2.0))
|
||||||
assertEquals(180.0, south.bearing.radiansToDegrees())
|
assertEquals(180.0, south.bearing.degrees)
|
||||||
val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
|
val west = StraightTrajectory2D(zero, vector(-2.0, 0.0))
|
||||||
assertEquals(270.0, west.bearing.radiansToDegrees())
|
assertEquals(270.0, west.bearing.degrees)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user