速度曲线限速逻辑重写（未完）

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

@@ -1,6 +1,5 @@
 package club.joylink.rtss.simulation.cbtc.ATS.service;
 
-import club.joylink.rtss.entity.PlanRouting;
 import club.joylink.rtss.exception.BusinessExceptionAssertEnum;
 import club.joylink.rtss.simulation.cbtc.ATS.service.stage.AtsHeadTrainStageService;
 import club.joylink.rtss.simulation.cbtc.ATS.service.stage.AtsPlanTrainStageService;
@@ -30,7 +29,6 @@ import org.springframework.util.CollectionUtils;
 import org.springframework.util.StringUtils;
 
 import java.time.LocalTime;
-import java.time.temporal.ChronoUnit;
 import java.util.*;
 import java.util.stream.Collectors;
 
@@ -783,7 +781,7 @@ public class AtsTrainService {
             boolean targetIsRight = targetTrain.isRight();
             targetPosition = CalculateService.calculateNextPositionByStartAndLen(targetTrainTailPosition, !targetIsRight, 2);
         }
-        SectionPosition physicalPosition = CalculateService.calculatePositionOfPhysical(targetPosition);
+        SectionPosition physicalPosition = targetPosition.convert2PhysicalSectionPosition();
         train.setRobotTargetPosition(physicalPosition);
     }
 

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

@@ -1238,22 +1238,4 @@ public class CalculateService {
         }
         return acceleratedDistance + varyingAcceleratedDistance;
     }
-
-    /**
-     * 计算该位置对应的物理区段位置
-     */
-    public static SectionPosition calculatePositionOfPhysical(SectionPosition sectionPosition) {
-        Section section = sectionPosition.getSection();
-        if (section.isPhysical()) {
-            return sectionPosition;
-        } else {
-            Section parent = section.getParent();
-            if (parent != null) {
-                return new SectionPosition(parent, section.getMinOffset() + sectionPosition.getOffset());
-            } else {
-                return null;
-            }
-        }
-    }
-
 }

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

@@ -976,6 +976,8 @@ public class Section extends MayOutOfOrderDevice {
             return this.getSpeedUpLimit() / 3.6f;
         } else if (this.isSwitchTrack() && this.getRelSwitch().isReversePosition()) {
             return 30 / 3.6f;
+        } else if (this.getParent() != null) {
+            return this.getParent().getNeedLimitSpeed();
         }
         return null;
     }

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

@@ -258,17 +258,6 @@ public class Switch extends MayOutOfOrderDevice {
 //        }
     }
 
-    /**
-     * 设置道岔区段上限速度
-     *
-     * @param upLimitSpeed
-     */
-    public void setSpeedUpperLimit(int upLimitSpeed) {
-        this.a.setSpeedUpLimit(upLimitSpeed);
-        this.b.setSpeedUpLimit(upLimitSpeed);
-        this.c.setSpeedUpLimit(upLimitSpeed);
-    }
-
     public void routeLock(Route route) {
         this.routeLock = true;
         this.route = route;

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

@@ -100,6 +100,20 @@ public class SectionPosition {
         }
     }
 
+    public SectionPosition convert2PhysicalSectionPosition() {
+        Section section = this.getSection();
+        if (section.isPhysical()) {
+            return this;
+        } else {
+            Section parent = section.getParent();
+            if (parent != null) {
+                return new SectionPosition(parent, section.getMinOffset() + this.getOffset());
+            } else {
+                return null;
+            }
+        }
+    }
+
     /**
      * 该位置位于对应区段的停车范围内
      */

src/main/java/club/joylink/rtss/simulation/cbtc/onboard/ATO/SpeedCurve.java

@@ -45,7 +45,6 @@ public class SpeedCurve {
     }
 
     /**
-     *
      * @param simulation
      * @param train
      * @param ma
@@ -132,6 +131,7 @@ public class SpeedCurve {
 
     public static final Map<Float, SpeedCalculator> LimitSpeedCalMap = new HashMap<>();
     public static final float Release_Limit_Speed = 5; // 释放功能限制速度，单位m/s
+
     public static SpeedCalculator getLimitSpeedCalculator(float limitSpeed) {
         SpeedCalculator speedCalculator = LimitSpeedCalMap.get(limitSpeed);
         if (speedCalculator == null) {
@@ -296,54 +296,23 @@ public class SpeedCurve {
         if (totalLen <= 0) {
             return SpeedCurve.ZERO;
         }
+        Section tailLogic = tailPosition.getLogicSection();
         Section base = tailPosition.getSection();
         Section headSection = headPosition.getSection();
         float limitSpeed = recommendedSpeedMax;
-        boolean limit = false;
-        if (Objects.nonNull(tailPosition.getSection().getNeedLimitSpeed())) {
-            limitSpeed = Math.min(limitSpeed, tailPosition.getSection().getNeedLimitSpeed() * 0.9f);
-            limit = true;
-        } else if (Objects.nonNull(headPosition.getSection().getNeedLimitSpeed())) {
-            limitSpeed = Math.min(limitSpeed, headPosition.getSection().getNeedLimitSpeed() * 0.9f);
-            limit = true;
-        }
         SectionPosition endPosition = null;
         boolean ahead = false;
         float vt = 0;
-        int count = 0;
+        for (int i = 0; i < 20; i++) {
-        if (!limit) {
+            List<Section> logicSections = base.getLogicList();
-            // 无限速，找前方限速
+            if (logicSections == null) {
-            while (Objects.nonNull(base) && count < 20) {
+                logicSections = Collections.singletonList(base);
-                ++count;
+            }
-                Float limitSpeed1 = base.getNeedLimitSpeed();
+            for (Section logic : logicSections) {
-                if (limitSpeed1 == null && !CollectionUtils.isEmpty(base.getLogicList())) { //兼容逻辑区段设置限速
+                Float needLimitSpeed = logic.getNeedLimitSpeed();
-                    float distanceBetweenLogicAndPhysical = 0; //逻辑区段距物理区段端点的距离（同向端点）
+                if (needLimitSpeed != null) {
-                    for (Section section : base.getLogicList()) {
+                    endPosition = new SectionPosition(base, logic.getEndOffsetByDirection(right));
-                        limitSpeed1 = section.getNeedLimitSpeed();
-                        if (limitSpeed1 != null) {
-                            float offsetOfLogicSectionOnPhysicalSection = distanceBetweenLogicAndPhysical + (right ? section.getLen() : 0); //逻辑区段的端点在物理区段上的偏移量
-                            SectionPosition logicSectionPosition = new SectionPosition(base, offsetOfLogicSectionOnPhysicalSection);
-                            if (logicSectionPosition.isAheadOf(tailPosition, right)) {
-                                break;
-                            }
-                        }
-                        distanceBetweenLogicAndPhysical += section.getLen();
-                    }
                 }
-                if (Objects.nonNull(limitSpeed1)) {
-                    if (ahead) {
-                        // 车头前方
-                        vt = Math.min(limitSpeed, limitSpeed1 * 0.9f);
-                        endPosition = new SectionPosition(base, right ? 0 : base.getLen());
-                        break;
-                    } else {
-                        limitSpeed = Math.min(limitSpeed, limitSpeed1 * 0.9f);
-                    }
-                }
-                if (Objects.equals(base, headSection)) {
-                    ahead = true;
-                }
-                base = base.getNextRunningSectionOf(right);
             }
         }
         SpeedCurve normalSpeedCurve = buildTargetSpeedCurve(totalLen, v0, 0, limitSpeed);
@@ -359,6 +328,75 @@ public class SpeedCurve {
         return normalSpeedCurve;
     }
 
+//    public static SpeedCurve buildTargetSpeedCurve(SectionPosition headPosition,
+//                                                   SectionPosition tailPosition, boolean right,
+//                                                   float totalLen, float v0, float recommendedSpeedMax) {
+//        if (totalLen <= 0) {
+//            return SpeedCurve.ZERO;
+//        }
+//        Section base = tailPosition.getSection();
+//        Section headSection = headPosition.getSection();
+//        float limitSpeed = recommendedSpeedMax;
+//        boolean limit = false;
+//        if (Objects.nonNull(tailPosition.getSection().getNeedLimitSpeed())) {
+//            limitSpeed = Math.min(limitSpeed, tailPosition.getSection().getNeedLimitSpeed() * 0.9f);
+//            limit = true;
+//        } else if (Objects.nonNull(headPosition.getSection().getNeedLimitSpeed())) {
+//            limitSpeed = Math.min(limitSpeed, headPosition.getSection().getNeedLimitSpeed() * 0.9f);
+//            limit = true;
+//        }
+//        SectionPosition endPosition = null;
+//        boolean ahead = false;
+//        float vt = 0;
+//        int count = 0;
+//        if (!limit) {
+//            // 无限速，找前方限速
+//            while (Objects.nonNull(base) && count < 20) {
+//                ++count;
+//                Float limitSpeed1 = base.getNeedLimitSpeed();
+//                if (limitSpeed1 == null && !CollectionUtils.isEmpty(base.getLogicList())) { //兼容逻辑区段设置限速
+//                    float distanceBetweenLogicAndPhysical = 0; //逻辑区段距物理区段端点的距离（同向端点）
+//                    for (Section section : base.getLogicList()) {
+//                        limitSpeed1 = section.getNeedLimitSpeed();
+//                        if (limitSpeed1 != null) {
+//                            float offsetOfLogicSectionOnPhysicalSection = distanceBetweenLogicAndPhysical + (right ? section.getLen() : 0); //逻辑区段的端点在物理区段上的偏移量
+//                            SectionPosition logicSectionPosition = new SectionPosition(base, offsetOfLogicSectionOnPhysicalSection);
+//                            if (logicSectionPosition.isAheadOf(tailPosition, right)) {
+//                                break;
+//                            }
+//                        }
+//                        distanceBetweenLogicAndPhysical += section.getLen();
+//                    }
+//                }
+//                if (Objects.nonNull(limitSpeed1)) {
+//                    if (ahead) {
+//                        // 车头前方
+//                        vt = Math.min(limitSpeed, limitSpeed1 * 0.9f);
+//                        endPosition = new SectionPosition(base, right ? 0 : base.getLen());
+//                        break;
+//                    } else {
+//                        limitSpeed = Math.min(limitSpeed, limitSpeed1 * 0.9f);
+//                    }
+//                }
+//                if (Objects.equals(base, headSection)) {
+//                    ahead = true;
+//                }
+//                base = base.getNextRunningSectionOf(right);
+//            }
+//        }
+//        SpeedCurve normalSpeedCurve = buildTargetSpeedCurve(totalLen, v0, 0, limitSpeed);
+//        if (endPosition != null) {
+//            Float distance = CalculateService.calculateDistance(headPosition, endPosition, right);
+//            if (distance != null && distance < totalLen) {
+//                SpeedCurve limitSpeedCurve = buildTargetSpeedCurve(distance, v0, vt, limitSpeed);
+//                float normalStartSpeed = normalSpeedCurve.getSpeedOf(normalSpeedCurve.getTotalDistance());
+//                float limitStartSpeed = limitSpeedCurve.getSpeedOf(limitSpeedCurve.getTotalDistance());
+//                return normalStartSpeed < limitStartSpeed ? normalSpeedCurve : limitSpeedCurve;
+//            }
+//        }
+//        return normalSpeedCurve;
+//    }
+
     /**
      * 构建目标速度曲线
      *
@@ -374,11 +412,6 @@ public class SpeedCurve {
             return SpeedCurve.ZERO;
         }
         SpeedCurve speedCurve = new SpeedCurve(targetDistance);
-        /*float uv = limitSpeed;
-        if (vt > uv) {
-            vt = uv;
-        }
-        float a1 = VirtualRealityTrain.Stable_Acceleration; // 加速段平均加速度*/
         float a2 = VirtualRealityTrain.Max_Braking_Acceleration; // 减速段平均加速度
         if (vt >= limitSpeed) {
             speedCurve.addCalculator(new SpeedCalculator(0, targetDistance,
@@ -396,41 +429,6 @@ public class SpeedCurve {
                     decelerateV0, vt, -a2, VSFormula.UNIFORM_DECELERATION));
         }
         return speedCurve;
-
-
-        /*//通过两线相交计算交点y值（即速度）
-        float vMid = (float) Math.sqrt(2 * a1 * a2 * targetDistance + a2 * Math.pow(v0, 2) + a1 * Math.pow(vt, 2)) / (a1 + a2);
-        if (vMid > uv) { // 加速-匀速-减速
-            float la = (float) ((Math.pow(uv, 2) - Math.pow(v0, 2)) / (2 * a1)); // 加速段长度
-            float ld = (float) (Math.pow(uv, 2) - Math.pow(vt, 2) / (2 * a2)); // 减速段长度
-            float da = la;
-            float dd = targetDistance - ld;
-            speedCurve.addCalculator(new SpeedCalculator(0, da,
-                    v0, uv, a1,
-                    VSFormula.UNIFORM_ACCELERATION));
-            speedCurve.addCalculator(new SpeedCalculator(da, dd,
-                    uv, uv, 0,
-                    VSFormula.UNIFORM));
-            speedCurve.addCalculator(new SpeedCalculator(dd, targetDistance,
-                    uv, vt, -a2,
-                    VSFormula.UNIFORM_DECELERATION));
-        } else {
-            if (vMid > v0) { // 加速-减速
-                float dA2 = (float) (Math.pow(vMid, 2) - Math.pow(vt, 2) / (2 * a2)); // 减速段长度
-                float dMid = targetDistance - dA2;//加速-减速段中点位置
-                speedCurve.addCalculator(new SpeedCalculator(0, dMid,
-                        v0, vMid, a1,
-                        VSFormula.UNIFORM_ACCELERATION));
-                speedCurve.addCalculator(new SpeedCalculator(dMid, targetDistance,
-                        vMid, vt, -a2,
-                        VSFormula.UNIFORM_DECELERATION));
-            } else { // 只减速
-                speedCurve.addCalculator(new SpeedCalculator(0, targetDistance,
-                        v0, vt, -a2,
-                        VSFormula.UNIFORM_DECELERATION));
-            }
-        }
-        return speedCurve;*/
     }
 
     /**