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.* import kotlin.math.*
internal data class ObstacleNode( internal data class ObstacleConnection(
val obstacle: Obstacle, val obstacle: Obstacle,
val nodeIndex: Int, val nodeIndex: Int,
val direction: Trajectory2D.Direction, val direction: Trajectory2D.Direction,
@ -20,8 +20,8 @@ internal data class ObstacleNode(
internal data class ObstacleTangent( internal data class ObstacleTangent(
val lineSegment: LineSegment2D, val lineSegment: LineSegment2D,
val beginNode: ObstacleNode, val beginNode: ObstacleConnection,
val endNode: ObstacleNode, val endNode: ObstacleConnection,
) : LineSegment2D by lineSegment { ) : LineSegment2D by lineSegment {
val startCircle get() = beginNode.circle val startCircle get() = beginNode.circle
val startDirection get() = beginNode.direction val startDirection get() = beginNode.direction
@ -165,7 +165,7 @@ internal class ObstacleShell(
|| circles.any { circle -> segment.intersectsCircle(circle) } || circles.any { circle -> segment.intersectsCircle(circle) }
override fun intersects(circle: Circle2D): Boolean = 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 * Tangent to next obstacle node in given direction
@ -184,8 +184,8 @@ internal class ObstacleShell(
shell[nextCircleIndex].end, shell[nextCircleIndex].end,
shell[nextCircleIndex].begin shell[nextCircleIndex].begin
), ),
ObstacleNode(this, circleIndex, direction), ObstacleConnection(this, circleIndex, direction),
ObstacleNode(this, nextCircleIndex, direction), ObstacleConnection(this, nextCircleIndex, direction),
) )
} }
@ -221,18 +221,14 @@ internal fun ObstacleShell(vararg circles: Circle2D): ObstacleShell = ObstacleSh
private fun LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean { private fun LineSegment2D.intersectsSegment(other: LineSegment2D): Boolean {
fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { infix fun DoubleVector2D.cross(v2: DoubleVector2D): Double = x * v2.y - y * v2.x
return v1.x * v2.y - v1.y * v2.x infix fun DoubleVector2D.crossSign(v2: DoubleVector2D) = cross(v2).sign
}
return if (crossProduct(other.begin - begin, other.end - begin).sign == return with(Euclidean2DSpace) {
crossProduct(other.begin - end, other.end - end).sign (other.begin - begin) crossSign (other.end - begin) !=
) { (other.begin - end) crossSign (other.end - end) &&
false (begin - other.begin) crossSign (end - other.begin) !=
} else { (begin - other.end) crossSign (end - other.end)
crossProduct(begin - other.begin, end - other.begin).sign != crossProduct(
begin - other.end,
end - other.end
).sign
} }
} }
@ -272,8 +268,8 @@ private fun outerTangents(first: Obstacle, second: Obstacle): Map<DubinsPath.Typ
)) { )) {
val tangent = ObstacleTangent( val tangent = ObstacleTangent(
segment, segment,
ObstacleNode(first, firstCircleIndex, pathType.first), ObstacleConnection(first, firstCircleIndex, pathType.first),
ObstacleNode(second, secondCircleIndex, pathType.third) ObstacleConnection(second, secondCircleIndex, pathType.third)
) )
if (!(first.intersects(tangent)) && !(second.intersects(tangent))) { 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 //We need only the direction of the final segment from this
ObstacleTangent( ObstacleTangent(
LineSegment(start, start), LineSegment(start, start),
ObstacleNode(initialObstacle, 0, i), ObstacleConnection(initialObstacle, 0, i),
ObstacleNode(initialObstacle, 0, i), ObstacleConnection(initialObstacle, 0, i),
) )
) )
) )
@ -524,8 +520,8 @@ internal fun findAllPaths(
tangentPath.tangents + tangentPath.tangents +
ObstacleTangent( ObstacleTangent(
LineSegment(finish, finish), LineSegment(finish, finish),
ObstacleNode(end, 0, j), ObstacleConnection(end, 0, j),
ObstacleNode(end, 0, j) ObstacleConnection(end, 0, j)
) )
) )
}.map { it.toTrajectory() } }.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 space.kscience.kmath.geometry.degrees
import kotlin.test.Test import kotlin.test.Test
import kotlin.test.assertEquals import kotlin.test.assertEquals
import kotlin.test.assertTrue
class ObstacleTest { class ObstacleTest {
@Test @Test
@ -84,19 +85,19 @@ class ObstacleTest {
@Test @Test
fun fromMap() { fun fromMap() {
val paths = Obstacle.avoidObstacles( val paths = Obstacle.avoidObstacles(
DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees), DubinsPose2D(x = 484149.535516561, y = 2995086.2534208703, bearing = 3.401475378237137.degrees),
DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees), DubinsPose2D(x = 456663.8489126448, y = 2830054.1087567504, bearing = 325.32183928982727.degrees),
5000.0, 5000.0,
Obstacle( Obstacle(
Circle2D(vector(x=446088.2236175772, y=2895264.0759535935), 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 = 455587.51549431164, y = 2897116.5594902174), radius = 5000.0),
Circle2D(vector(x=465903.08440141426, y=2893897.500160981), 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 = 462421.19397653354, y = 2879496.4842121634), radius = 5000.0),
Circle2D(vector(x=449231.8047505464, y=2880132.403305273), radius=5000.0) Circle2D(vector(x = 449231.8047505464, y = 2880132.403305273), radius = 5000.0)
) )
) )
assertTrue { paths.isNotEmpty() }
val length = paths.minOf { it.length } val length = paths.minOf { it.length }
} }