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

Small improvement in test classes, theta function

Browse Source
This commit is contained in:
Erik Schouten 2022-07-15 22:12:36 +02:00
parent ada1141738
commit fa6d741869
7 changed files with 60 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt
View File

@ -34,19 +34,19 @@ public class DubinsPathFactory(
val centers = Line2D(c1.center, c2.center) val centers = Line2D(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null if (centers.length > turningRadius * 4) return null
var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta var theta = theta(centers.theta - 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 = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle(p, turningRadius) val e = Circle(p, turningRadius)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta theta = theta(centers.theta + acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT) val a1 = Arc(c1.center, base, p1, Arc.Direction.RIGHT)
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT) val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT)
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT) val a3 = Arc(c2.center, p2, direction, Arc.Direction.RIGHT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -56,19 +56,19 @@ public class DubinsPathFactory(
val centers = Line2D(c1.center, c2.center) val centers = Line2D(c1.center, c2.center)
if (centers.length > turningRadius * 4) return null if (centers.length > turningRadius * 4) return null
var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta var theta = theta(centers.theta + 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 = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
val e = Circle(p, turningRadius) val e = Circle(p, turningRadius)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta theta = theta(centers.theta - acos(centers.length / (turningRadius * 4)))
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.LEFT) val a1 = Arc(c1.center, base, p1, Arc.Direction.LEFT)
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT) val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT) val a3 = Arc(c2.center, p2, direction, Arc.Direction.LEFT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -76,8 +76,8 @@ public class DubinsPathFactory(
val c1 = base.getRightCircle(turningRadius) val c1 = base.getRightCircle(turningRadius)
val c2 = direction.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius)
val l = leftOuterTangent(c1, c2) val l = leftOuterTangent(c1, c2)
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT)
return DubinsPath(a1, LineSegment(l), a3) return DubinsPath(a1, LineSegment(l), a3)
} }
@ -86,8 +86,8 @@ public class DubinsPathFactory(
val c1 = base.getLeftCircle(turningRadius) val c1 = base.getLeftCircle(turningRadius)
val c2 = direction.getLeftCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius)
val l = rightOuterTangent(c1, c2) val l = rightOuterTangent(c1, c2)
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT)
return DubinsPath(a1, LineSegment(l), a3) return DubinsPath(a1, LineSegment(l), a3)
} }
@ -97,8 +97,8 @@ public class DubinsPathFactory(
val l = rightInnerTangent(c1, c2) val l = rightInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT) val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT)
return DubinsPath(a1, LineSegment(l), a3) return DubinsPath(a1, LineSegment(l), a3)
} }
@ -108,8 +108,8 @@ public class DubinsPathFactory(
val l = leftInnerTangent(c1, c2) val l = leftInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT) val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT)
return DubinsPath(a1, LineSegment(l), a3) return DubinsPath(a1, LineSegment(l), a3)
} }
} }
@ -148,10 +148,12 @@ private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(ba
private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? { private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? {
val centers = Line2D(base.center, direction.center) val centers = Line2D(base.center, direction.center)
return if (centers.length > base.radius * 2) { return if (centers.length > base.radius * 2) {
val angle = when (side) { val angle = theta(
when (side) {
SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length) SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length) SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
}.theta }
)
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 = Vector2D(base.center.x + dX, base.center.y + dY) val p1 = Vector2D(base.center.x + dX, base.center.y + dY)

11
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt
View File

@ -1,5 +1,6 @@
package space.kscience.kmath.trajectory.segments package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Line2D
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Circle
@ -8,11 +9,10 @@ import kotlin.math.PI
public class Arc( public class Arc(
center: Vector2D, center: Vector2D,
radius: Double,
a: Vector2D, a: Vector2D,
b: Vector2D, b: Vector2D,
internal val direction: Direction internal val direction: Direction
) : Circle(center, radius), Segment { ) : Circle(center, center.distanceTo(a)), Segment {
private val l1 = Line2D(center, a) private val l1 = Line2D(center, a)
private val l2 = Line2D(center, b) private val l2 = Line2D(center, b)
@ -26,8 +26,7 @@ public class Arc(
LEFT, RIGHT LEFT, RIGHT
} }
private fun calculateAngle() = private fun calculateAngle() = theta(if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta)
(if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta).theta
private fun calculateLength(): Double { private fun calculateLength(): Double {
val proportion = angle / (2 * PI) val proportion = angle / (2 * PI)
@ -36,8 +35,8 @@ public class Arc(
private fun calculatePose(vector: Vector2D, theta: Double): Pose2D = private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
if (direction == Direction.LEFT) { if (direction == Direction.LEFT) {
Pose2D(vector.x, vector.y, (theta - PI / 2).theta) Pose2D(vector.x, vector.y, theta(theta - PI / 2))
} else { } else {
Pose2D(vector.x, vector.y, (theta + PI / 2).theta) Pose2D(vector.x, vector.y, theta(theta + PI / 2))
} }
} }

7
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt
View File

@ -7,7 +7,7 @@ import kotlin.math.PI
import kotlin.math.atan2 import kotlin.math.atan2
import kotlin.math.sqrt import kotlin.math.sqrt
public class LineSegment( public data class LineSegment(
internal val line: Line2D internal val line: Line2D
) : Segment { ) : Segment {
override val length: Double override val length: Double
@ -15,10 +15,9 @@ public class LineSegment(
} }
internal val Line2D.theta: Double internal val Line2D.theta: Double
get() = atan2(direction.x - base.x, direction.y - base.y).theta get() = theta(atan2(direction.x - base.x, direction.y - base.y))
internal val Line2D.length: Double internal val Line2D.length: Double
get() = base.distanceTo(direction) get() = base.distanceTo(direction)
internal val Double.theta: Double internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
get() = (this + (2 * PI)) % (2 * PI)

7
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
View File

@ -4,15 +4,10 @@ import space.kscience.kmath.geometry.Vector2D
import kotlin.math.cos import kotlin.math.cos
import kotlin.math.sin import kotlin.math.sin
public class Pose2D( public data class Pose2D(
override val x: Double, override val x: Double,
override val y: Double, override val y: Double,
public val theta: Double public val theta: Double
) : Vector2D { ) : Vector2D {
internal constructor(vector: Vector2D, theta: Double) : this(vector.x, vector.y, theta) internal constructor(vector: Vector2D, theta: Double) : this(vector.x, vector.y, theta)
override fun toString(): String {
return "Pose2D(x=$x, y=$y, theta=$theta)"
}
} }

2
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
View File

@ -8,7 +8,7 @@ import kotlin.math.PI
import kotlin.math.abs import kotlin.math.abs
import kotlin.math.sin import kotlin.math.sin
private const val maxFloatDelta = 0.000001 const val maxFloatDelta = 0.000001
fun Double.radiansToDegrees() = this * 180 / PI fun Double.radiansToDegrees() = this * 180 / PI

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

@ -10,14 +10,9 @@ class ArcTests {
@Test @Test
fun arcTest() { fun arcTest() {
val center = Vector2D(0.0, 0.0) val circle = Circle(Vector2D(0.0, 0.0), 2.0)
val radius = 2.0 val arc = Arc(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
val expectedCircumference = 12.56637 assertEquals(circle.circumference / 4, arc.length, 1.0)
val circle = Circle(center, radius)
assertEquals(expectedCircumference, circle.circumference, 1.0)
val arc = Arc(center, radius, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
assertEquals(expectedCircumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) assertEquals(0.0, arc.pose1.theta.radiansToDegrees())
assertEquals(90.0, arc.pose2.theta.radiansToDegrees()) assertEquals(90.0, arc.pose2.theta.radiansToDegrees())
} }

23
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/components/CircleTests.kt Normal file
View File

@ -0,0 +1,23 @@
/*
* Copyright 2018-2021 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.segments.components
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.maxFloatDelta
import kotlin.test.Test
import kotlin.test.assertEquals
class CircleTests {
@Test
fun arcTest() {
val center = Vector2D(0.0, 0.0)
val radius = 2.0
val expectedCircumference = 12.56637
val circle = Circle(center, radius)
assertEquals(expectedCircumference, circle.circumference, maxFloatDelta)
}
}