forked from kscience/kmath
Cleanup
This commit is contained in:
parent
32769d6906
commit
cdb116fa20
@ -32,7 +32,8 @@ public class DubinsPathFactory(
|
||||
val c1 = base.getRightCircle(turningRadius)
|
||||
val c2 = direction.getRightCircle(turningRadius)
|
||||
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 dY = turningRadius * cos(theta)
|
||||
@ -46,17 +47,15 @@ public class DubinsPathFactory(
|
||||
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.RIGHT)
|
||||
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.LEFT)
|
||||
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.RIGHT)
|
||||
DubinsPath(a1, a2, a3)
|
||||
} else {
|
||||
null
|
||||
}
|
||||
return DubinsPath(a1, a2, a3)
|
||||
}
|
||||
|
||||
private val lrl: DubinsPath? get () {
|
||||
val c1 = base.getLeftCircle(turningRadius)
|
||||
val c2 = direction.getLeftCircle(turningRadius)
|
||||
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 dY = turningRadius * cos(theta)
|
||||
@ -70,13 +69,10 @@ public class DubinsPathFactory(
|
||||
val a1 = Arc(c1.center, turningRadius, base, p1, Arc.Direction.LEFT)
|
||||
val a2 = Arc(e.center, turningRadius, p1, p2, Arc.Direction.RIGHT)
|
||||
val a3 = Arc(c2.center, turningRadius, p2, direction, Arc.Direction.LEFT)
|
||||
DubinsPath(a1, a2, a3)
|
||||
} else {
|
||||
null
|
||||
}
|
||||
return DubinsPath(a1, a2, a3)
|
||||
}
|
||||
|
||||
public val rsr: DubinsPath? get () {
|
||||
public val rsr: DubinsPath get () {
|
||||
val c1 = base.getRightCircle(turningRadius)
|
||||
val c2 = direction.getRightCircle(turningRadius)
|
||||
val l = leftOuterTangent(c1, c2)
|
||||
@ -99,26 +95,22 @@ public class DubinsPathFactory(
|
||||
val c1 = base.getRightCircle(turningRadius)
|
||||
val c2 = direction.getLeftCircle(turningRadius)
|
||||
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)
|
||||
DubinsPath(a1, LineSegment(l), a3)
|
||||
} else {
|
||||
null
|
||||
}
|
||||
return DubinsPath(a1, LineSegment(l), a3)
|
||||
}
|
||||
|
||||
public val lsr: DubinsPath? get () {
|
||||
val c1 = base.getLeftCircle(turningRadius)
|
||||
val c2 = direction.getRightCircle(turningRadius)
|
||||
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)
|
||||
DubinsPath(a1, LineSegment(l), a3)
|
||||
} else {
|
||||
null
|
||||
}
|
||||
return DubinsPath(a1, LineSegment(l), a3)
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user