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

Fix segment-circle intersection

Browse Source
This commit is contained in:
Alexander Nozik 2023-05-02 16:17:04 +03:00
parent 8947998e0c
commit 05ef3aa4dd
3 changed files with 25 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt
View File

@ -1,9 +1,10 @@
package space.kscience.kmath.geometry
import space.kscience.kmath.operations.DoubleField.pow
import space.kscience.trajectory.*
import kotlin.math.abs
import kotlin.math.pow
import kotlin.math.sign
import kotlin.math.sqrt
public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D =
Circle2D(vector(x, y), radius = radius.toDouble())
@ -23,17 +24,25 @@ public fun Euclidean2DSpace.intersectsOrInside(circle1: Circle2D, circle2: Circl
* https://mathworld.wolfram.com/Circle-LineIntersection.html
*/
public fun Euclidean2DSpace.intersects(segment: LineSegment2D, circle: Circle2D): Boolean {
val begin = segment.begin
val end = segment.end
val d = begin.distanceTo(end)
val det = (begin.x - circle.center.x) * (end.y - circle.center.y) -
(end.x - circle.center.x) * (begin.y - circle.center.y)
val direction = segment.end - segment.begin
val radiusVector = segment.begin - circle.center
val incidence = circle.radius.pow(2) * d.pow(2) - det.pow(2)
val a = direction dot direction
val b = 2 * (radiusVector dot direction)
val c = (radiusVector dot radiusVector) - circle.radius.pow(2)
return incidence >= 0
val discriminantSquared = b * b - 4 * a * c
if (discriminantSquared < 0) return false
val discriminant = sqrt(discriminantSquared)
val t1 = (-b - discriminant) / (2 * a) // first intersection point in relative coordinates
val t2 = (-b + discriminant) / (2 * a) //second intersection point in relative coordinates
return t1.sign != t2.sign || (t1-1.0).sign != (t2-1).sign
}
public fun Euclidean2DSpace.intersects(circle: Circle2D, segment: LineSegment2D): Boolean =
intersects(segment, circle)

7
trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt
View File

@ -149,7 +149,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
//cutting first and last arcs to accommodate connection points
val first = circumvention.first() as CircleTrajectory2D
val last = circumvention.last() as CircleTrajectory2D
//arc between end of the nangent and end of previous arc (begin of the the next one)
//arc between end of the tangent and end of previous arc (begin of the next one)
circumvention[0] = CircleTrajectory2D(
first.circle,
tangent1.tangentTrajectory.endPose,
@ -182,7 +182,7 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
private fun avoiding(
dubinsPath: CompositeTrajectory2D,
): Collection<Trajectory2D> = with(Euclidean2DSpace) {
//fast return if no obstacles intersect direct path
//fast return if no obstacles intersect the direct path
if (obstacles.none { it.intersects(dubinsPath) }) return listOf(dubinsPath)
val beginArc = dubinsPath.segments.first() as CircleTrajectory2D
@ -208,7 +208,8 @@ public class Obstacles(public val obstacles: List<Obstacle>) {
endArc
) ?: return emptySet()
if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
// if no intersections, finish
if (obstacles.indices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf(
TangentPath(tangents + tangentToEnd)
)

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

@ -89,13 +89,14 @@ class ObstacleTest {
)
val paths: List<Trajectory2D> = Obstacles.avoidObstacles(
Pose2D(-0.9, -0.9, Angle.pi),
Pose2D(-0.9, -0.9, Angle.piDiv2),
Pose2D(-1, -1, Angle.pi),
Pose2D(-1, -1, Angle.piDiv2),
1.0,
obstacle
)
assertTrue { paths.isNotEmpty() }
assertEquals(12.0, paths.minOf { it.length }, 2.0)
assertEquals(9.5, paths.minOf { it.length }, 1.0)
assertEquals(12.5, paths.maxOf { it.length }, 1.0)
}
@Test