@esengine/pathfinding 12.1.2 → 13.1.0

package/dist/chunk-JTZP55BJ.js

@@ -0,0 +1,935 @@
+import {
+  __name,
+  __publicField
+} from "./chunk-T626JPC7.js";
+
+// src/avoidance/ILocalAvoidance.ts
+var DEFAULT_ORCA_CONFIG = {
+  defaultTimeHorizon: 2,
+  defaultTimeHorizonObst: 1,
+  timeStep: 1 / 60,
+  epsilon: 1e-5
+};
+var DEFAULT_AGENT_PARAMS = {
+  radius: 0.5,
+  maxSpeed: 5,
+  neighborDist: 15,
+  maxNeighbors: 10,
+  timeHorizon: 2,
+  timeHorizonObst: 1
+};
+
+// src/avoidance/LinearProgram.ts
+import { Vector2 } from "@esengine/ecs-framework-math";
+var EPSILON = 1e-5;
+var { dot, det, lengthSq } = Vector2;
+function linearProgram1(lines, lineNo, radius, optVelocity, directionOpt, result) {
+  const line = lines[lineNo];
+  const dotProduct = dot(line.point, line.direction);
+  const discriminant = dotProduct * dotProduct + radius * radius - lengthSq(line.point);
+  if (discriminant < 0) {
+    return false;
+  }
+  const sqrtDiscriminant = Math.sqrt(discriminant);
+  let tLeft = -dotProduct - sqrtDiscriminant;
+  let tRight = -dotProduct + sqrtDiscriminant;
+  for (let i = 0; i < lineNo; i++) {
+    const constraint = lines[i];
+    const denominator = det(line.direction, constraint.direction);
+    const numerator = det(constraint.direction, {
+      x: line.point.x - constraint.point.x,
+      y: line.point.y - constraint.point.y
+    });
+    if (Math.abs(denominator) <= EPSILON) {
+      if (numerator < 0) {
+        return false;
+      }
+      continue;
+    }
+    const t2 = numerator / denominator;
+    if (denominator >= 0) {
+      tRight = Math.min(tRight, t2);
+    } else {
+      tLeft = Math.max(tLeft, t2);
+    }
+    if (tLeft > tRight) {
+      return false;
+    }
+  }
+  let t;
+  if (directionOpt) {
+    if (dot(optVelocity, line.direction) > 0) {
+      t = tRight;
+    } else {
+      t = tLeft;
+    }
+  } else {
+    t = dot(line.direction, {
+      x: optVelocity.x - line.point.x,
+      y: optVelocity.y - line.point.y
+    });
+    if (t < tLeft) {
+      t = tLeft;
+    } else if (t > tRight) {
+      t = tRight;
+    }
+  }
+  result.x = line.point.x + t * line.direction.x;
+  result.y = line.point.y + t * line.direction.y;
+  return true;
+}
+__name(linearProgram1, "linearProgram1");
+function linearProgram2(lines, radius, optVelocity, directionOpt, result) {
+  if (directionOpt) {
+    result.x = optVelocity.x * radius;
+    result.y = optVelocity.y * radius;
+  } else if (lengthSq(optVelocity) > radius * radius) {
+    const len2 = Math.sqrt(lengthSq(optVelocity));
+    result.x = optVelocity.x / len2 * radius;
+    result.y = optVelocity.y / len2 * radius;
+  } else {
+    result.x = optVelocity.x;
+    result.y = optVelocity.y;
+  }
+  for (let i = 0; i < lines.length; i++) {
+    const line = lines[i];
+    const detVal = det(line.direction, {
+      x: line.point.x - result.x,
+      y: line.point.y - result.y
+    });
+    if (detVal > 0) {
+      const tempResult = result.clone();
+      if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
+        result.copy(tempResult);
+        return i;
+      }
+    }
+  }
+  return lines.length;
+}
+__name(linearProgram2, "linearProgram2");
+function linearProgram3(lines, numObstLines, beginLine, radius, result) {
+  let distance = 0;
+  for (let i = beginLine; i < lines.length; i++) {
+    const line = lines[i];
+    if (det(line.direction, {
+      x: line.point.x - result.x,
+      y: line.point.y - result.y
+    }) > distance) {
+      const projLines = [];
+      for (let j = 0; j < numObstLines; j++) {
+        projLines.push(lines[j]);
+      }
+      for (let j = numObstLines; j < i; j++) {
+        const line1 = lines[j];
+        const line2 = lines[i];
+        let newLine;
+        const determinant = det(line1.direction, line2.direction);
+        if (Math.abs(determinant) <= EPSILON) {
+          if (dot(line1.direction, line2.direction) > 0) {
+            continue;
+          }
+          newLine = {
+            point: {
+              x: 0.5 * (line1.point.x + line2.point.x),
+              y: 0.5 * (line1.point.y + line2.point.y)
+            },
+            direction: {
+              x: 0,
+              y: 0
+            }
+          };
+        } else {
+          const diff = {
+            x: line1.point.x - line2.point.x,
+            y: line1.point.y - line2.point.y
+          };
+          const t = det(line2.direction, diff) / determinant;
+          newLine = {
+            point: {
+              x: line1.point.x + t * line1.direction.x,
+              y: line1.point.y + t * line1.direction.y
+            },
+            direction: {
+              x: 0,
+              y: 0
+            }
+          };
+        }
+        const dirDiff = {
+          x: line1.direction.x - line2.direction.x,
+          y: line1.direction.y - line2.direction.y
+        };
+        const dirLen = Math.sqrt(lengthSq(dirDiff));
+        if (dirLen > EPSILON) {
+          newLine.direction.x = dirDiff.x / dirLen;
+          newLine.direction.y = dirDiff.y / dirLen;
+        }
+        projLines.push(newLine);
+      }
+      const tempResult = result.clone();
+      const optVelocity = {
+        x: -lines[i].direction.y,
+        y: lines[i].direction.x
+      };
+      if (linearProgram2(projLines, radius, optVelocity, true, result) < projLines.length) {
+        result.copy(tempResult);
+      }
+      distance = det(lines[i].direction, {
+        x: lines[i].point.x - result.x,
+        y: lines[i].point.y - result.y
+      });
+    }
+  }
+}
+__name(linearProgram3, "linearProgram3");
+function solveORCALinearProgram(lines, numObstLines, maxSpeed, preferredVelocity) {
+  const result = new Vector2();
+  const lineFail = linearProgram2(lines, maxSpeed, preferredVelocity, false, result);
+  if (lineFail < lines.length) {
+    linearProgram3(lines, numObstLines, lineFail, maxSpeed, result);
+  }
+  return result;
+}
+__name(solveORCALinearProgram, "solveORCALinearProgram");
+
+// src/avoidance/ObstacleBuilder.ts
+var EPSILON2 = 1e-5;
+function leftOf(p1, p2, p3) {
+  return (p1.x - p3.x) * (p2.y - p1.y) - (p1.y - p3.y) * (p2.x - p1.x);
+}
+__name(leftOf, "leftOf");
+function createObstacleVertices(vertices, startId = 0) {
+  const n = vertices.length;
+  if (n < 2) {
+    return [];
+  }
+  const obstacleVertices = [];
+  for (let i = 0; i < n; i++) {
+    obstacleVertices.push({
+      point: {
+        x: vertices[i].x,
+        y: vertices[i].y
+      },
+      direction: {
+        x: 0,
+        y: 0
+      },
+      next: null,
+      previous: null,
+      isConvex: false,
+      id: startId + i
+    });
+  }
+  for (let i = 0; i < n; i++) {
+    const curr = obstacleVertices[i];
+    const next = obstacleVertices[(i + 1) % n];
+    const prev = obstacleVertices[(i + n - 1) % n];
+    curr.next = next;
+    curr.previous = prev;
+    const dx = next.point.x - curr.point.x;
+    const dy = next.point.y - curr.point.y;
+    const edgeLen = Math.sqrt(dx * dx + dy * dy);
+    if (edgeLen > EPSILON2) {
+      curr.direction = {
+        x: dx / edgeLen,
+        y: dy / edgeLen
+      };
+    } else {
+      curr.direction = {
+        x: 1,
+        y: 0
+      };
+    }
+  }
+  for (let i = 0; i < n; i++) {
+    const curr = obstacleVertices[i];
+    const prev = curr.previous;
+    const next = curr.next;
+    curr.isConvex = leftOf(prev.point, curr.point, next.point) >= 0;
+  }
+  return obstacleVertices;
+}
+__name(createObstacleVertices, "createObstacleVertices");
+function buildObstacleVertices(obstacles) {
+  const allVertices = [];
+  let nextId = 0;
+  for (const obstacle of obstacles) {
+    const vertices = createObstacleVertices(obstacle.vertices, nextId);
+    allVertices.push(...vertices);
+    nextId += vertices.length;
+  }
+  return allVertices;
+}
+__name(buildObstacleVertices, "buildObstacleVertices");
+function ensureCCW(vertices, yAxisDown = false) {
+  if (vertices.length < 3) {
+    return vertices;
+  }
+  let signedArea = 0;
+  for (let i = 0; i < vertices.length; i++) {
+    const curr = vertices[i];
+    const next = vertices[(i + 1) % vertices.length];
+    signedArea += curr.x * next.y - next.x * curr.y;
+  }
+  signedArea *= 0.5;
+  const isCCW = yAxisDown ? signedArea < 0 : signedArea > 0;
+  if (isCCW) {
+    return vertices;
+  }
+  return [
+    ...vertices
+  ].reverse();
+}
+__name(ensureCCW, "ensureCCW");
+
+// src/avoidance/ORCASolver.ts
+import { Vector2 as Vector22 } from "@esengine/ecs-framework-math";
+var EPSILON3 = 1e-5;
+var { det: det2, dot: dot2, lengthSq: lengthSq2, len } = Vector22;
+function normalize(v) {
+  const length = len(v);
+  if (length < EPSILON3) {
+    return {
+      x: 0,
+      y: 0
+    };
+  }
+  return {
+    x: v.x / length,
+    y: v.y / length
+  };
+}
+__name(normalize, "normalize");
+var _ORCASolver = class _ORCASolver {
+  constructor(config = {}) {
+    __publicField(this, "config");
+    this.config = {
+      ...DEFAULT_ORCA_CONFIG,
+      ...config
+    };
+  }
+  /**
+   * @zh 计算代理的新速度
+   * @en Compute new velocity for agent
+   *
+   * @param agent - @zh 当前代理 @en Current agent
+   * @param neighbors - @zh 邻近代理列表 @en List of neighboring agents
+   * @param obstacles - @zh 障碍物列表 @en List of obstacles
+   * @param deltaTime - @zh 时间步长 @en Time step
+   * @returns @zh 计算得到的新速度 @en Computed new velocity
+   */
+  computeNewVelocity(agent, neighbors, obstacles, deltaTime) {
+    const orcaLines = [];
+    const obstacleVertices = buildObstacleVertices(obstacles);
+    const numObstLines = this.createObstacleORCALines(agent, obstacleVertices, orcaLines);
+    this.createAgentORCALines(agent, neighbors, deltaTime, orcaLines);
+    return solveORCALinearProgram(orcaLines, numObstLines, agent.maxSpeed, agent.preferredVelocity);
+  }
+  /**
+   * @zh 创建代理间的 ORCA 约束线
+   * @en Create ORCA constraint lines for agent-agent avoidance
+   */
+  createAgentORCALines(agent, neighbors, deltaTime, orcaLines) {
+    const invTimeHorizon = 1 / agent.timeHorizon;
+    for (const other of neighbors) {
+      if (other.id === agent.id) continue;
+      const relativePosition = {
+        x: other.position.x - agent.position.x,
+        y: other.position.y - agent.position.y
+      };
+      const relativeVelocity = {
+        x: agent.velocity.x - other.velocity.x,
+        y: agent.velocity.y - other.velocity.y
+      };
+      const distSq = lengthSq2(relativePosition);
+      const combinedRadius = agent.radius + other.radius;
+      const combinedRadiusSq = combinedRadius * combinedRadius;
+      const line = {
+        point: {
+          x: 0,
+          y: 0
+        },
+        direction: {
+          x: 0,
+          y: 0
+        }
+      };
+      let u;
+      if (distSq > combinedRadiusSq) {
+        const w = {
+          x: relativeVelocity.x - invTimeHorizon * relativePosition.x,
+          y: relativeVelocity.y - invTimeHorizon * relativePosition.y
+        };
+        const wLengthSq = lengthSq2(w);
+        const dotProduct1 = dot2(w, relativePosition);
+        if (dotProduct1 < 0 && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) {
+          const wLength = Math.sqrt(wLengthSq);
+          const unitW = normalize(w);
+          line.direction = {
+            x: unitW.y,
+            y: -unitW.x
+          };
+          u = {
+            x: (combinedRadius * invTimeHorizon - wLength) * unitW.x,
+            y: (combinedRadius * invTimeHorizon - wLength) * unitW.y
+          };
+        } else {
+          const leg = Math.sqrt(distSq - combinedRadiusSq);
+          if (det2(relativePosition, w) > 0) {
+            line.direction = {
+              x: (relativePosition.x * leg - relativePosition.y * combinedRadius) / distSq,
+              y: (relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq
+            };
+          } else {
+            line.direction = {
+              x: -(relativePosition.x * leg + relativePosition.y * combinedRadius) / distSq,
+              y: -(-relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq
+            };
+          }
+          const dotProduct2 = dot2(relativeVelocity, line.direction);
+          u = {
+            x: dotProduct2 * line.direction.x - relativeVelocity.x,
+            y: dotProduct2 * line.direction.y - relativeVelocity.y
+          };
+        }
+      } else {
+        const invTimeStep = 1 / deltaTime;
+        const w = {
+          x: relativeVelocity.x - invTimeStep * relativePosition.x,
+          y: relativeVelocity.y - invTimeStep * relativePosition.y
+        };
+        const wLength = len(w);
+        const unitW = wLength > EPSILON3 ? {
+          x: w.x / wLength,
+          y: w.y / wLength
+        } : {
+          x: 1,
+          y: 0
+        };
+        line.direction = {
+          x: unitW.y,
+          y: -unitW.x
+        };
+        u = {
+          x: (combinedRadius * invTimeStep - wLength) * unitW.x,
+          y: (combinedRadius * invTimeStep - wLength) * unitW.y
+        };
+      }
+      line.point = {
+        x: agent.velocity.x + 0.5 * u.x,
+        y: agent.velocity.y + 0.5 * u.y
+      };
+      orcaLines.push(line);
+    }
+  }
+  /**
+   * @zh 创建障碍物的 ORCA 约束线
+   * @en Create ORCA constraint lines for obstacle avoidance
+   */
+  createObstacleORCALines(agent, obstacleVertices, orcaLines) {
+    const invTimeHorizonObst = 1 / agent.timeHorizonObst;
+    const radiusSq = agent.radius * agent.radius;
+    let numObstLines = 0;
+    for (const obstacle1 of obstacleVertices) {
+      const obstacle2 = obstacle1.next;
+      const relativePosition1 = {
+        x: obstacle1.point.x - agent.position.x,
+        y: obstacle1.point.y - agent.position.y
+      };
+      const relativePosition2 = {
+        x: obstacle2.point.x - agent.position.x,
+        y: obstacle2.point.y - agent.position.y
+      };
+      const obstacleVector = {
+        x: obstacle2.point.x - obstacle1.point.x,
+        y: obstacle2.point.y - obstacle1.point.y
+      };
+      const signedDistToEdge = det2(obstacleVector, relativePosition1);
+      if (signedDistToEdge < -EPSILON3) {
+        continue;
+      }
+      let alreadyCovered = false;
+      for (const existingLine of orcaLines) {
+        const scaledRelPos1 = {
+          x: invTimeHorizonObst * relativePosition1.x - existingLine.point.x,
+          y: invTimeHorizonObst * relativePosition1.y - existingLine.point.y
+        };
+        const scaledRelPos2 = {
+          x: invTimeHorizonObst * relativePosition2.x - existingLine.point.x,
+          y: invTimeHorizonObst * relativePosition2.y - existingLine.point.y
+        };
+        if (det2(scaledRelPos1, existingLine.direction) - invTimeHorizonObst * agent.radius >= -EPSILON3 && det2(scaledRelPos2, existingLine.direction) - invTimeHorizonObst * agent.radius >= -EPSILON3) {
+          alreadyCovered = true;
+          break;
+        }
+      }
+      if (alreadyCovered) {
+        continue;
+      }
+      const distSq1 = lengthSq2(relativePosition1);
+      const distSq2 = lengthSq2(relativePosition2);
+      const obstacleVectorSq = lengthSq2(obstacleVector);
+      const s = obstacleVectorSq > EPSILON3 ? -dot2(relativePosition1, obstacleVector) / obstacleVectorSq : 0;
+      const distSqLineToEdge = lengthSq2({
+        x: -relativePosition1.x - s * obstacleVector.x,
+        y: -relativePosition1.y - s * obstacleVector.y
+      });
+      const line = {
+        point: {
+          x: 0,
+          y: 0
+        },
+        direction: {
+          x: 0,
+          y: 0
+        }
+      };
+      if (s < 0 && distSq1 <= radiusSq) {
+        if (obstacle1.isConvex) {
+          line.point = {
+            x: 0,
+            y: 0
+          };
+          line.direction = normalize({
+            x: -relativePosition1.y,
+            y: relativePosition1.x
+          });
+          orcaLines.push(line);
+          numObstLines++;
+        }
+        continue;
+      }
+      if (s > 1 && distSq2 <= radiusSq) {
+        if (obstacle2.isConvex && det2(relativePosition2, obstacle2.direction) >= 0) {
+          line.point = {
+            x: 0,
+            y: 0
+          };
+          line.direction = normalize({
+            x: -relativePosition2.y,
+            y: relativePosition2.x
+          });
+          orcaLines.push(line);
+          numObstLines++;
+        }
+        continue;
+      }
+      if (s >= 0 && s <= 1 && distSqLineToEdge <= radiusSq) {
+        line.point = {
+          x: 0,
+          y: 0
+        };
+        line.direction = {
+          x: -obstacle1.direction.x,
+          y: -obstacle1.direction.y
+        };
+        orcaLines.push(line);
+        numObstLines++;
+        continue;
+      }
+      let obs1 = obstacle1;
+      let obs2 = obstacle2;
+      let leftLegDirection;
+      let rightLegDirection;
+      if (s < 0 && distSqLineToEdge <= radiusSq) {
+        if (!obstacle1.isConvex) continue;
+        obs2 = obstacle1;
+        const leg1 = Math.sqrt(Math.max(0, distSq1 - radiusSq));
+        leftLegDirection = {
+          x: (relativePosition1.x * leg1 - relativePosition1.y * agent.radius) / distSq1,
+          y: (relativePosition1.x * agent.radius + relativePosition1.y * leg1) / distSq1
+        };
+        rightLegDirection = {
+          x: (relativePosition1.x * leg1 + relativePosition1.y * agent.radius) / distSq1,
+          y: (-relativePosition1.x * agent.radius + relativePosition1.y * leg1) / distSq1
+        };
+      } else if (s > 1 && distSqLineToEdge <= radiusSq) {
+        if (!obstacle2.isConvex) continue;
+        obs1 = obstacle2;
+        const leg2 = Math.sqrt(Math.max(0, distSq2 - radiusSq));
+        leftLegDirection = {
+          x: (relativePosition2.x * leg2 - relativePosition2.y * agent.radius) / distSq2,
+          y: (relativePosition2.x * agent.radius + relativePosition2.y * leg2) / distSq2
+        };
+        rightLegDirection = {
+          x: (relativePosition2.x * leg2 + relativePosition2.y * agent.radius) / distSq2,
+          y: (-relativePosition2.x * agent.radius + relativePosition2.y * leg2) / distSq2
+        };
+      } else {
+        if (obstacle1.isConvex) {
+          const leg1 = Math.sqrt(Math.max(0, distSq1 - radiusSq));
+          leftLegDirection = {
+            x: (relativePosition1.x * leg1 - relativePosition1.y * agent.radius) / distSq1,
+            y: (relativePosition1.x * agent.radius + relativePosition1.y * leg1) / distSq1
+          };
+        } else {
+          leftLegDirection = {
+            x: -obstacle1.direction.x,
+            y: -obstacle1.direction.y
+          };
+        }
+        if (obstacle2.isConvex) {
+          const leg2 = Math.sqrt(Math.max(0, distSq2 - radiusSq));
+          rightLegDirection = {
+            x: (relativePosition2.x * leg2 + relativePosition2.y * agent.radius) / distSq2,
+            y: (-relativePosition2.x * agent.radius + relativePosition2.y * leg2) / distSq2
+          };
+        } else {
+          rightLegDirection = {
+            x: obstacle1.direction.x,
+            y: obstacle1.direction.y
+          };
+        }
+      }
+      const leftNeighbor = obs1.previous;
+      let isLeftLegForeign = false;
+      let isRightLegForeign = false;
+      if (obs1.isConvex) {
+        const negLeftNeighborDir = {
+          x: -leftNeighbor.direction.x,
+          y: -leftNeighbor.direction.y
+        };
+        if (det2(leftLegDirection, negLeftNeighborDir) >= 0) {
+          leftLegDirection = negLeftNeighborDir;
+          isLeftLegForeign = true;
+        }
+      }
+      if (obs2.isConvex) {
+        if (det2(rightLegDirection, obs2.direction) <= 0) {
+          rightLegDirection = {
+            x: obs2.direction.x,
+            y: obs2.direction.y
+          };
+          isRightLegForeign = true;
+        }
+      }
+      const leftCutoff = {
+        x: invTimeHorizonObst * (obs1.point.x - agent.position.x),
+        y: invTimeHorizonObst * (obs1.point.y - agent.position.y)
+      };
+      const rightCutoff = {
+        x: invTimeHorizonObst * (obs2.point.x - agent.position.x),
+        y: invTimeHorizonObst * (obs2.point.y - agent.position.y)
+      };
+      const cutoffVector = {
+        x: rightCutoff.x - leftCutoff.x,
+        y: rightCutoff.y - leftCutoff.y
+      };
+      const sameVertex = obs1 === obs2;
+      const cutoffVectorSq = lengthSq2(cutoffVector);
+      const t = sameVertex ? 0.5 : cutoffVectorSq > EPSILON3 ? dot2({
+        x: agent.velocity.x - leftCutoff.x,
+        y: agent.velocity.y - leftCutoff.y
+      }, cutoffVector) / cutoffVectorSq : 0.5;
+      const tLeft = dot2({
+        x: agent.velocity.x - leftCutoff.x,
+        y: agent.velocity.y - leftCutoff.y
+      }, leftLegDirection);
+      const tRight = dot2({
+        x: agent.velocity.x - rightCutoff.x,
+        y: agent.velocity.y - rightCutoff.y
+      }, rightLegDirection);
+      if (t < 0 && tLeft < 0 || sameVertex && tLeft < 0 && tRight < 0) {
+        const unitW = normalize({
+          x: agent.velocity.x - leftCutoff.x,
+          y: agent.velocity.y - leftCutoff.y
+        });
+        line.direction = {
+          x: unitW.y,
+          y: -unitW.x
+        };
+        line.point = {
+          x: leftCutoff.x + agent.radius * invTimeHorizonObst * unitW.x,
+          y: leftCutoff.y + agent.radius * invTimeHorizonObst * unitW.y
+        };
+        orcaLines.push(line);
+        numObstLines++;
+        continue;
+      }
+      if (t > 1 && tRight < 0) {
+        const unitW = normalize({
+          x: agent.velocity.x - rightCutoff.x,
+          y: agent.velocity.y - rightCutoff.y
+        });
+        line.direction = {
+          x: unitW.y,
+          y: -unitW.x
+        };
+        line.point = {
+          x: rightCutoff.x + agent.radius * invTimeHorizonObst * unitW.x,
+          y: rightCutoff.y + agent.radius * invTimeHorizonObst * unitW.y
+        };
+        orcaLines.push(line);
+        numObstLines++;
+        continue;
+      }
+      const distSqCutoff = t < 0 || t > 1 || sameVertex ? Infinity : lengthSq2({
+        x: agent.velocity.x - (leftCutoff.x + t * cutoffVector.x),
+        y: agent.velocity.y - (leftCutoff.y + t * cutoffVector.y)
+      });
+      const distSqLeft = tLeft < 0 ? Infinity : lengthSq2({
+        x: agent.velocity.x - (leftCutoff.x + tLeft * leftLegDirection.x),
+        y: agent.velocity.y - (leftCutoff.y + tLeft * leftLegDirection.y)
+      });
+      const distSqRight = tRight < 0 ? Infinity : lengthSq2({
+        x: agent.velocity.x - (rightCutoff.x + tRight * rightLegDirection.x),
+        y: agent.velocity.y - (rightCutoff.y + tRight * rightLegDirection.y)
+      });
+      if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
+        line.direction = {
+          x: -obs1.direction.x,
+          y: -obs1.direction.y
+        };
+        line.point = {
+          x: leftCutoff.x + agent.radius * invTimeHorizonObst * -line.direction.y,
+          y: leftCutoff.y + agent.radius * invTimeHorizonObst * line.direction.x
+        };
+        orcaLines.push(line);
+        numObstLines++;
+        continue;
+      }
+      if (distSqLeft <= distSqRight) {
+        if (isLeftLegForeign) {
+          continue;
+        }
+        line.direction = {
+          x: leftLegDirection.x,
+          y: leftLegDirection.y
+        };
+        line.point = {
+          x: leftCutoff.x + agent.radius * invTimeHorizonObst * -line.direction.y,
+          y: leftCutoff.y + agent.radius * invTimeHorizonObst * line.direction.x
+        };
+        orcaLines.push(line);
+        numObstLines++;
+        continue;
+      }
+      if (isRightLegForeign) {
+        continue;
+      }
+      line.direction = {
+        x: -rightLegDirection.x,
+        y: -rightLegDirection.y
+      };
+      line.point = {
+        x: rightCutoff.x + agent.radius * invTimeHorizonObst * -line.direction.y,
+        y: rightCutoff.y + agent.radius * invTimeHorizonObst * line.direction.x
+      };
+      orcaLines.push(line);
+      numObstLines++;
+    }
+    return numObstLines;
+  }
+};
+__name(_ORCASolver, "ORCASolver");
+var ORCASolver = _ORCASolver;
+function createORCASolver(config) {
+  return new ORCASolver(config);
+}
+__name(createORCASolver, "createORCASolver");
+
+// src/avoidance/KDTree.ts
+var _KDTree = class _KDTree {
+  constructor() {
+    __publicField(this, "agents", []);
+    __publicField(this, "agentIndices", []);
+    __publicField(this, "nodes", []);
+    /**
+     * @zh 最大叶节点大小
+     * @en Maximum leaf size
+     */
+    __publicField(this, "maxLeafSize", 10);
+  }
+  /**
+   * @zh 构建 KD-Tree
+   * @en Build KD-Tree
+   */
+  build(agents) {
+    this.agents = agents;
+    this.agentIndices = [];
+    this.nodes = [];
+    if (agents.length === 0) {
+      return;
+    }
+    for (let i = 0; i < agents.length; i++) {
+      this.agentIndices.push(i);
+    }
+    this.buildRecursive(0, agents.length, 0);
+  }
+  /**
+   * @zh 递归构建 KD-Tree
+   * @en Recursively build KD-Tree
+   */
+  buildRecursive(begin, end, depth) {
+    const nodeIndex = this.nodes.length;
+    const node = {
+      agentIndex: -1,
+      splitValue: 0,
+      left: -1,
+      right: -1,
+      begin,
+      end,
+      minX: Infinity,
+      minY: Infinity,
+      maxX: -Infinity,
+      maxY: -Infinity
+    };
+    this.nodes.push(node);
+    for (let i = begin; i < end; i++) {
+      const agent = this.agents[this.agentIndices[i]];
+      node.minX = Math.min(node.minX, agent.position.x);
+      node.minY = Math.min(node.minY, agent.position.y);
+      node.maxX = Math.max(node.maxX, agent.position.x);
+      node.maxY = Math.max(node.maxY, agent.position.y);
+    }
+    const count = end - begin;
+    if (count <= this.maxLeafSize) {
+      return nodeIndex;
+    }
+    const splitDim = depth % 2;
+    if (splitDim === 0) {
+      this.sortByX(begin, end);
+    } else {
+      this.sortByY(begin, end);
+    }
+    const mid = Math.floor((begin + end) / 2);
+    const midAgent = this.agents[this.agentIndices[mid]];
+    node.splitValue = splitDim === 0 ? midAgent.position.x : midAgent.position.y;
+    node.left = this.buildRecursive(begin, mid, depth + 1);
+    node.right = this.buildRecursive(mid, end, depth + 1);
+    return nodeIndex;
+  }
+  /**
+   * @zh 按 X 坐标排序
+   * @en Sort by X coordinate
+   */
+  sortByX(begin, end) {
+    const indices = this.agentIndices;
+    const agents = this.agents;
+    for (let i = begin + 1; i < end; i++) {
+      const key = indices[i];
+      const keyX = agents[key].position.x;
+      let j = i - 1;
+      while (j >= begin && agents[indices[j]].position.x > keyX) {
+        indices[j + 1] = indices[j];
+        j--;
+      }
+      indices[j + 1] = key;
+    }
+  }
+  /**
+   * @zh 按 Y 坐标排序
+   * @en Sort by Y coordinate
+   */
+  sortByY(begin, end) {
+    const indices = this.agentIndices;
+    const agents = this.agents;
+    for (let i = begin + 1; i < end; i++) {
+      const key = indices[i];
+      const keyY = agents[key].position.y;
+      let j = i - 1;
+      while (j >= begin && agents[indices[j]].position.y > keyY) {
+        indices[j + 1] = indices[j];
+        j--;
+      }
+      indices[j + 1] = key;
+    }
+  }
+  /**
+   * @zh 查询邻居
+   * @en Query neighbors
+   */
+  queryNeighbors(position, radius, maxResults, excludeId) {
+    const results = [];
+    const radiusSq = radius * radius;
+    if (this.nodes.length === 0) {
+      return results;
+    }
+    this.queryRecursive(0, position, radiusSq, maxResults, excludeId, results);
+    results.sort((a, b) => a.distanceSq - b.distanceSq);
+    if (results.length > maxResults) {
+      results.length = maxResults;
+    }
+    return results;
+  }
+  /**
+   * @zh 递归查询
+   * @en Recursive query
+   */
+  queryRecursive(nodeIndex, position, radiusSq, maxResults, excludeId, results) {
+    const node = this.nodes[nodeIndex];
+    if (!node) return;
+    const closestX = Math.max(node.minX, Math.min(position.x, node.maxX));
+    const closestY = Math.max(node.minY, Math.min(position.y, node.maxY));
+    const dx = position.x - closestX;
+    const dy = position.y - closestY;
+    const distSqToBBox = dx * dx + dy * dy;
+    if (distSqToBBox > radiusSq) {
+      return;
+    }
+    if (node.left === -1 && node.right === -1) {
+      for (let i = node.begin; i < node.end; i++) {
+        const agentIndex = this.agentIndices[i];
+        const agent = this.agents[agentIndex];
+        if (excludeId !== void 0 && agent.id === excludeId) {
+          continue;
+        }
+        const adx = position.x - agent.position.x;
+        const ady = position.y - agent.position.y;
+        const distSq = adx * adx + ady * ady;
+        if (distSq < radiusSq) {
+          results.push({
+            agent,
+            distanceSq: distSq
+          });
+        }
+      }
+      return;
+    }
+    if (node.left !== -1) {
+      this.queryRecursive(node.left, position, radiusSq, maxResults, excludeId, results);
+    }
+    if (node.right !== -1) {
+      this.queryRecursive(node.right, position, radiusSq, maxResults, excludeId, results);
+    }
+  }
+  /**
+   * @zh 清空索引
+   * @en Clear the index
+   */
+  clear() {
+    this.agents = [];
+    this.agentIndices = [];
+    this.nodes = [];
+  }
+  /**
+   * @zh 获取代理数量
+   * @en Get agent count
+   */
+  get agentCount() {
+    return this.agents.length;
+  }
+};
+__name(_KDTree, "KDTree");
+var KDTree = _KDTree;
+function createKDTree() {
+  return new KDTree();
+}
+__name(createKDTree, "createKDTree");
+
+export {
+  DEFAULT_ORCA_CONFIG,
+  DEFAULT_AGENT_PARAMS,
+  linearProgram2,
+  linearProgram3,
+  solveORCALinearProgram,
+  createObstacleVertices,
+  buildObstacleVertices,
+  ensureCCW,
+  ORCASolver,
+  createORCASolver,
+  KDTree,
+  createKDTree
+};
+//# sourceMappingURL=chunk-JTZP55BJ.js.map