kscience/maps-kt
8
0
Fork 0
Code Issues Pull Requests 3 Packages Projects Releases 1 Wiki Activity

intersection of lineSegment and Circle fixed

Browse Source
This commit is contained in:
Artyom Degtyarev 2023-04-28 13:07:18 +03:00
parent a6a5baa352
commit dde5b2c9f7
2 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/obstacleInternal.kt
View File

@ -242,12 +242,17 @@ private fun LineSegment2D.intersectsCircle(circle: Circle2D): Boolean {
(begin.y - end.y) * (end.y - circle.center.y)) (begin.y - end.y) * (end.y - circle.center.y))
val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) - val c = (end.x - circle.center.x).pow(2.0) + (end.y - circle.center.y).pow(2.0) -
circle.radius.pow(2.0) circle.radius.pow(2.0)
val d = b.pow(2.0) - 4 * a * c
val aNormalized = a / (a * a + b * b + c * c)
val bNormalized = b / (a * a + b * b + c * c)
val cNormalized = c / (a * a + b * b + c * c)
val d = bNormalized.pow(2.0) - 4 * aNormalized * cNormalized
if (d < 1e-6) { if (d < 1e-6) {
return false return false
} else { } else {
val t1 = (-b - d.pow(0.5)) * 0.5 / a val t1 = (-bNormalized - d.pow(0.5)) * 0.5 / aNormalized
val t2 = (-b + d.pow(0.5)) * 0.5 / a val t2 = (-bNormalized + d.pow(0.5)) * 0.5 / aNormalized
if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) { if (((0 < t1) and (t1 < 1)) or ((0 < t2) and (t2 < 1))) {
return true return true
} }

17
trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt
View File

@ -99,6 +99,23 @@ class ObstacleTest {
) )
val length = paths.minOf { it.length } val length = paths.minOf { it.length }
} }
@Test
fun fromMapLess() {
val paths = Obstacle.avoidObstacles(
DubinsPose2D(x = 48.4149535516561, y = 299.50862534208703, bearing = 3.401475378237137.degrees),
DubinsPose2D(x = 45.66638489126448, y = 283.00541087567504, bearing = 325.32183928982727.degrees),
0.5,
Obstacle(
Circle2D(vector(x=44.60882236175772, y=289.52640759535935), radius=0.5),
Circle2D(vector(x=45.558751549431164, y=289.71165594902174), radius=0.5),
Circle2D(vector(x=46.590308440141426, y=289.3897500160981), radius=0.5),
Circle2D(vector(x=46.242119397653354, y=287.94964842121634), radius=0.5),
Circle2D(vector(x=44.92318047505464, y=288.0132403305273), radius=0.5)
)
)
val length = paths.minOf { it.length }
}
@Test @Test
fun equalObstacles() { fun equalObstacles() {