[Domain] Prevent narrow-door wall sticking

Closes #174

Author: learncold

src/application/LayoutPreviewWidget.cpp

@@ -449,6 +449,12 @@ QPointF entryOutsideSample(const QRectF& rectangle, safecrowd::domain::StairEntr
 std::optional<std::vector<std::pair<safecrowd::domain::Point2D, safecrowd::domain::Point2D>>> barrierSegmentsAfterGap(
     const safecrowd::domain::Barrier2D& barrier,
     const safecrowd::domain::LineSegment2D& gap);
+bool spanOverlapsPolygonBoundary(
+    const safecrowd::domain::Polygon2D& polygon,
+    const safecrowd::domain::LineSegment2D& span);
+std::optional<QPointF> outsideSampleForBoundarySpan(
+    const safecrowd::domain::Polygon2D& polygon,
+    const safecrowd::domain::LineSegment2D& span);
 
 bool isVerticalLink(const safecrowd::domain::Connection2D& connection) {
     return connection.kind == safecrowd::domain::ConnectionKind::Stair
@@ -470,6 +476,23 @@ const safecrowd::domain::Zone2D* findZoneById(
     return it == layout.zones.end() ? nullptr : &(*it);
 }
 
+bool connectionVisibleOnFloor(
+    const safecrowd::domain::FacilityLayout2D& layout,
+    const safecrowd::domain::Connection2D& connection,
+    const QString& floorId) {
+    if (matchesFloor(connection.floorId, floorId)) {
+        return true;
+    }
+    if (!isVerticalLink(connection)) {
+        return false;
+    }
+
+    const auto* fromZone = findZoneById(layout, connection.fromZoneId);
+    const auto* toZone = findZoneById(layout, connection.toZoneId);
+    return (fromZone != nullptr && matchesFloor(fromZone->floorId, floorId))
+        || (toZone != nullptr && matchesFloor(toZone->floorId, floorId));
+}
+
 std::optional<std::size_t> findZoneIndexById(
     const safecrowd::domain::FacilityLayout2D& layout,
     const std::string& zoneId) {
@@ -517,8 +540,7 @@ std::optional<safecrowd::domain::LineSegment2D> stairEntrySpanForFloor(
     const safecrowd::domain::FacilityLayout2D& layout,
     const safecrowd::domain::Connection2D& connection,
     const std::string& floorId) {
-    const auto direction = stairEntryDirectionForFloor(layout, connection, floorId);
-    if (!direction.has_value()) {
+    if (!isVerticalLink(connection)) {
         return std::nullopt;
     }
 
@@ -529,6 +551,15 @@ std::optional<safecrowd::domain::LineSegment2D> stairEntrySpanForFloor(
         return std::nullopt;
     }
 
+    if (spanOverlapsPolygonBoundary(stairZone->area, connection.centerSpan)) {
+        return connection.centerSpan;
+    }
+
+    const auto direction = stairEntryDirectionForFloor(layout, connection, floorId);
+    if (!direction.has_value()) {
+        return std::nullopt;
+    }
+
     const auto bounds = polygonBounds(stairZone->area);
     if (!bounds.valid()) {
         return std::nullopt;
@@ -542,8 +573,7 @@ std::optional<QPointF> stairEntryOutsideSampleForFloor(
     const safecrowd::domain::FacilityLayout2D& layout,
     const safecrowd::domain::Connection2D& connection,
     const std::string& floorId) {
-    const auto direction = stairEntryDirectionForFloor(layout, connection, floorId);
-    if (!direction.has_value()) {
+    if (!isVerticalLink(connection)) {
         return std::nullopt;
     }
 
@@ -554,6 +584,15 @@ std::optional<QPointF> stairEntryOutsideSampleForFloor(
         return std::nullopt;
     }
 
+    if (spanOverlapsPolygonBoundary(stairZone->area, connection.centerSpan)) {
+        return outsideSampleForBoundarySpan(stairZone->area, connection.centerSpan);
+    }
+
+    const auto direction = stairEntryDirectionForFloor(layout, connection, floorId);
+    if (!direction.has_value()) {
+        return std::nullopt;
+    }
+
     const auto bounds = polygonBounds(stairZone->area);
     if (!bounds.valid()) {
         return std::nullopt;
@@ -714,6 +753,62 @@ bool pointInPolygon(const safecrowd::domain::Polygon2D& polygon, const QPointF&
     return true;
 }
 
+bool spanOverlapsPolygonBoundary(
+    const safecrowd::domain::Polygon2D& polygon,
+    const safecrowd::domain::LineSegment2D& span) {
+    const auto spanLength = std::hypot(span.end.x - span.start.x, span.end.y - span.start.y);
+    if (spanLength <= kGeometryEpsilon) {
+        return false;
+    }
+
+    const auto checkRing = [&](const auto& ring) {
+        if (ring.size() < 2) {
+            return false;
+        }
+        for (std::size_t index = 0; index < ring.size(); ++index) {
+            if (segmentsShareSpan(span, ring[index], ring[(index + 1) % ring.size()])) {
+                return true;
+            }
+        }
+        return false;
+    };
+
+    if (checkRing(polygon.outline)) {
+        return true;
+    }
+    return std::any_of(polygon.holes.begin(), polygon.holes.end(), checkRing);
+}
+
+std::optional<QPointF> outsideSampleForBoundarySpan(
+    const safecrowd::domain::Polygon2D& polygon,
+    const safecrowd::domain::LineSegment2D& span) {
+    const auto dx = span.end.x - span.start.x;
+    const auto dy = span.end.y - span.start.y;
+    const auto length = std::hypot(dx, dy);
+    if (length <= kGeometryEpsilon) {
+        return std::nullopt;
+    }
+
+    const QPointF center(
+        (span.start.x + span.end.x) * 0.5,
+        (span.start.y + span.end.y) * 0.5);
+    constexpr double sampleOffset = 0.45;
+    const QPointF normalA(-dy / length * sampleOffset, dx / length * sampleOffset);
+    const QPointF normalB(dy / length * sampleOffset, -dx / length * sampleOffset);
+    const QPointF sampleA = center + normalA;
+    const QPointF sampleB = center + normalB;
+    const bool sampleAInside = pointInPolygon(polygon, sampleA);
+    const bool sampleBInside = pointInPolygon(polygon, sampleB);
+
+    if (!sampleAInside && sampleBInside) {
+        return sampleA;
+    }
+    if (!sampleBInside && sampleAInside) {
+        return sampleB;
+    }
+    return std::nullopt;
+}
+
 double distanceToLineSegmentWorld(const QPointF& point, const safecrowd::domain::Point2D& start, const safecrowd::domain::Point2D& end) {
     return distanceToSegment(point, QPointF(start.x, start.y), QPointF(end.x, end.y));
 }
@@ -1970,7 +2065,7 @@ std::optional<QString> hitTestConnection(
     std::optional<QString> bestId;
 
     for (const auto& connection : layout.connections) {
-        if (!matchesFloor(connection.floorId, floorId)) {
+        if (!connectionVisibleOnFloor(layout, connection, floorId)) {
             continue;
         }
         const auto start = transform.map(connection.centerSpan.start);
@@ -2057,7 +2152,7 @@ bool selectedConnectionContainsContextPoint(
 
     for (const auto& connection : layout.connections) {
         const auto connectionId = QString::fromStdString(connection.id);
-        if (!selectedConnectionIds.contains(connectionId) || !matchesFloor(connection.floorId, floorId)) {
+        if (!selectedConnectionIds.contains(connectionId) || !connectionVisibleOnFloor(layout, connection, floorId)) {
             continue;
         }
 
@@ -2744,7 +2839,7 @@ void LayoutPreviewWidget::paintEvent(QPaintEvent* event) {
                 }
             }
             for (const auto& connection : importResult_.layout->connections) {
-                if (!matchesFloor(connection.floorId, currentFloorId())) {
+                if (!connectionVisibleOnFloor(*importResult_.layout, connection, currentFloorId())) {
                     continue;
                 }
                 const auto id = QString::fromStdString(connection.id);
@@ -4336,7 +4431,7 @@ void LayoutPreviewWidget::selectElementsInRect(const QRectF& screenRect, const L
         }
     }
     for (const auto& connection : layout.connections) {
-        if (!matchesFloor(connection.floorId, floorId)) {
+        if (!connectionVisibleOnFloor(layout, connection, floorId)) {
             continue;
         }
         if (strokedPathIntersectsRect(linePath(connection.centerSpan, transform), screenRect, kSelectionStrokeWidthPixels)) {

src/domain/ScenarioSimulationInternal.cpp

@@ -376,6 +376,73 @@ std::vector<LineSegment2D> stairEntryBarrierSegments(const Zone2D& zone, StairEn
     return segments;
 }
 
+std::vector<LineSegment2D> barrierSegmentAfterConnectionGap(
+    const LineSegment2D& segment,
+    const LineSegment2D& gap) {
+    const auto segmentDelta = segment.end - segment.start;
+    const auto segmentLength = lengthOf(segmentDelta);
+    if (segmentLength <= kGeometryEpsilon) {
+        return {};
+    }
+
+    if (std::fabs(cross(segment.start, segment.end, gap.start)) > kGeometryEpsilon
+        || std::fabs(cross(segment.start, segment.end, gap.end)) > kGeometryEpsilon) {
+        return {segment};
+    }
+
+    const auto direction = segmentDelta * (1.0 / segmentLength);
+    auto gapStart = dot(gap.start - segment.start, direction);
+    auto gapEnd = dot(gap.end - segment.start, direction);
+    if (gapStart > gapEnd) {
+        std::swap(gapStart, gapEnd);
+    }
+
+    const auto overlapStart = std::max(0.0, gapStart);
+    const auto overlapEnd = std::min(segmentLength, gapEnd);
+    if (overlapEnd <= overlapStart + kGeometryEpsilon) {
+        return {segment};
+    }
+
+    std::vector<LineSegment2D> remaining;
+    if (overlapStart > kGeometryEpsilon) {
+        remaining.push_back({
+            .start = segment.start,
+            .end = segment.start + (direction * overlapStart),
+        });
+    }
+    if (overlapEnd < segmentLength - kGeometryEpsilon) {
+        remaining.push_back({
+            .start = segment.start + (direction * overlapEnd),
+            .end = segment.end,
+        });
+    }
+    return remaining;
+}
+
+std::vector<LineSegment2D> clipStairEntryBarrierSegment(
+    const LineSegment2D& segment,
+    const FacilityLayout2D& source,
+    const Zone2D& stairZone) {
+    std::vector<LineSegment2D> remaining{segment};
+    for (const auto& connection : source.connections) {
+        if (!isVerticalConnection(connection)
+            || (connection.fromZoneId != stairZone.id && connection.toZoneId != stairZone.id)) {
+            continue;
+        }
+
+        std::vector<LineSegment2D> next;
+        for (const auto& candidate : remaining) {
+            auto clipped = barrierSegmentAfterConnectionGap(candidate, connection.centerSpan);
+            next.insert(next.end(), clipped.begin(), clipped.end());
+        }
+        remaining = std::move(next);
+        if (remaining.empty()) {
+            break;
+        }
+    }
+    return remaining;
+}
+
 void appendStairEntryBarriers(FacilityLayout2D& filtered, const FacilityLayout2D& source, const std::string& floorId) {
     int suffix = 1;
     for (const auto& connection : source.connections) {
@@ -394,12 +461,14 @@ void appendStairEntryBarriers(FacilityLayout2D& filtered, const FacilityLayout2D
         }
 
         for (const auto& segment : stairEntryBarrierSegments(*stairZone, direction)) {
-            filtered.barriers.push_back({
-                .id = connection.id + "-entry-wall-" + std::to_string(suffix++),
-                .floorId = stairZone->floorId,
-                .geometry = {.vertices = {segment.start, segment.end}},
-                .blocksMovement = true,
-            });
+            for (const auto& clippedSegment : clipStairEntryBarrierSegment(segment, source, *stairZone)) {
+                filtered.barriers.push_back({
+                    .id = connection.id + "-entry-wall-" + std::to_string(suffix++),
+                    .floorId = stairZone->floorId,
+                    .geometry = {.vertices = {clippedSegment.start, clippedSegment.end}},
+                    .blocksMovement = true,
+                });
+            }
         }
     }
 }
@@ -995,7 +1064,7 @@ bool pointInsideClosedBarrier(const FacilityLayout2D& layout, const Point2D& poi
 }
 
 bool pointHasClearance(const FacilityLayout2D& layout, const Point2D& point, double clearance) {
-    if (!pointInAnyZone(layout, point) || pointInsideClosedBarrier(layout, point)) {
+    if ((!layout.zones.empty() && !pointInAnyZone(layout, point)) || pointInsideClosedBarrier(layout, point)) {
         return false;
     }
 
@@ -1371,18 +1440,29 @@ std::vector<Point2D> buildPath(const FacilityLayout2D& layout, const Point2D& st
     return path;
 }
 
-Point2D constrainedMove(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to) {
-    if (!movementCrossesBarrier(layout, from, to)) {
+Point2D constrainedMove(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to, double clearance) {
+    const auto effectiveClearance = std::max(0.0, clearance);
+    auto validDestination = [&](const Point2D& point) {
+        if (effectiveClearance > 0.0) {
+            return pointHasClearance(layout, point, effectiveClearance);
+        }
+        return (layout.zones.empty() || pointInAnyZone(layout, point)) && !pointInsideClosedBarrier(layout, point);
+    };
+    auto validMove = [&](const Point2D& candidate) {
+        return validDestination(candidate) && !movementCrossesBarrier(layout, from, candidate);
+    };
+
+    if (validMove(to)) {
         return to;
     }
 
     const Point2D xOnly{.x = to.x, .y = from.y};
-    if (!movementCrossesBarrier(layout, from, xOnly)) {
+    if (validMove(xOnly)) {
         return xOnly;
     }
 
     const Point2D yOnly{.x = from.x, .y = to.y};
-    if (!movementCrossesBarrier(layout, from, yOnly)) {
+    if (validMove(yOnly)) {
         return yOnly;
     }
 
@@ -1392,11 +1472,11 @@ Point2D constrainedMove(const FacilityLayout2D& layout, const Point2D& from, con
     for (int i = 0; i < 8; ++i) {
         const auto t = (low + high) * 0.5;
         const auto candidate = from + ((to - from) * t);
-        if (movementCrossesBarrier(layout, from, candidate)) {
-            high = t;
-        } else {
+        if (validMove(candidate)) {
             low = t;
             best = candidate;
+        } else {
+            high = t;
         }
     }
     return best;

src/domain/ScenarioSimulationInternal.h

@@ -161,6 +161,6 @@ Point2D barrierSeparationVelocity(const FacilityLayout2D& layout, const Position
 bool movementCrossesBarrier(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to);
 bool lineOfSightClear(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to, double clearance);
 std::vector<Point2D> buildPath(const FacilityLayout2D& layout, const Point2D& start, const Point2D& goal, double clearance);
-Point2D constrainedMove(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to);
+Point2D constrainedMove(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to, double clearance = 0.0);
 
 }  // namespace safecrowd::domain::simulation_internal