进路路径对象调整

进路路径查询方法重写
按计划行车加载列车逻辑调整
进路延续保护侧防逻辑问题修改

src/main/java/club/joylink/rtss/simulation/cbtc/ATS/service/AtsTrainLoadService.java

@@ -205,15 +205,10 @@ public class AtsTrainLoadService {
                                     RoutePath routePath = this.selectDefaultRoutePath(repository, stationPlan.getSection(), nextStationPlan.getSection());
                                     right = routePath.isRight();
                                     int t = current.toSecondOfDay() - stationPlan.getLeaveTime().toSecondOfDay();
-                                    float s = routePath.getLenFromSection(stationPlan.getSection());
+                                    float totalLen = routePath.getLength();
-                                    //右：-最后区段+后右停车点 + 前区段-前右停车点   左：-左停车点+前左停车点
-                                    s= right?
-                                            s-nextStationPlan.getSection().getLen()+nextStationPlan.getSection().getStopPointByDirection(right)+stationPlan.getSection().getLen()-stationPlan.getSection().getStopPointByDirection(right)
-                                            :
-                                            s-nextStationPlan.getSection().getStopPointByDirection(right)+stationPlan.getSection().getStopPointByDirection(right);
                                     int totalTime = nextStationPlan.getArriveTime().toSecondOfDay() - stationPlan.getLeaveTime().toSecondOfDay();
-                                    float moveDistance = s / totalTime * t; // 移动的距离
+                                    float moveDistance = totalLen / totalTime * t; // 移动的距离
-                                    headPosition = routePath.getSectionPositionOfLen(moveDistance,stationPlan.getSection());
+                                    headPosition = routePath.getSectionPositionOfLen(moveDistance);
                                     SectionPosition nextTargetPosition = new SectionPosition(nextStationPlan.getSection(),
                                             nextStationPlan.getSection().getStopPointByDirection(right));
                                     if (headPosition.isAheadOf(nextTargetPosition, right)) {
@@ -431,8 +426,6 @@ public class AtsTrainLoadService {
                     trainInfo.updateEstimatedArriveInfo(endStationPlan.getSection(), endStationPlan.getArriveTime());
                     train.updateNextStationPlan(endStationPlan.getStation(), endStationPlan.getSection(), tripPlan.isPlanParking(endStationPlan));
                     break;
-                } else if (passedSectionStopPoint(headPosition, endStationPlan.getSection(), right)) {
-                    continue;
                 } else {
                     RoutePath routePath = this.selectDefaultRoutePath(repository, startStationPlan.getSection(), endStationPlan.getSection());
                     if (routePath.containsSection(headPosition.getSection())) {
@@ -461,16 +454,6 @@ public class AtsTrainLoadService {
         return false;
     }
 
-
-    private boolean passedSectionStopPoint(SectionPosition headPosition, Section funSection, boolean right) {
-        float d = headPosition.getOffset() - funSection.getStopPointByDirection(right);
-        if (Objects.equals(headPosition.getSection(), funSection)
-                && d > SimulationConstants.PARK_POINT_MAX_OFFSET) {
-            return true;
-        }
-        return false;
-    }
-
     /**
      * 处理碰撞逻辑
      * @param simulation

src/main/java/club/joylink/rtss/simulation/cbtc/CI/service/RouteService.java

@@ -478,10 +478,12 @@ public class RouteService {
             return;
         }
         MapConfig config = simulation.getRepository().getConfig();
-        if (simulation.getSystemTime().isAfter(overlap.getSettingStartTime().plusSeconds(10))) {
+        if (!config.isRouteSettingNoFail()) {
-            overlap.settingFailed();
+            if (simulation.getSystemTime().isAfter(overlap.getSettingStartTime().plusSeconds(10))) {
-            log.info(String.format("进路延续保护[%s]办理失败", overlap.debugStr()));
+                overlap.settingFailed();
-            return;
+                log.info(String.format("进路延续保护[%s]办理失败", overlap.debugStr()));
+                return;
+            }
         }
         boolean right = overlap.isRight();
         SectionPath sectionPath = overlap.selectPath();
@@ -515,26 +517,22 @@ public class RouteService {
             }
         }
         List<SwitchElement> switchList = sectionPath.getSwitchList();
-        if (!overlap.isRequisition()) { // 设备征用
+        List<RouteFls> flsList = sectionPath.getFlsList();
-            // 延续保护位置转动
+        // 延续保护位置转动
-            this.routeSwitchTurn(simulation, switchList);
+        this.routeSwitchTurn(simulation, switchList);
-            List<RouteFls> flsList = sectionPath.getFlsList();
+        this.routeFlsControl(simulation, flsList);
-            this.routeFlsControl(simulation, flsList);
+        if (config.isLockFirst()) {
-            if (config.isLockFirst()) {
+            for (SwitchElement switchElement : switchList) {
-                for (SwitchElement switchElement : switchList) {
+                switchElement.getASwitch().overlapLock();
-                    switchElement.getASwitch().overlapLock();
+            }
-                }
+            if (!CollectionUtils.isEmpty(flsList)) {
-                if (!CollectionUtils.isEmpty(flsList)) {
+                for (RouteFls routeFls : flsList) {
-                    for (RouteFls routeFls : flsList) {
+                    routeFls.lock();
-                        routeFls.lock();
-                    }
                 }
             }
-            overlap.updateRequisition(true);
-            return;
         }
         // 道岔是否转换到位置
-        boolean onPosition = this.checkRouteSwitchPosition(switchList);
+        boolean onPosition = this.checkRouteSwitchPosition(switchList) && this.isFlsSwitchOnPosition(flsList);
         if (!onPosition) return; // 未到位，返回
         if (!overlap.getSection().isRouteLock()) {
             return;
@@ -544,6 +542,26 @@ public class RouteService {
         log.debug(String.format("进路延续保护[%s]办理成功", overlap.debugStr()));
     }
 
+    private boolean isFlsSwitchOnPosition(List<RouteFls> flsList) {
+        if (!CollectionUtils.isEmpty(flsList)) {
+            for (RouteFls routeFls : flsList) {
+                List<RouteFls.FlsElement> level1List = routeFls.getLevel1List();
+                for (RouteFls.FlsElement flsElement : level1List) {
+                    SwitchElement pSwitch = flsElement.getPSwitch();
+                    if (pSwitch != null && !pSwitch.getASwitch().isOnPosition(pSwitch.isNormal())) {
+                        return false;
+                    } else {
+                        SwitchElement fpae = flsElement.getFpae();
+                        if (fpae != null && !fpae.getASwitch().isOnPosition(fpae.isNormal())) {
+                            return false;
+                        }
+                    }
+                }
+            }
+        }
+        return true;
+    }
+
     private boolean checkRouteSwitchPosition(List<SwitchElement> switchList) {
         if (!CollectionUtils.isEmpty(switchList)) {
             for (SwitchElement switchElement : switchList) {

src/main/java/club/joylink/rtss/simulation/cbtc/build/InterlockBuilder2.java

@@ -758,9 +758,6 @@ public class InterlockBuilder2 {
                 errMsgList.add(String.format("站间运行等级[%s]无法找到路径",
                         runLevel.debugStr(), startSection.debugStr(), endSection.debugStr()));
             } else {
-                for (RoutePath routePath : routePaths) {
-                    routePath.calculateDistance();
-                }
                 routePathMap.put(routePaths.get(0).getKey(), routePaths);
             }
         }

src/main/java/club/joylink/rtss/simulation/cbtc/build/RunPlanBuilder.java

@@ -269,7 +269,7 @@ public class RunPlanBuilder {
 
                         StationPlan firstStationPlan = tripPlan.getFirstStationPlan();
                         if (!Objects.equals(startSection.getCode(), firstStationPlan.getSection().getCode()) &&
-                                !Objects.equals(startSection.getDeviceStation().getCode(), firstStationPlan.getStation().getCode())) {
+                                !Objects.equals(startSection.getStation().getCode(), firstStationPlan.getStation().getCode())) {
                             log.warn(String.format("计划车次[%s|%s|%s]起始区段[%s(%s)]错误：既不是第一个车站[%s(%s)]到发计划的停靠区段[%s(%s)]，也不属于第一个到站计划车站",
                                     tripPlan.getServiceNumber(), tripPlan.getTripNumber(),
                                     tripPlan.getDestinationCode(), startSection.getName(),
@@ -280,7 +280,7 @@ public class RunPlanBuilder {
                         }
                         StationPlan lastStationPlan = tripPlan.getLastStationPlan();
                         if (!Objects.equals(endSection.getCode(), lastStationPlan.getSection().getCode()) &&
-                                !Objects.equals(endSection.getDeviceStation().getCode(), lastStationPlan.getStation().getCode())) {
+                                !Objects.equals(endSection.getStation().getCode(), lastStationPlan.getStation().getCode())) {
                             log.warn(String.format("计划车次[%s|%s|%s]结束区段[%s(%s)]错误：既不是最后车站[%s(%s)]到发计划的停靠区段[%s(%s)]，也不属于最后到站的计划车站",
                                     tripPlan.getServiceNumber(), tripPlan.getTripNumber(),
                                     tripPlan.getDestinationCode(), endSection.getName(),

src/main/java/club/joylink/rtss/simulation/cbtc/data/CalculateService.java

@@ -85,7 +85,6 @@ public class CalculateService {
      * 计算从起点到终点距离（单方向）
      */
     public static Float calculateDistance(SectionPosition startPosition, SectionPosition endPosition, boolean right) {
-//        long start = System.currentTimeMillis();
         Section startSection = startPosition.getSection();
         Section endSection = endPosition.getSection();
         float distance = 0;
@@ -119,8 +118,6 @@ public class CalculateService {
                 distance += baseOffset - endPosition.getOffset();
             }
         }
-//        long end = System.currentTimeMillis();
-//        System.out.println(String.format("计算距离用时：%s", (end-start)));
         return distance;
     }
 
@@ -640,99 +637,174 @@ public class CalculateService {
 
     public static List<RoutePath> queryRoutePathsOnDirection(Section start, Section end, Boolean right) {
         List<RoutePath> list = new ArrayList<>();
-        Signal signal = null;
+        List<String> warnList = new ArrayList<>();
-//        if (start.isSwitchTrack()) {
-//            signal = start.querySwitchSectionRelatedSignalByDirection(right);
-//        } else {
-//            signal = start.getSignalOf(right);
-//        }
-        Section section = start;
-        int rc = 0;
-        while (signal == null && rc < 20) {
-            ++rc;
-            if (section == null) {
-                break;
-            }
-            signal = section.getSignalOf(right);
-            if (signal != null) {
-                break;
-            }
-            Section sectionOf = section.getSectionOf(!right);
-            if (sectionOf != null) {
-                section = sectionOf;
-            } else if (sectionOf == null && section.isSwitchTrack()) {
-                Switch relSwitch = section.getRelSwitch();
-                if (relSwitch.isA(section)) {
-                    section = relSwitch.getB();
-                } else {
-                    section = relSwitch.getA();
-                }
-            }
-        }
         RoutePath routePath = new RoutePath(start, end, right);
-        getRoutePaths(signal, right, end, 1, routePath, list);
+        queryRoutePaths(0, start, routePath, list, warnList);
+        if (!CollectionUtils.isEmpty(list)) {
+            for (RoutePath path : list) {
+                path.calculateDistance();
+                log.debug(path.debugStr2());
+            }
+        } else {
+            log.warn(String.format("进路路径[%s]未找到：[%s]",
+                    routePath.debugStr(),
+                    String.join(",", warnList)));
+        }
         return list;
     }
 
-    private static void getRoutePaths(Signal signal, Boolean right, Section end, int iter,
+    private static void queryRoutePaths(int iter, Section section, RoutePath routePath,
-                                      RoutePath routePath, List<RoutePath> list) {
+                                        List<RoutePath> list, List<String> warnList) {
-        if (Objects.isNull(signal)) {
+        if (section == null) {
+            warnList.add(String.format("进路路径[%s]未找到,找到的最后区段为[%s],下一区段为null",
+                    routePath.debugStr(), routePath.getLastSection().debugStr()));
             return;
         }
-        if (iter > 20) {
+        if (iter > 10) {
+            warnList.add(String.format("进路路径[%s]未找到：迭代10次,最后区段为[%s]",
+                    routePath.debugStr(), routePath.getLastSection().debugStr()));
             return;
         }
+        Section end = routePath.getEnd();
+        boolean right = routePath.isRight();
+        Signal signal = section.getSignalOf(right);
+        if (signal == null) {
+            // 区段没有信号机
+            Section next = section.getSectionOf(right);
+            if (next != null) {
+                // 下一区段存在
+                if (Objects.equals(next, end)) {
+                    // 找到，结束
+                    list.add(routePath);
+                } else {
+                    // 添加并进行迭代
+                    routePath.addSection(next);
+                    queryRoutePaths(iter + 1, next, routePath, list, warnList);
+                }
+            } else if (section.isSwitchTrack()) { // 道岔区段
+                Switch relSwitch = section.getRelSwitch();
+                if (relSwitch.isA(section)) {
+                    RoutePath path1 = routePath.cloneNew();
+                    path1.addSection(relSwitch.getB());
+                    queryRoutePaths(iter + 1, relSwitch.getB(), path1, list, warnList);
+                    RoutePath path2 = routePath.cloneNew();
+                    path2.addSection(relSwitch.getC());
+                    queryRoutePaths(iter + 1, relSwitch.getC(), path2, list, warnList);
+                } else {
+                    routePath.addSection(relSwitch.getA());
+                    queryRoutePaths(iter + 1, relSwitch.getA(), routePath, list, warnList);
+                }
+            } else {
+                warnList.add(String.format("区段[%s]的[%s]区段不存在",
+                        section.debugStr(), right ? "右向" : "左向"));
+            }
+            return;
+        }
+        // 信号机存在
         routePath.addSignal(signal);
         List<Route> routeList = signal.getRouteList();
         if (!CollectionUtils.isEmpty(routeList)) {
-            // 根据信号机的进路查询
+            // 进路
-            Set<Signal> signals = new HashSet<>();
+            boolean find = false;
-            for (Route route : routeList) {
+            if (signal.isVirtual()) { // 虚拟信号机，应该只有一条进路
-                if (route.isGround() || route.isGuide()) { // 如果进路是分为ATP信号、地面信号、引导信号的，只处理ATP信号
+                BusinessExceptionAssertEnum.SYSTEM_EXCEPTION.assertTrue(routeList.size() == 1);
-                    continue;
+                Route route = routeList.get(0);
+                routePath.addRoute(route);
+                List<Section> sectionList = route.getSectionList();
+                int i = sectionList.indexOf(end);
+                if (i >= 0) {
+                    // 找到
+                    routePath.addSections(sectionList.subList(0, i));
+                    list.add(routePath);
+                } else {
+                    routePath.addSections(sectionList);
+                    queryRoutePaths(iter+1, route.getLastRouteSection(), routePath, list, warnList);
                 }
-                if (route.isRouteSection(end)) {
+                return;
-                    for (Section section : route.getSectionList()) {
+            }
-                        routePath.addSection(section);
+            Map<Section, List<Route>> routeMap = new HashMap<>(); // 进路终端对应进路列表(ATP进路合并进正常进路终端)
-                        if (Objects.equals(section, end)) {
+            routeList.sort(Comparator.comparing(Route::isAtp)); // 将ATP进路排在最后处理
-                            break;
+            for (Route route : routeList) {
+                Section lastRouteSection = route.getLastRouteSection();
+                List<Route> routes = routeMap.get(lastRouteSection);
+                if (routes == null) {
+                    routes = new ArrayList<>();
+                    routeMap.put(lastRouteSection, routes);
+                }
+                routes.add(route);
+                if (route.isAtp()) { // 哈尔滨一ATP进路
+                    boolean handled = false;
+                    Set<Section> sections = routeMap.keySet();
+                    for (Section last : sections) {
+                        List<Route> tempList = routeMap.get(last);
+                        if (tempList.contains(lastRouteSection)) {
+                            handled = true;
+                            tempList.add(route);
+                            List<Route> nextRouteList = route.getDestination().getRouteList();
+                            if (nextRouteList.size() == 1 &&
+                                    Objects.equals(nextRouteList.get(0).getLastRouteSection(), last)) {
+                                tempList.addAll(nextRouteList);
+                            } else {
+                                throw BusinessExceptionAssertEnum.SYSTEM_EXCEPTION.exception();
+                            }
                         }
                     }
-                    list.add(routePath);
+                    if (handled) {
-                    return;
+                        routeMap.remove(lastRouteSection);
-                } else {
+                    }
-                    signals.add(route.getDestination());
                 }
             }
-            if (!CollectionUtils.isEmpty(signals)) {
+            Set<Section> keys = routeMap.keySet();
-                for (Signal endSignal : signals) {
+            List<RoutePath> iterList = new ArrayList<>();
-                    RoutePath clone = routePath.cloneNew();
+            for (Section key : keys) {
-                    for (Route route : routeList) {
+                List<Route> routeList1 = routeMap.get(key);
-                        if (Objects.equals(route.getDestination(), endSignal)) {
+                List<Section> sectionList = routeList1.get(0).getSectionList();
-                            clone.addSections(route.getSectionList());
+                int i = sectionList.indexOf(end);
-                            break;
+                if (i >= 0) {
-                        }
+                    // 找到
+                    routePath.addRoutes(routeList1);
+                    if (!find) {
+                        find = true;
+                        routePath.addSections(sectionList.subList(0, i));
                     }
-                    getRoutePaths(endSignal, right, end, iter + 1, clone, list);
+                } else {
+                    // 不包含
+                    RoutePath clone = routePath.cloneNew();
+                    clone.addRoutes(routeList1);
+                    clone.addSections(sectionList);
+                    iterList.add(clone);
+                }
+            }
+            if (find) {
+                list.add(routePath);
+                return;
+            } else {
+                for (RoutePath clone : iterList) {
+                    Section next = clone.getLastSection();
+                    queryRoutePaths(iter + 1, next, clone, list, warnList);
                 }
             }
         } else {
-            // 循着自动信号后面的区段查询
+            // 自动信号
             AutoSignal autoSignal = signal.getAutoSignal();
-            if (Objects.nonNull(autoSignal)) {
+            if (autoSignal == null) {
-                List<Section> sectionList = autoSignal.getSectionList();
+                warnList.add(String.format("信号机[%s]既没有进路也不是自动信号", signal.debugStr()));
-                for (Section section : sectionList) {
+                return;
-                    routePath.addSection(section);
+            }
-                    if (Objects.equals(section, end)) {
+            List<Section> sectionList = autoSignal.getSectionList();
-                        list.add(routePath);
+            int i = sectionList.indexOf(end);
-                        return;
+            if (i >= 0) {
-                    } else {
+                // 找到
-                        Signal signalOf = section.getSignalOf(right);
+                routePath.addSections(sectionList.subList(0, i));
-                        if (Objects.nonNull(signalOf)) {
+                list.add(routePath);
-                            RoutePath clone = routePath.cloneNew();
+                return;
-                            getRoutePaths(signalOf, right, end, iter + 1, clone, list);
+            } else {
-                            break;
+                // 顺着区段找下一个有信号机的区段为止
-                        }
+                for (Section next : sectionList) {
+                    routePath.addSection(next);
+                    if (next.getSignalOf(right) != null) {
+                        queryRoutePaths(iter + 1, next, routePath, list, warnList);
+                        break;
                     }
                 }
             }
@@ -742,8 +814,8 @@ public class CalculateService {
     /**
      * 查找从startSection到destination的途径区段路径
      */
-    public static Set<List<Section>> querySectionPaths2Destination(@NonNull Section startSection, @NonNull Section destination,
+    public static Set<List<Section>> querySectionPaths2Destination(Section startSection, Section destination,
-                                                                   @NonNull Boolean right, @NonNull boolean stop) {
+                                                                   Boolean right, boolean stop) {
         long start = System.currentTimeMillis();
         //从startSection开始寻找第一个同向信号机（如果是折返轨，两边的信号机都算），并记录途径区段
         List<Signal> signals = new ArrayList<>();

src/main/java/club/joylink/rtss/simulation/cbtc/data/map/Route.java

@@ -35,7 +35,7 @@ public class Route extends MapNamedElement {
     /** 是否折返进路 */
     private boolean turnBack;
 
-    /** 是否引导进路 */
+    /** 是否ATP进路 */
     private boolean atp;
 
     /** 是否地面信号进路 */

src/main/java/club/joylink/rtss/simulation/cbtc/data/map/RouteFls.java

@@ -64,11 +64,11 @@ public class RouteFls {
     public void lock() {
         for (FlsElement flsElement : this.getLevel1List()) {
             SwitchElement pSwitch = flsElement.getPSwitch();
-            if (pSwitch != null) {
+            if (pSwitch != null && !pSwitch.getASwitch().isSectionOccupied()) {
                 pSwitch.getASwitch().fpLock();
             } else {
                 SwitchElement fpae = flsElement.getFpae();
-                if (fpae != null) {
+                if (fpae != null && !fpae.getASwitch().isSectionOccupied()) {
                     fpae.getASwitch().fpLock();
                 }
             }

src/main/java/club/joylink/rtss/simulation/cbtc/data/support/RoutePath.java

@@ -1,5 +1,6 @@
 package club.joylink.rtss.simulation.cbtc.data.support;
 
+import club.joylink.rtss.exception.BusinessExceptionAssertEnum;
 import club.joylink.rtss.simulation.cbtc.constant.RunLevel;
 import club.joylink.rtss.simulation.cbtc.data.map.*;
 import club.joylink.rtss.simulation.cbtc.exception.SimulationException;
@@ -10,6 +11,7 @@ import org.springframework.util.CollectionUtils;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Objects;
+import java.util.stream.Collectors;
 
 @Getter
 public class RoutePath {
@@ -26,7 +28,10 @@ public class RoutePath {
     /** 信号机列表 */
     private List<Signal> signalList = new ArrayList<>();
 
-    /** 区段列表 */
+    /** 进路列表 */
+    private List<Route> routeList = new ArrayList<>();
+
+    /** 区段列表(不包含start和end) */
     private List<Section> sectionList = new ArrayList<>();
 
     /** 方向 */
@@ -48,15 +53,40 @@ public class RoutePath {
     }
 
     public void addSignal(Signal signal) {
+        if (this.signalList.contains(signal)) {
+            return;
+        }
         this.signalList.add(signal);
     }
 
     public void addSection(Section section) {
+        if (this.sectionList.contains(section)) {
+            return;
+        }
         this.sectionList.add(section);
     }
 
     public void addSections(List<Section> sections) {
-        this.sectionList.addAll(sections);
+        if (!CollectionUtils.isEmpty(sections)) {
+            for (Section section : sections) {
+                this.addSection(section);
+            }
+        }
+    }
+
+    public void addRoute(Route route) {
+        if (this.routeList.contains(route)) {
+            return;
+        }
+        this.routeList.add(route);
+    }
+
+    public void addRoutes(List<Route> routeList) {
+        if (!CollectionUtils.isEmpty(routeList)) {
+            for (Route route : routeList) {
+                this.addRoute(route);
+            }
+        }
     }
 
     public RoutePath cloneNew() {
@@ -73,9 +103,25 @@ public class RoutePath {
             return length;
         }
         float len = 0;
+        if (start.isFunctionTrack()) {
+            if (this.isRight()) {
+                len += start.getLen() - start.getStopPointRight();
+            } else {
+                len += start.getStopPointLeft();
+            }
+        }
         for (Section section : this.sectionList) {
             len += section.getLen();
         }
+        if (end.isFunctionTrack()) {
+            if (this.isRight()) {
+                len += end.getStopPointRight();
+            } else {
+                len += end.getLen() - end.getStopPointLeft();
+            }
+        } else {
+            len += end.getLen();
+        }
         this.length = len;
         return len;
     }
@@ -85,7 +131,9 @@ public class RoutePath {
     }
 
     public boolean containsSection(Section section) {
-        return this.sectionList.contains(section) || Objects.equals(section, this.start);
+        return this.start.isSamePhysical(section.getCode()) ||
+                this.end.isSamePhysical(section.getCode()) ||
+                this.sectionList.contains(section);
     }
 
     public boolean isPathSection(Section section) {
@@ -103,14 +151,7 @@ public class RoutePath {
         }
         return true;
     }
-
+    public float getLenFromSection(Section fromSection){
-    public String debugStr() {
-        return String.format("%s(%s)->%s(%s)",
-                this.start.getStation().getName(), this.start.getName(),
-                this.end.getStation().getName(), this.end.getName());
-    }
-
-     public float getLenFromSection(Section fromSection){
          List<Section> list = new ArrayList<>();
          if(sectionList.contains(fromSection)){
              int i = sectionList.indexOf(fromSection);
@@ -128,6 +169,56 @@ public class RoutePath {
          }
          return len;
      }
+
+    public SectionPosition getSectionPositionOfLen(float len) {
+        BusinessExceptionAssertEnum.SYSTEM_EXCEPTION.assertTrue(len <= this.length);
+
+        float l = len;
+        Section section = null;
+        float offset = 0;
+        if (this.start.isFunctionTrack()) {
+            float s = 0;
+            if (this.isRight()) {
+                s = this.start.getLen() - this.start.getStopPointRight();
+            } else {
+                s = this.start.getStopPointLeft();
+            }
+            if (l < s) {
+                offset = this.start.getStopPointRight() + l;
+                section = this.start;
+            } else {
+                l -= s;
+            }
+        }
+        if (section == null) {
+            for (Section next : this.sectionList) {
+                if (l <= next.getLen()) {
+                    section = next;
+                    if (this.isRight()) {
+                        offset = l;
+                    } else {
+                        offset = next.getLen() - l;
+                    }
+                    break;
+                } else {
+                    l -= next.getLen();
+                }
+            }
+        }
+        if (section == null) {
+            // end区段
+            BusinessExceptionAssertEnum.SYSTEM_EXCEPTION.assertTrue(l <= this.end.getLen());
+            section = this.end;
+            if (this.isRight()) {
+                offset = l;
+            } else {
+                offset = this.end.getLen() - l;
+            }
+        }
+        BusinessExceptionAssertEnum.SYSTEM_EXCEPTION.assertNotNull(section);
+        return new SectionPosition(section, offset);
+    }
+
     public SectionPosition getSectionPositionOfLen(float runLen,Section fromSection) {
         float remain = runLen;
         Section target = null;
@@ -242,4 +333,36 @@ public class RoutePath {
         }
         return false;
     }
+
+    public Section getLastSection() {
+        return this.sectionList.get(this.sectionList.size() - 1);
+    }
+
+    public String debugStr() {
+        return String.format("%s(%s)->%s(%s)",
+                this.start.getStation().getName(), this.start.getName(),
+                this.end.getStation().getName(), this.end.getName());
+    }
+
+    public String debugStr2() {
+        String sections = String.join(",", this.sectionList.stream()
+                .map(Section::debugStr)
+                .collect(Collectors.toList()));
+        String signals = String.join(",", this.signalList.stream()
+                .map(Signal::debugStr)
+                .collect(Collectors.toList()));
+        String routes = String.join(",", this.routeList.stream()
+                .map(Route::debugStr)
+                .collect(Collectors.toList()));
+        return String.format("进路路径[%s]:{方向:%s," +
+                "距离:%s" +
+                "途经区段:[%s]," +
+                "途经信号机:[%s]," +
+                "途经进路(多种方式):[%s]}",
+                this.debugStr(), this.isRight()?"右":"左",
+                this.length,
+                sections,
+                signals,
+                routes);
+    }
 }