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

@@ -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)
+        return DubinsPath(a1, a2, a3)
-        } else {
-            null
-        }
     }
 
     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)
+        return DubinsPath(a1, a2, a3)
-        } else {
-            null
-        }
     }
 
-    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)
+        return DubinsPath(a1, LineSegment(l), a3)
-        } else {
-            null
-        }
     }
 
     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)
+        return DubinsPath(a1, LineSegment(l), a3)
-        } else {
-            null
-        }
     }
 }