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

Rename ObstacleNode to ObstacleConnection

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-26 10:44:13 +03:00
parent a6a5baa352
commit 5e35ce8b5f
2 changed files with 27 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,7 +10,7 @@ import space.kscience.kmath.operations.DoubleField.pow
import kotlin.math.*
internal data class ObstacleNode(
internal data class ObstacleConnection(
val obstacle: Obstacle,
val nodeIndex: Int,
val direction: Trajectory2D.Direction,
@ -20,8 +20,8 @@ internal data class ObstacleNode(
internal data class ObstacleTangent(
val lineSegment: LineSegment2D,
val beginNode: ObstacleNode,
val endNode: ObstacleNode,
val beginNode: ObstacleConnection,
val endNode: ObstacleConnection,
) : LineSegment2D by lineSegment {
val startCircle get() = beginNode.circle
val startDirection get() = beginNode.direction
@ -165,7 +165,7 @@ internal class ObstacleShell(
|| circles.any { circle -> segment.intersectsCircle(circle) }
override fun intersects(circle: Circle2D): Boolean =
shell.any{tangent -> tangent.intersectsCircle(circle) }
shell.any { tangent -> tangent.intersectsCircle(circle) }
/**
* Tangent to next obstacle node in given direction
@ -184,8 +184,8 @@ internal class ObstacleShell(
shell[nextCircleIndex].end,
shell[nextCircleIndex].begin
),
ObstacleNode(this, circleIndex, direction),
ObstacleNode(this, nextCircleIndex, direction),
ObstacleConnection(this, circleIndex, direction),
ObstacleConnection(this, nextCircleIndex, direction),
)
}
@ -221,18 +221,14 @@ internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleSh
private fun LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean {
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double {
return v1.x * v2.y - v1.y * v2.x
}
return if (crossProduct(other.begin - begin, other.end - begin).sign ==
crossProduct(other.begin - end, other.end - end).sign
) {
false
} else {
crossProduct(begin - other.begin, end - other.begin).sign != crossProduct(
begin - other.end,
end - other.end
).sign
infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
return with(Euclidean2DSpace) {
(other.begin - begin) crossSign (other.end - begin) !=
(other.begin - end) crossSign (other.end - end) &&
(begin - other.begin) crossSign (end - other.begin) !=
(begin - other.end) crossSign (end - other.end)
}
}
@ -272,8 +268,8 @@ private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Typ
)) {
val tangent = ObstacleTangent(
segment,
ObstacleNode(first, firstCircleIndex, pathType.first),
ObstacleNode(second, secondCircleIndex, pathType.third)
ObstacleConnection(first, firstCircleIndex, pathType.first),
ObstacleConnection(second, secondCircleIndex, pathType.third)
)
if (!(first.intersects(tangent)) && !(second.intersects(tangent))) {
@ -425,8 +421,8 @@ internal fun findAllPaths(
//We need only the direction of the final segment from this
ObstacleTangent(
LineSegment(start, start),
ObstacleNode(initialObstacle, 0, i),
ObstacleNode(initialObstacle, 0, i),
ObstacleConnection(initialObstacle, 0, i),
ObstacleConnection(initialObstacle, 0, i),
)
)
)
@ -524,8 +520,8 @@ internal fun findAllPaths(
tangentPath.tangents +
ObstacleTangent(
LineSegment(finish, finish),
ObstacleNode(end, 0, j),
ObstacleNode(end, 0, j)
ObstacleConnection(end, 0, j),
ObstacleConnection(end, 0, j)
)
)
}.map { it.toTrajectory() }

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

@ -10,6 +10,7 @@ import space.kscience.kmath.geometry.Euclidean2DSpace.vector
import space.kscience.kmath.geometry.degrees
import kotlin.test.Test
import kotlin.test.assertEquals
import kotlin.test.assertTrue
class ObstacleTest {
@Test
@ -84,19 +85,19 @@ class ObstacleTest {
@Test
fun fromMap() {
val paths = Obstacle.avoidObstacles(
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
5000.0,
Obstacle(
Circle2D(vector(x=446088.2236175772, y=2895264.0759535935), radius=5000.0),
Circle2D(vector(x=455587.51549431164, y=2897116.5594902174), radius=5000.0),
Circle2D(vector(x=465903.08440141426, y=2893897.500160981), radius=5000.0),
Circle2D(vector(x=462421.19397653354, y=2879496.4842121634), radius=5000.0),
Circle2D(vector(x=449231.8047505464, y=2880132.403305273), radius=5000.0)
Circle2D(vector(x = 446088.2236175772, y = 2895264.0759535935), radius = 5000.0),
Circle2D(vector(x = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0),
Circle2D(vector(x = 465903.08440141426, y = 2893897.500160981), radius = 5000.0),
Circle2D(vector(x = 462421.19397653354, y = 2879496.4842121634), radius = 5000.0),
Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0)
)
)
assertTrue { paths.isNotEmpty() }
val length = paths.minOf { it.length }
}