diff --git a/qaup-collision/src/main/java/com/qaup/collision/pathconflict/service/PathConflictDetectionService.java b/qaup-collision/src/main/java/com/qaup/collision/pathconflict/service/PathConflictDetectionService.java index 7fe08ac..a778ea4 100644 --- a/qaup-collision/src/main/java/com/qaup/collision/pathconflict/service/PathConflictDetectionService.java +++ b/qaup-collision/src/main/java/com/qaup/collision/pathconflict/service/PathConflictDetectionService.java @@ -1,127 +1,171 @@ package com.qaup.collision.pathconflict.service; -import com.qaup.collision.pathconflict.model.entity.TransportRoute; -import com.qaup.collision.pathconflict.repository.TransportRouteRepository; import com.qaup.collision.common.model.MovingObject; import com.qaup.collision.common.model.MovingObject.MovingObjectType; -import com.qaup.collision.pathconflict.model.dto.ConflictAlertEvent; -import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment; -import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository; -import com.qaup.collision.pathconflict.model.entity.ConflictAlertLog; -import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository; import com.qaup.collision.dataprocessing.service.CoordinateSystemService; - +import com.qaup.collision.pathconflict.model.dto.ConflictAlertEvent; +import com.qaup.collision.pathconflict.model.entity.ConflictAlertLog; +import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment; +import com.qaup.collision.pathconflict.model.entity.TransportRoute; +import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository; +import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository; +import com.qaup.collision.pathconflict.repository.TransportRouteRepository; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; -import org.locationtech.jts.geom.*; +import org.locationtech.jts.geom.Coordinate; +import org.locationtech.jts.geom.Geometry; +import org.locationtech.jts.geom.GeometryFactory; +import org.locationtech.jts.geom.LineString; +import org.locationtech.jts.geom.MultiPoint; +import org.locationtech.jts.geom.Point; +import org.locationtech.jts.geom.PrecisionModel; import org.locationtech.jts.operation.distance.DistanceOp; -import org.springframework.stereotype.Service; import org.springframework.context.ApplicationEventPublisher; +import org.springframework.stereotype.Service; -import java.util.*; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; import java.util.Optional; +import java.util.UUID; -/** - * 路径冲突检测服务 - * 基于路径、速度和距离的冲突检测算法 - * - * @author AI Assistant - * @version 1.0 - * @since 2025-01-17 - */ @Slf4j @Service @RequiredArgsConstructor public class PathConflictDetectionService { - + private final TransportRouteRepository routeRepository; private final ObjectRouteAssignmentRepository objectRouteAssignmentRepository; private final ConflictAlertLogRepository conflictAlertLogRepository; private final ApplicationEventPublisher eventPublisher; - private final CoordinateSystemService coordinateSystemService; // 注入 CoordinateSystemService - - // 预警距离阈值（米） + private final CoordinateSystemService coordinateSystemService; + private static final double WARNING_DISTANCE_THRESHOLD = 200.0; - // 告警距离阈值（米） private static final double ALERT_DISTANCE_THRESHOLD = 100.0; - // 最大检测时间范围（秒） - private static final int MAX_PREDICTION_TIME_SECONDS = 300; // 5分钟 - // 最小时间间隔阈值（秒） + private static final int MAX_PREDICTION_TIME_SECONDS = 300; private static final double MIN_TIME_GAP_SECONDS = 30.0; - - /** - * 执行路径冲突检测 - * - * @param activeObjects 当前活跃的移动对象（无人车和航空器） - */ + + // Strong accuracy guards + private static final double MIN_FORWARD_DISTANCE_METERS = 3.0; + private static final double MAX_ROUTE_DEVIATION_METERS = 80.0; + private static final double HEADING_ALIGNMENT_MIN_COS = 0.0; + private static final double HEADING_CHECK_MIN_SPEED_KPH = 3.0; + private static final double MIN_EFFECTIVE_SPEED_KPH = 1.0; + public void detectPathConflicts(List activeObjects) { - log.debug("开始路径冲突检测，活跃对象数量: {}", activeObjects.size()); - + log.debug("Starting path conflict detection for {} active objects", activeObjects.size()); + List detectedAlertEvents = new ArrayList<>(); - - // 生成所有对象组合 + for (int i = 0; i < activeObjects.size(); i++) { for (int j = i + 1; j < activeObjects.size(); j++) { MovingObject obj1 = activeObjects.get(i); MovingObject obj2 = activeObjects.get(j); - - // 检测两个对象之间的潜在冲突 Optional alertEventOptional = detectConflictBetweenObjects(obj1, obj2); alertEventOptional.ifPresent(detectedAlertEvents::add); } } - - // 发布告警事件 + for (ConflictAlertEvent event : detectedAlertEvents) { eventPublisher.publishEvent(event); - log.info("发布冲突告警事件: conflictId={}, alertType={}, alertLevel={}", - event.getConflictId(), event.getAlertType(), event.getAlertLevel()); + log.info("Published path conflict event: conflictId={}, alertType={}, alertLevel={}", + event.getConflictId(), event.getAlertType(), event.getAlertLevel()); } - - log.info("路径冲突检测完成，检测到并发布 {} 个潜在冲突告警事件", detectedAlertEvents.size()); + + log.info("Path conflict detection completed, {} events published", detectedAlertEvents.size()); } - - /** - * 检测两个对象之间的路径冲突 - */ + private Optional detectConflictBetweenObjects(MovingObject obj1, MovingObject obj2) { if (obj1.getObjectType() != MovingObjectType.UNMANNED_VEHICLE && obj2.getObjectType() != MovingObjectType.UNMANNED_VEHICLE) { - log.debug("对象 {} 和 {} 都不是无人车，跳过冲突检测", obj1.getObjectName(), obj2.getObjectName()); return Optional.empty(); } try { - // 获取对象的路径 - TransportRoute route1 = getObjectRoute(obj1); - TransportRoute route2 = getObjectRoute(obj2); - - if (route1 == null || route2 == null) { - log.debug("对象 {} 或 {} 没有分配路径，跳过冲突检测", obj1.getObjectName(), obj2.getObjectName()); + if (obj1.getCurrentPosition() == null || obj2.getCurrentPosition() == null) { return Optional.empty(); } - - // 计算路径交点 + + TransportRoute route1 = getObjectRoute(obj1); + TransportRoute route2 = getObjectRoute(obj2); + + if (route1 == null || route2 == null || route1.getRouteGeometry() == null || route2.getRouteGeometry() == null) { + return Optional.empty(); + } + + LocalRoute localRoute1 = toLocalRoute(route1); + LocalRoute localRoute2 = toLocalRoute(route2); + if (localRoute1 == null || localRoute2 == null) { + return Optional.empty(); + } + + Point localObjPos1 = toLocalPoint(obj1.getCurrentPosition()); + Point localObjPos2 = toLocalPoint(obj2.getCurrentPosition()); + if (localObjPos1 == null || localObjPos2 == null) { + return Optional.empty(); + } + + ProjectedPosition projected1 = projectToRoute(localObjPos1, localRoute1.localLine); + ProjectedPosition projected2 = projectToRoute(localObjPos2, localRoute2.localLine); + if (projected1 == null || projected2 == null) { + return Optional.empty(); + } + + if (projected1.lateralDistanceMeters > MAX_ROUTE_DEVIATION_METERS || projected2.lateralDistanceMeters > MAX_ROUTE_DEVIATION_METERS) { + return Optional.empty(); + } + + double speed1Kph = normalizeSpeedKph(obj1.getCurrentSpeed()); + double speed2Kph = normalizeSpeedKph(obj2.getCurrentSpeed()); + + // Ignore pairs that are effectively static. + if (speed1Kph < MIN_EFFECTIVE_SPEED_KPH || speed2Kph < MIN_EFFECTIVE_SPEED_KPH) { + return Optional.empty(); + } + List intersectionPoints = calculateRouteIntersections(route1, route2); - for (Point intersectionPoint : intersectionPoints) { - // 计算每个对象到交点的距离和时间 - ConflictCalculationResult result = calculateConflictDetails(obj1, obj2, route1, route2, intersectionPoint); - + Point localIntersection = toLocalPoint(intersectionPoint); + if (localIntersection == null) { + continue; + } + + ProjectedPosition projectedIntersection1 = projectToRoute(localIntersection, localRoute1.localLine); + ProjectedPosition projectedIntersection2 = projectToRoute(localIntersection, localRoute2.localLine); + if (projectedIntersection1 == null || projectedIntersection2 == null) { + continue; + } + + double forwardDistance1 = projectedIntersection1.distanceAlongRouteMeters - projected1.distanceAlongRouteMeters; + double forwardDistance2 = projectedIntersection2.distanceAlongRouteMeters - projected2.distanceAlongRouteMeters; + + // Only future conflicts are valid. + if (forwardDistance1 <= MIN_FORWARD_DISTANCE_METERS || forwardDistance2 <= MIN_FORWARD_DISTANCE_METERS) { + continue; + } + + // Ensure current heading points to the intersection direction when speed is meaningful. + if (!isHeadingAligned(obj1, localObjPos1, localIntersection, speed1Kph) + || !isHeadingAligned(obj2, localObjPos2, localIntersection, speed2Kph)) { + continue; + } + + ConflictCalculationResult result = calculateConflictDetails( + obj1, obj2, forwardDistance1, forwardDistance2, speed1Kph, speed2Kph); + if (result != null && isSignificantConflict(result)) { String description = generateConflictDescription(obj1, obj2, route1, route2); - + ConflictAlertLog alertLog = ConflictAlertLog.builder() .alertType(result.getAlertType().orElse(null)) .alertLevel(result.getAlertLevel().orElse(null)) .alertMessage(description) .object1Distance(result.getDistance1()) .object2Distance(result.getDistance2()) - .minimumDistance(Math.min(result.getDistance1(), result.getDistance2())) // 简化处理，可根据需要调整 + .minimumDistance(Math.min(result.getDistance1(), result.getDistance2())) .build(); conflictAlertLogRepository.save(alertLog); - log.info("保存冲突告警日志: {}", alertLog); - + return Optional.of(ConflictAlertEvent.builder() .conflictId(Optional.of(alertLog.getId())) .alertType(result.getAlertType()) @@ -140,39 +184,34 @@ public class PathConflictDetectionService { .build()); } } - } catch (Exception e) { - log.error("检测对象 {} 和 {} 之间的冲突时发生异常", obj1.getObjectName(), obj2.getObjectName(), e); + log.error("Error detecting conflict between {} and {}", obj1.getObjectName(), obj2.getObjectName(), e); } - + return Optional.empty(); } - - /** - * 获取对象当前使用的路径 - */ - private TransportRoute getObjectRoute(MovingObject obj) { - Optional assignmentOptional = objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc( - obj.getObjectName(), // Changed from getObjectId - ObjectRouteAssignment.ObjectType.valueOf(obj.getObjectType().name()) - ); - return assignmentOptional.map(assignment -> routeRepository.findById(assignment.getAssignedRouteId()).orElse(null)) // Changed findByRouteId to findById - .orElse(null); + private TransportRoute getObjectRoute(MovingObject obj) { + Optional assignmentOptional = objectRouteAssignmentRepository + .findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc( + obj.getObjectName(), + ObjectRouteAssignment.ObjectType.valueOf(obj.getObjectType().name()) + ); + + return assignmentOptional + .map(assignment -> routeRepository.findById(assignment.getAssignedRouteId()).orElse(null)) + .orElse(null); } - - /** - * 计算两条路径的交点 - */ + private List calculateRouteIntersections(TransportRoute route1, TransportRoute route2) { List intersections = new ArrayList<>(); - + try { LineString line1 = route1.getRouteGeometry(); LineString line2 = route2.getRouteGeometry(); - + Geometry intersection = line1.intersection(line2); - + if (intersection instanceof Point) { intersections.add((Point) intersection); } else if (intersection instanceof MultiPoint) { @@ -181,114 +220,102 @@ public class PathConflictDetectionService { intersections.add((Point) multiPoint.getGeometryN(i)); } } else if (intersection instanceof LineString) { - // 如果是线段重叠，取线段的中点 LineString overlapLine = (LineString) intersection; Point midPoint = overlapLine.getInteriorPoint(); - // 确保中间点不为空且不是POINT EMPTY if (midPoint != null && !midPoint.isEmpty()) { intersections.add(midPoint); - } else { - log.warn("LineString交点生成了空的中间点或无效点，跳过此交点。OverlapLine: {}", overlapLine); } } - } catch (Exception e) { - log.error("计算路径交点时出错", e); + log.error("Error calculating route intersections", e); } - + return intersections; } - - /** - * 计算冲突详细信息 - */ + private ConflictCalculationResult calculateConflictDetails( - MovingObject obj1, MovingObject obj2, - TransportRoute route1, TransportRoute route2, - Point intersectionPoint) { - + MovingObject obj1, + MovingObject obj2, + double distance1, + double distance2, + double speed1Kph, + double speed2Kph) { + try { - // 计算对象到交点的距离 - double distance1 = calculateDistanceAlongRoute(obj1.getCurrentPosition(), intersectionPoint, route1); - double distance2 = calculateDistanceAlongRoute(obj2.getCurrentPosition(), intersectionPoint, route2); - - // 计算到达交点的时间（基于当前速度） - // 处理速度为null的情况，使用默认最小速度1 km/h - double speed1 = Math.max(obj1.getCurrentSpeed() != null ? obj1.getCurrentSpeed() : 1.0, 1.0); // 避免除零，最小速度1 km/h - double speed2 = Math.max(obj2.getCurrentSpeed() != null ? obj2.getCurrentSpeed() : 1.0, 1.0); - - // 转换为m/s - double speed1_ms = speed1 * 1000.0 / 3600.0; - double speed2_ms = speed2 * 1000.0 / 3600.0; - - int timeToConflict1 = (int) (distance1 / speed1_ms); - int timeToConflict2 = (int) (distance2 / speed2_ms); - - // 计算时间差 + double speed1Ms = speed1Kph * 1000.0 / 3600.0; + double speed2Ms = speed2Kph * 1000.0 / 3600.0; + + int timeToConflict1 = (int) Math.round(distance1 / speed1Ms); + int timeToConflict2 = (int) Math.round(distance2 / speed2Ms); + + if (timeToConflict1 > MAX_PREDICTION_TIME_SECONDS || timeToConflict2 > MAX_PREDICTION_TIME_SECONDS) { + return null; + } + double timeGap = Math.abs(timeToConflict1 - timeToConflict2); - - // 评估冲突告警级别和类型 - Optional alertLevelOptional = evaluateAlertLevel(distance1, obj1.getObjectType(), distance2, obj2.getObjectType(), timeGap); + + Optional alertLevelOptional = evaluateAlertLevel( + distance1, + obj1.getObjectType(), + distance2, + obj2.getObjectType(), + timeGap + ); Optional alertTypeOptional = alertLevelOptional.isPresent() ? evaluateAlertType(alertLevelOptional.get()) : Optional.empty(); - + return new ConflictCalculationResult( - distance1, distance2, timeToConflict1, timeToConflict2, timeGap, - alertTypeOptional, alertLevelOptional, - obj1.getObjectType(), obj2.getObjectType()); // Pass object types - + distance1, + distance2, + timeToConflict1, + timeToConflict2, + timeGap, + alertTypeOptional, + alertLevelOptional, + obj1.getObjectType(), + obj2.getObjectType()); + } catch (Exception e) { - log.error("计算冲突详细信息时出错", e); + log.error("Error calculating conflict details", e); return null; } } - - /** - * 评估冲突的告警级别 - */ + private Optional evaluateAlertLevel( - double distance1, MovingObjectType obj1Type, - double distance2, MovingObjectType obj2Type, - double timeGap) { + double distance1, + MovingObjectType obj1Type, + double distance2, + MovingObjectType obj2Type, + double timeGap) { boolean obj1IsUnmannedVehicle = MovingObjectType.UNMANNED_VEHICLE.equals(obj1Type); boolean obj2IsUnmannedVehicle = MovingObjectType.UNMANNED_VEHICLE.equals(obj2Type); if (!obj1IsUnmannedVehicle && !obj2IsUnmannedVehicle) { - // 双方都不是无人车，根据业务规则，不生成告警。 return Optional.empty(); } double distanceToEvaluate; if (obj1IsUnmannedVehicle && obj2IsUnmannedVehicle) { - // 双方都是无人车，关注两者中离交点最近的距离 distanceToEvaluate = Math.min(distance1, distance2); } else if (obj1IsUnmannedVehicle) { - // 只有 obj1 是无人车，关注 obj1 的距离。obj2 是无人车需要避让的对象。 distanceToEvaluate = distance1; - } else { // obj2IsUnmannedVehicle - // 只有 obj2 是无人车，关注 obj2 的距离。obj1 是无人车需要避让的对象。 + } else { distanceToEvaluate = distance2; } - // 现在根据确定的 relevantDistance 评估告警级别 if (distanceToEvaluate <= ALERT_DISTANCE_THRESHOLD && timeGap <= MIN_TIME_GAP_SECONDS) { return Optional.of(ConflictAlertLog.AlertLevel.CRITICAL); } else if (distanceToEvaluate <= WARNING_DISTANCE_THRESHOLD && timeGap <= MIN_TIME_GAP_SECONDS * 2) { return Optional.of(ConflictAlertLog.AlertLevel.WARNING); } else { - // 不满足 CRITICAL/WARNING 阈值，不生成告警。 return Optional.empty(); } } - /** - * 根据告警级别评估告警类型 - */ private Optional evaluateAlertType(ConflictAlertLog.AlertLevel alertLevel) { - // 处理null输入 if (alertLevel == null) { return Optional.empty(); } @@ -300,165 +327,195 @@ public class PathConflictDetectionService { case EMERGENCY: return Optional.of(ConflictAlertLog.AlertType.CONFLICT_ALERT); default: - // 应该不会到达这里，因为所有的 AlertLevel 都已经被明确处理 return Optional.empty(); } } - - /** - * 判断是否为显著冲突 - */ + private boolean isSignificantConflict(ConflictCalculationResult result) { - return result.getAlertLevel().isPresent() && - (result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.WARNING || - result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.CRITICAL || - result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.EMERGENCY); + return result.getAlertLevel().isPresent() && + (result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.WARNING + || result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.CRITICAL + || result.getAlertLevel().get() == ConflictAlertLog.AlertLevel.EMERGENCY); } - - /** - * 计算沿路径的距离 - */ - private double calculateDistanceAlongRoute(Point currentPos, Point targetPos, TransportRoute route) { + + private double normalizeSpeedKph(Double speedKph) { + if (speedKph == null || speedKph.isNaN() || speedKph.isInfinite() || speedKph < 0.0) { + return 0.0; + } + return speedKph; + } + + private boolean isHeadingAligned(MovingObject obj, Point currentPos, Point targetPos, double speedKph) { + if (speedKph < HEADING_CHECK_MIN_SPEED_KPH) { + return true; + } + + if (obj.getCurrentHeading() == null) { + return true; + } + + double vx = targetPos.getX() - currentPos.getX(); + double vy = targetPos.getY() - currentPos.getY(); + double vNorm = Math.hypot(vx, vy); + if (vNorm < 1e-6) { + return true; + } + + // Heading convention: 0 north, 90 east. + double headingRad = Math.toRadians(obj.getCurrentHeading()); + double hx = Math.sin(headingRad); + double hy = Math.cos(headingRad); + double dot = (hx * vx + hy * vy) / vNorm; + + return dot >= HEADING_ALIGNMENT_MIN_COS; + } + + private Point toLocalPoint(Point wgs84Point) { try { - // 1. 获取原始路径几何和点 - LineString originalRouteLine = route.getRouteGeometry(); - - // 2. 将所有相关几何对象转换为局部米制坐标系 - // 2.1 转换 currentPos 和 targetPos - double[] localCurrentCoords = coordinateSystemService.convertToLocalCoordinate(currentPos.getX(), currentPos.getY()); - double[] localTargetCoords = coordinateSystemService.convertToLocalCoordinate(targetPos.getX(), targetPos.getY()); - - GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0); // SRID 0 for Cartesian system (meters) - Point localCurrentPos = geometryFactory.createPoint(new Coordinate(localCurrentCoords[0], localCurrentCoords[1])); - Point localTargetPos = geometryFactory.createPoint(new Coordinate(localTargetCoords[0], localTargetCoords[1])); - - // 2.2 转换 routeLine - Coordinate[] originalCoords = originalRouteLine.getCoordinates(); - Coordinate[] localRouteCoords = new Coordinate[originalCoords.length]; - for (int i = 0; i < originalCoords.length; i++) { - double[] localCoord = coordinateSystemService.convertToLocalCoordinate(originalCoords[i].x, originalCoords[i].y); - localRouteCoords[i] = new Coordinate(localCoord[0], localCoord[1]); - } - LineString localRouteLine = geometryFactory.createLineString(localRouteCoords); - - // 3. 在局部米制坐标系中执行距离计算 - // 3.1 找到当前位置在路径上的最近点 - DistanceOp distOp1 = new DistanceOp(localCurrentPos, localRouteLine); - Coordinate[] nearestPoints1 = distOp1.nearestPoints(); - Point nearestOnRoute1 = geometryFactory.createPoint(nearestPoints1[1]); - - // 3.2 找到目标位置在路径上的最近点 - DistanceOp distOp2 = new DistanceOp(localTargetPos, localRouteLine); - Coordinate[] nearestPoints2 = distOp2.nearestPoints(); - Point nearestOnRoute2 = geometryFactory.createPoint(nearestPoints2[1]); - - // 3.3 计算沿路径的距离 (现在这些距离将是米) - double distanceToCurrentOnRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute1); - double distanceToTargetOnRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute2); - - return Math.abs(distanceToTargetOnRoute - distanceToCurrentOnRoute); - + double[] local = coordinateSystemService.convertToLocalCoordinate(wgs84Point.getX(), wgs84Point.getY()); + GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0); + return geometryFactory.createPoint(new Coordinate(local[0], local[1])); } catch (Exception e) { - log.error("计算沿路径距离时出错: currentPos={}, targetPos={}", - currentPos, targetPos, e); - return -1.0; // 表示错误 + log.debug("Failed to convert point to local coordinates", e); + return null; } } - - /** - * 生成冲突ID - */ + + private LocalRoute toLocalRoute(TransportRoute route) { + try { + LineString original = route.getRouteGeometry(); + Coordinate[] originalCoords = original.getCoordinates(); + if (originalCoords.length < 2) { + return null; + } + + Coordinate[] localCoords = new Coordinate[originalCoords.length]; + for (int i = 0; i < originalCoords.length; i++) { + double[] local = coordinateSystemService.convertToLocalCoordinate(originalCoords[i].x, originalCoords[i].y); + localCoords[i] = new Coordinate(local[0], local[1]); + } + + GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0); + return new LocalRoute(route, geometryFactory.createLineString(localCoords)); + } catch (Exception e) { + log.debug("Failed to convert route {} to local coordinates", route.getRouteName(), e); + return null; + } + } + + private ProjectedPosition projectToRoute(Point localPoint, LineString localRouteLine) { + try { + DistanceOp distanceOp = new DistanceOp(localPoint, localRouteLine); + Coordinate[] nearestPoints = distanceOp.nearestPoints(); + if (nearestPoints == null || nearestPoints.length < 2 || nearestPoints[1] == null) { + return null; + } + + GeometryFactory geometryFactory = new GeometryFactory(new PrecisionModel(), 0); + Point nearestOnRoute = geometryFactory.createPoint(nearestPoints[1]); + double lateralDistance = localPoint.distance(nearestOnRoute); + double distanceAlongRoute = getLinearLengthToPoint(localRouteLine, nearestOnRoute); + if (distanceAlongRoute < 0) { + return null; + } + + return new ProjectedPosition(distanceAlongRoute, lateralDistance); + } catch (Exception e) { + return null; + } + } + private String generateConflictId(String obj1Name, String obj2Name) { - // 确保ID的生成是确定性的，与对象的顺序无关 String[] names = {obj1Name, obj2Name}; Arrays.sort(names); return "CONFLICT-" + names[0] + "-" + names[1] + "-" + UUID.randomUUID().toString().substring(0, 8); } - - /** - * 生成冲突描述 - */ + private String generateConflictDescription( - MovingObject obj1, MovingObject obj2, - TransportRoute route1, TransportRoute route2) { - return String.format("对象 %s (%s) 在路径上与对象 %s (%s) 可能发生冲突。", + MovingObject obj1, + MovingObject obj2, + TransportRoute route1, + TransportRoute route2) { + return String.format("Objects %s (%s) and %s (%s) may conflict on assigned routes", obj1.getObjectName(), obj1.getObjectType(), obj2.getObjectName(), obj2.getObjectType()); } - - /** - * 计算沿LineString从起点到给定点的距离。 - * 假定point已经投影到line上。 - * 如果点不在line上，结果可能不准确。 - */ + private double getLinearLengthToPoint(LineString line, Point point) { double length = 0.0; Coordinate[] coords = line.getCoordinates(); - + for (int i = 0; i < coords.length - 1; i++) { Point p1 = line.getFactory().createPoint(coords[i]); - Point p2 = line.getFactory().createPoint(coords[i+1]); - - // 如果点在线段 (p1, p2) 上，则计算到p1的距离，然后加上点到p1的距离 + Point p2 = line.getFactory().createPoint(coords[i + 1]); + if (point.equals(p1)) { return length; } - + double segmentLength = p1.distance(p2); - // 检查点是否在当前线段上 (使用一个小的容差) - if (point.distance(p1) + point.distance(p2) - segmentLength < 1e-6) { // 容差检查 - return length + p1.distance(point); + if (point.distance(p1) + point.distance(p2) - segmentLength < 1e-6) { + return length + p1.distance(point); } length += segmentLength; } - // 如果点是最后一个点 + if (point.equals(line.getEndPoint())) { return line.getLength(); } - // 如果没有找到匹配，返回-1或者抛异常，这里返回线长表示无法准确计算 - return -1.0; // 表示未找到精确位置或计算错误 + + return -1.0; } - /** - * 冲突计算结果内部类 - */ + private record LocalRoute(TransportRoute route, LineString localLine) {} + + private record ProjectedPosition(double distanceAlongRouteMeters, double lateralDistanceMeters) {} + private static class ConflictCalculationResult { private final double distance1; private final double distance2; private final int timeToConflict1; private final int timeToConflict2; private final double timeGap; - private final Optional alertType; // Changed to Optional - private final Optional alertLevel; // Changed to Optional - private final MovingObjectType object1Type; // New - private final MovingObjectType object2Type; // New + private final Optional alertType; + private final Optional alertLevel; + private final MovingObjectType object1Type; + private final MovingObjectType object2Type; public ConflictCalculationResult(double distance1, double distance2, - int timeToConflict1, int timeToConflict2, - double timeGap, - Optional alertType, // Changed to Optional - Optional alertLevel, // Changed to Optional - MovingObjectType object1Type, // New - MovingObjectType object2Type) { // New + int timeToConflict1, int timeToConflict2, + double timeGap, + Optional alertType, + Optional alertLevel, + MovingObjectType object1Type, + MovingObjectType object2Type) { this.distance1 = distance1; this.distance2 = distance2; this.timeToConflict1 = timeToConflict1; this.timeToConflict2 = timeToConflict2; this.timeGap = timeGap; - this.alertType = alertType; // Changed - this.alertLevel = alertLevel; // Changed - this.object1Type = object1Type; // New - this.object2Type = object2Type; // New + this.alertType = alertType; + this.alertLevel = alertLevel; + this.object1Type = object1Type; + this.object2Type = object2Type; } public double getDistance1() { return distance1; } + public double getDistance2() { return distance2; } + public int getTimeToConflict1() { return timeToConflict1; } + public int getTimeToConflict2() { return timeToConflict2; } + public double getTimeGap() { return timeGap; } - public Optional getAlertType() { return alertType; } // Changed - public Optional getAlertLevel() { return alertLevel; } // Changed - public MovingObjectType getObject1Type() { return object1Type; } // New - public MovingObjectType getObject2Type() { return object2Type; } // New + + public Optional getAlertType() { return alertType; } + + public Optional getAlertLevel() { return alertLevel; } + + public MovingObjectType getObject1Type() { return object1Type; } + + public MovingObjectType getObject2Type() { return object2Type; } } -} \ No newline at end of file +} \ No newline at end of file diff --git a/qaup-collision/src/test/java/com/qaup/collision/pathconflict/service/PathConflictDetectionDirectionalTest.java b/qaup-collision/src/test/java/com/qaup/collision/pathconflict/service/PathConflictDetectionDirectionalTest.java new file mode 100644 index 0000000..294e6f1 --- /dev/null +++ b/qaup-collision/src/test/java/com/qaup/collision/pathconflict/service/PathConflictDetectionDirectionalTest.java @@ -0,0 +1,134 @@ +package com.qaup.collision.pathconflict.service; + +import com.qaup.collision.common.model.MovingObject; +import com.qaup.collision.common.model.MovingObject.MovingObjectType; +import com.qaup.collision.dataprocessing.service.CoordinateSystemService; +import com.qaup.collision.pathconflict.model.entity.ObjectRouteAssignment; +import com.qaup.collision.pathconflict.model.entity.TransportRoute; +import com.qaup.collision.pathconflict.repository.ConflictAlertLogRepository; +import com.qaup.collision.pathconflict.repository.ObjectRouteAssignmentRepository; +import com.qaup.collision.pathconflict.repository.TransportRouteRepository; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; +import org.locationtech.jts.geom.Coordinate; +import org.locationtech.jts.geom.GeometryFactory; +import org.locationtech.jts.geom.LineString; +import org.locationtech.jts.geom.Point; +import org.mockito.InjectMocks; +import org.mockito.Mock; +import org.mockito.junit.jupiter.MockitoExtension; +import org.springframework.context.ApplicationEventPublisher; + +import java.util.List; +import java.util.Optional; + +import static org.mockito.ArgumentMatchers.any; +import static org.mockito.ArgumentMatchers.anyDouble; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; + +@ExtendWith(MockitoExtension.class) +class PathConflictDetectionDirectionalTest { + + @Mock + private ObjectRouteAssignmentRepository objectRouteAssignmentRepository; + + @Mock + private TransportRouteRepository routeRepository; + + @Mock + private ConflictAlertLogRepository conflictAlertLogRepository; + + @Mock + private ApplicationEventPublisher eventPublisher; + + @Mock + private CoordinateSystemService coordinateSystemService; + + @InjectMocks + private PathConflictDetectionService pathConflictDetectionService; + + @Test + void shouldNotAlertWhenIntersectionIsBehindMovingDirection() throws Exception { + GeometryFactory geometryFactory = new GeometryFactory(); + + // Route 1: horizontal line, intersection at (0,0), object currently east of intersection. + LineString routeLine1 = geometryFactory.createLineString(new Coordinate[]{ + new Coordinate(-1.0, 0.0), + new Coordinate(1.0, 0.0) + }); + + // Route 2: vertical line crossing route 1 at (0,0). + LineString routeLine2 = geometryFactory.createLineString(new Coordinate[]{ + new Coordinate(0.0, -1.0), + new Coordinate(0.0, 1.0) + }); + + Point obj1Pos = geometryFactory.createPoint(new Coordinate(0.5, 0.0)); + Point obj2Pos = geometryFactory.createPoint(new Coordinate(0.0, -0.5)); + + MovingObject obj1 = MovingObject.builder() + .objectId("UV-1") + .objectName("UV-1") + .objectType(MovingObjectType.UNMANNED_VEHICLE) + .currentPosition(obj1Pos) + .currentSpeed(20.0) + .currentHeading(90.0) // East, away from intersection at x=0 + .altitude(0.0) + .build(); + + MovingObject obj2 = MovingObject.builder() + .objectId("AC-1") + .objectName("AC-1") + .objectType(MovingObjectType.AIRCRAFT) + .currentPosition(obj2Pos) + .currentSpeed(20.0) + .currentHeading(0.0) + .altitude(0.0) + .build(); + + ObjectRouteAssignment assignment1 = ObjectRouteAssignment.builder() + .objectName("UV-1") + .objectType(ObjectRouteAssignment.ObjectType.UNMANNED_VEHICLE) + .assignedRouteId(1L) + .build(); + + ObjectRouteAssignment assignment2 = ObjectRouteAssignment.builder() + .objectName("AC-1") + .objectType(ObjectRouteAssignment.ObjectType.AIRCRAFT) + .assignedRouteId(2L) + .build(); + + TransportRoute route1 = TransportRoute.builder() + .id(1L) + .routeName("R1") + .routeType(TransportRoute.RouteType.UNMANNED_VEHICLE) + .routeGeometry(routeLine1) + .build(); + + TransportRoute route2 = TransportRoute.builder() + .id(2L) + .routeName("R2") + .routeType(TransportRoute.RouteType.AIRCRAFT) + .routeGeometry(routeLine2) + .build(); + + when(objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc("UV-1", ObjectRouteAssignment.ObjectType.UNMANNED_VEHICLE)) + .thenReturn(Optional.of(assignment1)); + when(objectRouteAssignmentRepository.findFirstByObjectNameAndObjectTypeOrderByAssignedAtDesc("AC-1", ObjectRouteAssignment.ObjectType.AIRCRAFT)) + .thenReturn(Optional.of(assignment2)); + + when(routeRepository.findById(1L)).thenReturn(Optional.of(route1)); + when(routeRepository.findById(2L)).thenReturn(Optional.of(route2)); + + // Use identity transform for deterministic test geometry. + when(coordinateSystemService.convertToLocalCoordinate(anyDouble(), anyDouble())) + .thenAnswer(invocation -> new double[]{invocation.getArgument(0), invocation.getArgument(1)}); + + pathConflictDetectionService.detectPathConflicts(List.of(obj1, obj2)); + + verify(conflictAlertLogRepository, never()).save(any()); + verify(eventPublisher, never()).publishEvent(any()); + } +} \ No newline at end of file