feat(layout_optimizer): crossing penalty weighted by intersection length

Replace _line_of_sight_penalty (flat per-blocker) with _crossing_penalty
(DEFAULT_WEIGHT_DISTANCE * crossing_length). Uses opening→arm-OBB
endpoints. Applied regardless of reachability pass/fail.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

unilabos/layout_optimizer/constraints.py

@@ -16,7 +16,7 @@ from .obb import (
     obb_corners,
     obb_min_distance,
     obb_penetration_depth,
-    segment_intersects_obb,
+    segment_obb_intersection_length,
 )
 
 if TYPE_CHECKING:
@@ -298,10 +298,7 @@ def _evaluate_single(
         arm_dev = device_map.get(arm_id)
         target_dev = device_map.get(target_device_id)
 
-        # Distance from target's opening surface center to nearest point on arm OBB.
-        # This naturally enforces orientation: a device facing away has its opening
-        # far from the arm, so it fails reachability without needing a separate
-        # facing penalty.
+        # opening surface center → nearest point on arm OBB
         if arm_dev and target_dev:
             opening_pt = _opening_surface_center(target_dev, target_p)
             arm_corners = obb_corners(
@@ -310,27 +307,30 @@ def _evaluate_single(
             nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
             dist = math.sqrt((opening_pt[0] - nearest[0])**2 + (opening_pt[1] - nearest[1])**2)
         else:
+            opening_pt = (target_p.x, target_p.y)
+            nearest = (arm_p.x, arm_p.y)
             dist = _device_distance_center(arm_p, target_p) or 0.0
 
+        # 交叉惩罚始终计算（soft, 不依赖可达性结果）
+        crossing_cost = _crossing_penalty(
+            opening_pt, nearest,
+            arm_id, target_device_id,
+            device_map, placement_map,
+        )
+
         arm_pose = {"x": arm_p.x, "y": arm_p.y, "theta": arm_p.theta}
         target_point = {"x": target_p.x, "y": target_p.y, "z": 0.0}
         target_point["_obb_dist"] = dist
         if not reachability_checker.is_reachable(arm_id, arm_pose, target_point):
             if is_hard and not graduated:
                 return math.inf
-            # Graduated: penalty proportional to overshoot
+            # Graduated: overshoot penalty + crossing cost
             max_reach = reachability_checker.arm_reach.get(arm_id, 2.0)
             overshoot = max(0.0, dist - max_reach)
             w = effective_weight * (HARD_MULTIPLIER if is_hard else 1.0)
-            return w * overshoot * 10.0
+            return w * overshoot * 10.0 + crossing_cost
 
-        # Line-of-sight penalty: penalize if any other device OBB blocks
-        # the path from opening to arm
-        los_cost = _line_of_sight_penalty(
-            arm_id, arm_p, target_device_id, target_p,
-            device_map, placement_map, effective_weight,
-        )
-        return los_cost
+        return crossing_cost
 
     if rule == "prefer_aligned":
         alignment_cost = sum(
@@ -576,23 +576,19 @@ def _constraint_display_name(c: Constraint) -> str:
     return c.rule_name
 
 
-def _line_of_sight_penalty(
+def _crossing_penalty(
+    opening_pt: tuple[float, float],
+    arm_nearest_pt: tuple[float, float],
     arm_id: str,
-    arm_p: Placement,
     target_id: str,
-    target_p: Placement,
     device_map: dict[str, Device],
     placement_map: dict[str, Placement],
-    weight: float,
 ) -> float:
-    """Penalty for other devices blocking the line from target to arm center.
+    """交叉惩罚：其他设备 OBB 遮挡 opening→arm 路径的长度加权 penalty。
 
-    For each other device whose OBB intersects the segment (target_center → arm_center),
-    adds a penalty proportional to the weight. This encourages layouts where
-    the arm has a clear path to each target.
+    Soft penalty，权重 = DEFAULT_WEIGHT_DISTANCE * 穿过各遮挡设备 OBB 的线段长度之和。
+    始终生效（不论可达性是否通过），为 DE 提供清晰的梯度信号。
     """
-    p1 = (target_p.x, target_p.y)
-    p2 = (arm_p.x, arm_p.y)
     cost = 0.0
     for dev_id, p in placement_map.items():
         if dev_id == arm_id or dev_id == target_id:
@@ -601,6 +597,6 @@ def _line_of_sight_penalty(
         if dev is None:
             continue
         corners = obb_corners(p.x, p.y, dev.bbox[0], dev.bbox[1], p.theta)
-        if segment_intersects_obb(p1, p2, corners):
-            cost += weight * 2.0  # penalty per blocking device
+        crossing_len = segment_obb_intersection_length(opening_pt, arm_nearest_pt, corners)
+        cost += DEFAULT_WEIGHT_DISTANCE * crossing_len
     return cost

unilabos/layout_optimizer/tests/test_constraints.py

@@ -5,11 +5,15 @@ import math
 import pytest
 
 from ..constraints import (
+    _crossing_penalty,
+    _opening_surface_center,
+    DEFAULT_WEIGHT_DISTANCE,
     evaluate_constraints,
     evaluate_default_hard_constraints,
 )
 from ..mock_checkers import MockCollisionChecker, MockReachabilityChecker
-from ..models import Constraint, Device, Lab, Placement
+from ..models import Constraint, Device, Opening, Placement, Lab
+from ..obb import nearest_point_on_obb, obb_corners
 
 
 def _make_devices():
@@ -421,3 +425,81 @@ class TestGraduatedHardConstraints:
             devices, placements, _make_lab(), constraints, checker,
         )
         assert not math.isinf(cost)
+
+
+class TestCrossingPenalty:
+    """_crossing_penalty: 交叉长度加权的 soft penalty。"""
+
+    def _make_device(self, dev_id, bbox=(0.5, 0.5), direction=(0.0, -1.0)):
+        return Device(
+            id=dev_id, name=dev_id, device_type="static",
+            bbox=bbox, height=0.3,
+            openings=[Opening(direction=direction, label="front")],
+        )
+
+    def test_no_blockers_returns_zero(self):
+        """arm 与 target 之间无遮挡设备 → 交叉代价为 0。"""
+        arm = self._make_device("arm", bbox=(2.14, 0.35))
+        target = self._make_device("target")
+        arm_p = Placement(device_id="arm", x=2.0, y=1.0, theta=0.0)
+        target_p = Placement(device_id="target", x=0.5, y=1.0, theta=3.14159)
+        device_map = {"arm": arm, "target": target}
+        placement_map = {"arm": arm_p, "target": target_p}
+
+        opening_pt = _opening_surface_center(target, target_p)
+        arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
+        nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
+
+        cost = _crossing_penalty(
+            opening_pt, nearest,
+            "arm", "target",
+            device_map, placement_map,
+        )
+        assert cost == 0.0
+
+    def test_one_blocker_proportional_to_length(self):
+        """一个遮挡设备 → cost = DEFAULT_WEIGHT_DISTANCE * 穿过长度。"""
+        arm = self._make_device("arm", bbox=(2.14, 0.35))
+        target = self._make_device("target")
+        blocker = self._make_device("blocker", bbox=(0.5, 0.5))
+        arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0)
+        target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0)
+        blocker_p = Placement(device_id="blocker", x=1.5, y=1.0, theta=0.0)
+        device_map = {"arm": arm, "target": target, "blocker": blocker}
+        placement_map = {"arm": arm_p, "target": target_p, "blocker": blocker_p}
+
+        opening_pt = _opening_surface_center(target, target_p)
+        arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
+        nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
+
+        cost = _crossing_penalty(
+            opening_pt, nearest,
+            "arm", "target",
+            device_map, placement_map,
+        )
+        # blocker 宽 0.5m，theta=0，路径水平 → 穿过长度 ≈ 0.5m
+        # cost = DEFAULT_WEIGHT_DISTANCE * 0.5 = 100 * 0.5 = 50
+        assert cost > 0
+        assert abs(cost - DEFAULT_WEIGHT_DISTANCE * 0.5) < DEFAULT_WEIGHT_DISTANCE * 0.1
+
+    def test_blocker_off_path_returns_zero(self):
+        """不在路径上的设备 → 交叉代价为 0。"""
+        arm = self._make_device("arm", bbox=(2.14, 0.35))
+        target = self._make_device("target")
+        bystander = self._make_device("bystander", bbox=(0.5, 0.5))
+        arm_p = Placement(device_id="arm", x=3.0, y=1.0, theta=0.0)
+        target_p = Placement(device_id="target", x=0.0, y=1.0, theta=0.0)
+        bystander_p = Placement(device_id="bystander", x=1.5, y=3.0, theta=0.0)
+        device_map = {"arm": arm, "target": target, "bystander": bystander}
+        placement_map = {"arm": arm_p, "target": target_p, "bystander": bystander_p}
+
+        opening_pt = _opening_surface_center(target, target_p)
+        arm_corners = obb_corners(arm_p.x, arm_p.y, arm.bbox[0], arm.bbox[1], arm_p.theta)
+        nearest = nearest_point_on_obb(opening_pt[0], opening_pt[1], arm_corners)
+
+        cost = _crossing_penalty(
+            opening_pt, nearest,
+            "arm", "target",
+            device_map, placement_map,
+        )
+        assert cost == 0.0