# Conflicts:
#	kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/tangent.kt
This commit is contained in:
Artyom Degtyarev 2023-03-04 20:58:35 +03:00
commit 6288eb9d97
4 changed files with 42 additions and 100 deletions

View File

@ -9,7 +9,7 @@ kotlin.native.ignoreDisabledTargets=true
org.gradle.configureondemand=true org.gradle.configureondemand=true
org.gradle.jvmargs=-Xmx4096m org.gradle.jvmargs=-Xmx4096m
toolsVersion=0.14.0-kotlin-1.8.10 toolsVersion=0.14.2-kotlin-1.8.10
org.gradle.parallel=true org.gradle.parallel=true

View File

@ -6,9 +6,6 @@
package space.kscience.kmath.trajectory package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.* import space.kscience.kmath.geometry.*
import space.kscience.kmath.geometry.Euclidean2DSpace.plus
import space.kscience.kmath.geometry.Euclidean2DSpace.times
import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.* import kotlin.math.*
/** /**
@ -17,11 +14,15 @@ import kotlin.math.*
*/ */
public fun Circle2D.tangentsToCircle( public fun Circle2D.tangentsToCircle(
other: Circle2D, other: Circle2D,
): Map<DubinsPath.Type, LineSegment2D> = with(Euclidean2DSpace) { ): Map<DubinsPath.Type, LineSegment<DoubleVector2D>> = with(Euclidean2DSpace) {
//return empty map for concentric circles
if(center.equalsVector(other.center)) return@tangentsToCircle emptyMap()
// A line connecting centers
val line = LineSegment(center, other.center) val line = LineSegment(center, other.center)
val d = line.begin.distanceTo(line.end) // Distance between centers
val distance = line.begin.distanceTo(line.end)
val angle1 = atan2(other.center.x - center.x, other.center.y - center.y) val angle1 = atan2(other.center.x - center.x, other.center.y - center.y)
var r: Double
var angle2: Double var angle2: Double
val routes = mapOf( val routes = mapOf(
DubinsPath.Type.RSR to Pair(radius, other.radius), DubinsPath.Type.RSR to Pair(radius, other.radius),
@ -33,82 +34,27 @@ public fun Circle2D.tangentsToCircle(
for ((route, r1r2) in routes) { for ((route, r1r2) in routes) {
val r1 = r1r2.first val r1 = r1r2.first
val r2 = r1r2.second val r2 = r1r2.second
r = if (r1.sign == r2.sign) { val r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue r1.absoluteValue - r2.absoluteValue
} else { } else {
r1.absoluteValue + r2.absoluteValue r1.absoluteValue + r2.absoluteValue
} }
if (d * d > r * r) { if (distance <= r) TODO("Intersecting circles are not supported yet")
val l = (d * d - r * r).pow(0.5) val l = sqrt(distance * distance - r * r)
angle2 = if (r1.absoluteValue > r2.absoluteValue) { angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l) angle1 + r1.sign * atan2(r.absoluteValue, l)
} else { } else {
angle1 - r2.sign * atan2(r.absoluteValue, l) angle1 - r2.sign * atan2(r.absoluteValue, l)
} }
val w = vector(-cos(angle2), sin(angle2)) val w = vector(-cos(angle2), sin(angle2))
put( put(
route, route,
LineSegment2D( LineSegment(
center + w * r1, center + w * r1,
other.center + w * r2 other.center + w * r2
)
) )
} )
else {
throw Exception("Circles should not intersect")
}
}
}
}
public fun dubinsTangentsToCircles(
firstCircle: Circle2D,
secondCircle: Circle2D,
firstObstacle: DubinsObstacle,
secondObstacle: DubinsObstacle
): Map<DubinsPath.Type, DubinsTangent> = with(Euclidean2DSpace) {
val line = LineSegment(firstCircle.center, secondCircle.center)
val d = line.begin.distanceTo(line.end)
val angle1 = atan2(secondCircle.center.x - firstCircle.center.x,
secondCircle.center.y - firstCircle.center.y)
var r: Double
var angle2: Double
val routes = mapOf(
DubinsPath.Type.RSR to Pair(firstCircle.radius, secondCircle.radius),
DubinsPath.Type.RSL to Pair(firstCircle.radius, -secondCircle.radius),
DubinsPath.Type.LSR to Pair(-firstCircle.radius, secondCircle.radius),
DubinsPath.Type.LSL to Pair(-firstCircle.radius, -secondCircle.radius)
)
return buildMap {
for ((route, r1r2) in routes) {
val r1 = r1r2.first
val r2 = r1r2.second
r = if (r1.sign == r2.sign) {
r1.absoluteValue - r2.absoluteValue
} else {
r1.absoluteValue + r2.absoluteValue
}
if (d * d > r * r) {
val l = (d * d - r * r).pow(0.5)
angle2 = if (r1.absoluteValue > r2.absoluteValue) {
angle1 + r1.sign * atan2(r.absoluteValue, l)
} else {
angle1 - r2.sign * atan2(r.absoluteValue, l)
}
val w = Euclidean2DSpace.vector(-cos(angle2), sin(angle2))
put(route, DubinsTangent(Circle2D(firstCircle.center, firstCircle.radius),
secondCircle,
firstObstacle,
secondObstacle,
LineSegment2D(
firstCircle.center + w * r1,
secondCircle.center + w * r2
),
DubinsPath.toSimpleTypes(route))
)
} else {
throw Exception("Circles should not intersect")
}
} }
} }
} }

View File

@ -13,7 +13,6 @@ import space.kscience.kmath.geometry.equalsLine
import kotlin.test.Test import kotlin.test.Test
import kotlin.test.assertEquals import kotlin.test.assertEquals
import kotlin.test.assertTrue import kotlin.test.assertTrue
import kotlin.test.assertFailsWith
class TangentTest { class TangentTest {
@Test @Test
@ -56,26 +55,23 @@ class TangentTest {
} }
@Test @Test
fun nonExistingTangents() { fun concentric(){
assertFailsWith<Exception> { val c1 = Circle2D(vector(0.0, 0.0), 10.0)
val c1 = Circle2D(vector(0.0, 0.0), 10.0) val c2 = Circle2D(vector(0.0, 0.0), 1.0)
val c2 = Circle2D(vector(0.0, 0.0), 1.0) assertEquals(emptyMap(), c1.tangentsToCircle(c2))
val segments = c1.tangentsToCircle(c2)
}
assertFailsWith<Exception> {
val c1 = Circle2D(vector(0.0, 0.0), 1.0)
val c2 = Circle2D(vector(0.0, 0.0), 10.0)
val segments = c1.tangentsToCircle(c2)
}
assertFailsWith<Exception> {
val c1 = Circle2D(vector(0.0, 0.0), 1.0)
val c2 = Circle2D(vector(2.0, 0.0), 1.0)
val segments = c1.tangentsToCircle(c2)
}
assertFailsWith<Exception> {
val c1 = Circle2D(vector(0.0, 0.0), 1.0)
val c2 = Circle2D(vector(0.5, 0.0), 1.0)
val segments = c1.tangentsToCircle(c2)
}
} }
//
// @Test
// fun nonExistingTangents() {
// assertFailsWith<NotImplementedError> {
// val c1 = Circle2D(vector(0.0, 0.0), 1.0)
// val c2 = Circle2D(vector(2.0, 0.0), 1.0)
// c1.tangentsToCircle(c2)
// }
// assertFailsWith<NotImplementedError> {
// val c1 = Circle2D(vector(0.0, 0.0), 1.0)
// val c2 = Circle2D(vector(0.5, 0.0), 1.0)
// c1.tangentsToCircle(c2)
// }
// }
} }

View File

@ -19,6 +19,6 @@ class CircleTests {
val radius = 2.0 val radius = 2.0
val expectedCircumference = 12.56637 val expectedCircumference = 12.56637
val circle = Circle2D(center, radius) val circle = Circle2D(center, radius)
assertEquals(expectedCircumference, circle.circumference) assertEquals(expectedCircumference, circle.circumference, 1e-4)
} }
} }