kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

Cleanup

Browse Source
This commit is contained in:
Erik Schouten 2022-07-15 18:55:37 +02:00
parent 32769d6906
commit cdb116fa20
1 changed files with 43 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt
View File

@ -32,51 +32,47 @@ public class DubinsPathFactory(
val c1 = base.getRightCircle(turningRadius) val c1 = base.getRightCircle(turningRadius)
val c2 = direction.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius)
val centers = Line2D(c1.center, c2.center) val centers = Line2D(c1.center, c2.center)
return if (centers.length < turningRadius * 4) { if (centers.length > turningRadius * 4) return null
var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta
var dX = turningRadius * sin(theta) var theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta
var dY = turningRadius * cos(theta) var dX = turningRadius * sin(theta)
val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) var dY = turningRadius * cos(theta)
val e = Circle(p, turningRadius) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) val e = Circle(p, turningRadius)
theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
dX = turningRadius * sin(theta) theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta
dY = turningRadius * cos(theta) dX = turningRadius * sin(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) dY = turningRadius * cos(theta)
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT) val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT) val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT)
DubinsPath(a1, a2, a3) val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT)
} else { return DubinsPath(a1, a2, a3)
null
}
} }
private val lrl: DubinsPath? get () { private val lrl: DubinsPath? get () {
val c1 = base.getLeftCircle(turningRadius) val c1 = base.getLeftCircle(turningRadius)
val c2 = direction.getLeftCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius)
val centers = Line2D(c1.center, c2.center) val centers = Line2D(c1.center, c2.center)
return if (centers.length < turningRadius * 4) { if (centers.length > turningRadius * 4) return null
var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta
var dX = turningRadius * sin(theta) var theta = (centers.theta + acos(centers.length / (turningRadius * 4))).theta
var dY = turningRadius * cos(theta) var dX = turningRadius * sin(theta)
val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2) var dY = turningRadius * cos(theta)
val e = Circle(p, turningRadius) val p = Vector2D(c1.center.x + dX * 2, c1.center.y + dY * 2)
val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY) val e = Circle(p, turningRadius)
theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta val p1 = Vector2D(c1.center.x + dX, c1.center.y + dY)
dX = turningRadius * sin(theta) theta = (centers.theta - acos(centers.length / (turningRadius * 4))).theta
dY = turningRadius * cos(theta) dX = turningRadius * sin(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) dY = turningRadius * cos(theta)
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.LEFT) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT) val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.LEFT)
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT) val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT)
DubinsPath(a1, a2, a3) val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT)
} else { return DubinsPath(a1, a2, a3)
null
}
} }
public val rsr: DubinsPath? get () { public val rsr: DubinsPath get () {
val c1 = base.getRightCircle(turningRadius) val c1 = base.getRightCircle(turningRadius)
val c2 = direction.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius)
val l = leftOuterTangent(c1, c2) val l = leftOuterTangent(c1, c2)
@ -99,26 +95,22 @@ public class DubinsPathFactory(
val c1 = base.getRightCircle(turningRadius) val c1 = base.getRightCircle(turningRadius)
val c2 = direction.getLeftCircle(turningRadius) val c2 = direction.getLeftCircle(turningRadius)
val l = rightInnerTangent(c1, c2) val l = rightInnerTangent(c1, c2)
return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != null) { if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT) val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.RIGHT)
DubinsPath(a1, LineSegment(l), a3) val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.LEFT)
} else { return DubinsPath(a1, LineSegment(l), a3)
null
}
} }
public val lsr: DubinsPath? get () { public val lsr: DubinsPath? get () {
val c1 = base.getLeftCircle(turningRadius) val c1 = base.getLeftCircle(turningRadius)
val c2 = direction.getRightCircle(turningRadius) val c2 = direction.getRightCircle(turningRadius)
val l = leftInnerTangent(c1, c2) val l = leftInnerTangent(c1, c2)
return if (c1.center.distanceTo(c2.center) > turningRadius * 2 && l != null) { if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null
val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT)
val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT) val a1 = Arc(c1.center, turningRadius, base, l.base, Arc.Direction.LEFT)
DubinsPath(a1, LineSegment(l), a3) val a3 = Arc(c2.center, turningRadius, l.direction, direction, Arc.Direction.RIGHT)
} else { return DubinsPath(a1, LineSegment(l), a3)
null
}
} }
} }