teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Update type-safe angles

Browse Source
This commit is contained in:
Alexander Nozik 2023-02-05 20:05:53 +03:00
parent 0366a69123
commit 2c6d1e89c5
9 changed files with 104 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -10,6 +10,7 @@
- Algebra now has an obligatory `bufferFactory` (#477).
### Changed
- Trajectory use type-safe angles
- Tensor operations switched to prefix notation
- Row-wise and column-wise ND shapes in the core
- Shape is read-only

53
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/angles.kt
View File

@ -7,8 +7,9 @@ package space.kscience.kmath.geometry
import kotlin.jvm.JvmInline
import kotlin.math.PI
import kotlin.math.floor
public sealed interface Angle {
public sealed interface Angle : Comparable<Angle> {
public fun toRadians(): Radians
public fun toDegrees(): Degrees
@ -17,7 +18,15 @@ public sealed interface Angle {
public operator fun times(other: Number): Angle
public operator fun div(other: Number): Angle
public operator fun div(other: Angle): Double
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 toDegrees(): Degrees = Degrees(value * 180 / PI)
public override fun plus(other: Angle): Radians = Radians(value + other.toRadians().value)
public override fun minus(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.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())
override fun div(other: Angle): Double = value / other.radians
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)
@ -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 Angle.radians: Double get() = toRadians().value
/**
* 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 toDegrees(): Degrees = this
public override fun plus(other: Angle): Degrees = Degrees(value + other.toDegrees().value)
public override fun minus(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.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())
override fun div(other: Angle): Double = value / other.degrees
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 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 val radians: Radians = Radians(PI)
public val degrees: Degrees = radians.toDegrees()
}
public fun Angle.normalized(center: Angle = Angle.pi): Angle =
this - Angle.piTimes2 * floor((radians + PI - center.radians) / PI / 2)
public object PiTimes2 {
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()
}
public fun abs(angle: Angle): Angle = if (angle < Angle.zero) -angle else angle

16
kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/AngleTest.kt Normal file
View File

@ -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))
}
}

39
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
View File

@ -5,13 +5,9 @@
package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
internal fun DubinsPose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
@ -65,12 +61,11 @@ private fun innerTangent(
with(Euclidean2DSpace) {
val centers = StraightTrajectory2D(base.center, direction.center)
if (centers.length < base.radius * 2) return null
val angle = theta(
when (side) {
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length)
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length)
}
)
val angle = when (side) {
CircleTrajectory2D.Direction.LEFT -> centers.bearing + acos(base.radius * 2 / centers.length).radians
CircleTrajectory2D.Direction.RIGHT -> centers.bearing - acos(base.radius * 2 / centers.length).radians
}.normalized()
val dX = base.radius * sin(angle)
val dY = base.radius * cos(angle)
val p1 = vector(base.center.x + dX, base.center.y + dY)
@ -78,8 +73,6 @@ private fun innerTangent(
return StraightTrajectory2D(p1, p2)
}
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
@Suppress("DuplicatedCode")
public object DubinsPath {
@ -91,8 +84,8 @@ public object DubinsPath {
/**
* Return Dubins trajectory type or null if trajectory is not a Dubins path
*/
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type?{
if(trajectory2D.segments.size != 3) return null
public fun trajectoryTypeOf(trajectory2D: CompositeTrajectory2D): Type? {
if (trajectory2D.segments.size != 3) return null
val a = trajectory2D.segments.first() as? CircleTrajectory2D ?: return null
val b = trajectory2D.segments[1]
val c = trajectory2D.segments.last() as? CircleTrajectory2D ?: return null
@ -129,13 +122,13 @@ public object DubinsPath {
if (centers.length > turningRadius * 4) return null
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 dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
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)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
@ -146,13 +139,13 @@ public object DubinsPath {
}
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 dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
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)
dY = turningRadius * cos(theta)
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
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 dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
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)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)
@ -190,13 +183,13 @@ public object DubinsPath {
}
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 dY = turningRadius * cos(theta)
val p = vector(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle2D(p, turningRadius)
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)
dY = turningRadius * cos(theta)
val p2 = vector(e.center.x + dX, e.center.y + dY)

12
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPose2D.kt
View File

@ -12,9 +12,7 @@ import kotlinx.serialization.UseSerializers
import kotlinx.serialization.descriptors.SerialDescriptor
import kotlinx.serialization.encoding.Decoder
import kotlinx.serialization.encoding.Encoder
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Vector
import space.kscience.kmath.geometry.*
import kotlin.math.atan2
/**
@ -23,7 +21,7 @@ import kotlin.math.atan2
@Serializable(DubinsPose2DSerializer::class)
public interface DubinsPose2D : DoubleVector2D {
public val coordinates: DoubleVector2D
public val bearing: Double
public val bearing: Angle
}
@Serializable
@ -31,14 +29,14 @@ public class PhaseVector2D(
override val coordinates: DoubleVector2D,
public val velocity: DoubleVector2D,
) : 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
@SerialName("DubinsPose2D")
private class DubinsPose2DImpl(
override val coordinates: DoubleVector2D,
override val bearing: Double,
override val bearing: Angle,
) : DubinsPose2D, DoubleVector2D by coordinates{
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)

36
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt
View File

@ -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.
*/
@file:UseSerializers(Euclidean2DSpace.VectorSerializer::class)
package space.kscience.kmath.trajectory
import kotlinx.serialization.Serializable
import kotlinx.serialization.UseSerializers
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.DoubleVector2D
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import kotlin.math.PI
import kotlin.math.atan2
@Serializable
@ -29,7 +27,7 @@ public data class StraightTrajectory2D(
) : Trajectory2D {
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
*/
val arcLength: Double
get() = theta(
if (direction == Direction.LEFT) {
val arcLength: Angle
get() = if (direction == Direction.LEFT) {
start.bearing - end.bearing
} else {
end.bearing - start.bearing
}
)
}.normalized()
override val length: Double by lazy {
circle.radius * arcLength
circle.radius * arcLength.radians
}
public val direction: Direction by lazy {
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) {
if (start.bearing < PI) Direction.RIGHT else Direction.LEFT
if (start.bearing < Angle.pi) Direction.RIGHT else Direction.LEFT
} else {
if (start.bearing == 0.0) {
if (start.bearing == Angle.zero) {
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
} else {
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
@ -85,13 +82,13 @@ public data class CircleTrajectory2D(
): CircleTrajectory2D {
fun calculatePose(
vector: DoubleVector2D,
theta: Double,
theta: Angle,
direction: Direction,
): DubinsPose2D = DubinsPose2D(
vector,
when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
Direction.RIGHT -> theta(theta + PI / 2)
Direction.LEFT -> (theta - Angle.piDiv2).normalized()
Direction.RIGHT -> (theta + Angle.piDiv2).normalized()
}
)
@ -100,7 +97,7 @@ public data class CircleTrajectory2D(
val pose1 = calculatePose(start, s1.bearing, direction)
val pose2 = calculatePose(end, s2.bearing, direction)
val trajectory = CircleTrajectory2D(Circle2D(center, s1.length), pose1, pose2)
if(trajectory.direction != direction){
if (trajectory.direction != direction) {
error("Trajectory direction mismatch")
}
return trajectory
@ -113,5 +110,6 @@ public class CompositeTrajectory2D(public val segments: List<Trajectory2D>) : Tr
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())

8
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt
View File

@ -6,19 +6,21 @@
package space.kscience.kmath.trajectory
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.abs
import kotlin.math.sin
const val maxFloatDelta = 0.000001
fun Double.radiansToDegrees() = this * 180 / PI
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.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 dY = width * sin(bearing)

6
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
View File

@ -8,8 +8,8 @@ package space.kscience.kmath.trajectory.segments
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.kmath.trajectory.CircleTrajectory2D
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.test.Test
import kotlin.test.assertEquals
@ -20,7 +20,7 @@ class ArcTests {
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)
assertEquals(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.start.bearing.radiansToDegrees())
assertEquals(90.0, arc.end.bearing.radiansToDegrees())
assertEquals(0.0, arc.start.bearing.degrees)
assertEquals(90.0, arc.end.bearing.degrees)
}
}

12
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
View File

@ -6,8 +6,8 @@
package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.degrees
import space.kscience.kmath.trajectory.StraightTrajectory2D
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.math.pow
import kotlin.math.sqrt
import kotlin.test.Test
@ -19,19 +19,19 @@ class LineTests {
fun lineTest() = with(Euclidean2DSpace){
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(45.0, straight.bearing.radiansToDegrees())
assertEquals(45.0, straight.bearing.degrees)
}
@Test
fun lineAngleTest() = with(Euclidean2DSpace){
//val zero = Vector2D(0.0, 0.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))
assertEquals(90.0, east.bearing.radiansToDegrees())
assertEquals(90.0, east.bearing.degrees)
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))
assertEquals(270.0, west.bearing.radiansToDegrees())
assertEquals(270.0, west.bearing.degrees)
}
}