@esengine/pathfinding/dist/chunk-3VEX32JO.js

寻路系统 | Pathfinding System - A*, Grid, NavMesh

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,
  yAxisDown: false
};
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);
      }
      if (!verifyObstacleConstraints(lines, numObstLines, result)) {
        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 verifyObstacleConstraints(lines, numObstLines, velocity) {
  for (let i = 0; i < numObstLines; i++) {
    const line = lines[i];
    const detVal = det(line.direction, {
      x: line.point.x - velocity.x,
      y: line.point.y - velocity.y
    });
    if (detVal > EPSILON) {
      return false;
    }
  }
  return true;
}
__name(verifyObstacleConstraints, "verifyObstacleConstraints");
function solveORCALinearProgram(lines, numObstLines, maxSpeed, preferredVelocity) {
  const result = new Vector2();
  const lineFail = linearProgram2(lines, maxSpeed, preferredVelocity, false, result);
  let feasible = lineFail >= lines.length;
  let violatedConstraints = 0;
  if (!feasible) {
    linearProgram3(lines, numObstLines, lineFail, maxSpeed, result);
    violatedConstraints = lines.length - lineFail;
  }
  if (numObstLines > 0 && !verifyObstacleConstraints(lines, numObstLines, result)) {
    feasible = false;
    violatedConstraints = Math.max(violatedConstraints, 1);
  }
  return {
    velocity: result,
    feasible,
    violatedConstraints
  };
}
__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, options = {}) {
  const { yAxisDown = false } = options;
  const allVertices = [];
  let nextId = 0;
  for (const obstacle of obstacles) {
    const ccwVertices = ensureCCW([
      ...obstacle.vertices
    ], yAxisDown);
    const vertices = createObstacleVertices(ccwVertices, 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 result = this.computeNewVelocityWithResult(agent, neighbors, obstacles, deltaTime);
    return result.velocity;
  }
  /**
   * @zh 计算代理的新速度（带完整结果）
   * @en Compute new velocity for agent (with full result)
   *
   * @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 Full solve result
   */
  computeNewVelocityWithResult(agent, neighbors, obstacles, deltaTime) {
    const orcaLines = [];
    const obstacleVertices = buildObstacleVertices(obstacles, {
      yAxisDown: this.config.yAxisDown
    });
    const numObstLines = this.createObstacleORCALines(agent, obstacleVertices, orcaLines);
    this.createAgentORCALines(agent, neighbors, deltaTime, orcaLines);
    const result = solveORCALinearProgram(orcaLines, numObstLines, agent.maxSpeed, agent.preferredVelocity);
    return {
      ...result,
      numLines: orcaLines.length
    };
  }
  /**
   * @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");

// src/avoidance/CollisionResolver.ts
var EPSILON4 = 1e-5;
var EMPTY_COLLISION = {
  collided: false,
  penetration: 0,
  normal: {
    x: 0,
    y: 0
  },
  closestPoint: {
    x: 0,
    y: 0
  }
};
function closestPointOnSegment(point, segStart, segEnd) {
  const dx = segEnd.x - segStart.x;
  const dy = segEnd.y - segStart.y;
  const lengthSq3 = dx * dx + dy * dy;
  if (lengthSq3 < EPSILON4) {
    return {
      x: segStart.x,
      y: segStart.y
    };
  }
  const t = Math.max(0, Math.min(1, ((point.x - segStart.x) * dx + (point.y - segStart.y) * dy) / lengthSq3));
  return {
    x: segStart.x + t * dx,
    y: segStart.y + t * dy
  };
}
__name(closestPointOnSegment, "closestPointOnSegment");
function isPointInPolygon(point, vertices) {
  let inside = false;
  const n = vertices.length;
  for (let i = 0, j = n - 1; i < n; j = i++) {
    const xi = vertices[i].x;
    const yi = vertices[i].y;
    const xj = vertices[j].x;
    const yj = vertices[j].y;
    if (yi > point.y !== yj > point.y && point.x < (xj - xi) * (point.y - yi) / (yj - yi) + xi) {
      inside = !inside;
    }
  }
  return inside;
}
__name(isPointInPolygon, "isPointInPolygon");
function closestPointOnPolygon(point, vertices) {
  let minDistSq = Infinity;
  let closestPt = {
    x: 0,
    y: 0
  };
  let closestEdge = 0;
  for (let i = 0; i < vertices.length; i++) {
    const j = (i + 1) % vertices.length;
    const closest = closestPointOnSegment(point, vertices[i], vertices[j]);
    const dx = point.x - closest.x;
    const dy = point.y - closest.y;
    const distSq = dx * dx + dy * dy;
    if (distSq < minDistSq) {
      minDistSq = distSq;
      closestPt = closest;
      closestEdge = i;
    }
  }
  return {
    point: closestPt,
    distanceSq: minDistSq,
    edgeIndex: closestEdge
  };
}
__name(closestPointOnPolygon, "closestPointOnPolygon");
var DEFAULT_COLLISION_CONFIG = {
  responseFactor: 1,
  safetyMargin: 0.01
};
var _CollisionResolver = class _CollisionResolver {
  constructor(config = {}) {
    __publicField(this, "config");
    this.config = {
      ...DEFAULT_COLLISION_CONFIG,
      ...config
    };
  }
  /**
   * @zh 检测圆与单个障碍物的碰撞
   * @en Detect collision between circle and single obstacle
   *
   * @param position - @zh 圆心位置 @en Circle center position
   * @param radius - @zh 圆半径 @en Circle radius
   * @param obstacle - @zh 障碍物 @en Obstacle
   * @returns @zh 碰撞结果 @en Collision result
   */
  detectCollision(position, radius, obstacle) {
    const vertices = obstacle.vertices;
    if (vertices.length < 3) {
      return EMPTY_COLLISION;
    }
    const isInside = isPointInPolygon(position, vertices);
    const closest = closestPointOnPolygon(position, vertices);
    const distance = Math.sqrt(closest.distanceSq);
    let penetration;
    let normalX;
    let normalY;
    if (isInside) {
      penetration = radius + distance;
      const dx = closest.point.x - position.x;
      const dy = closest.point.y - position.y;
      const len2 = Math.sqrt(dx * dx + dy * dy);
      if (len2 > EPSILON4) {
        normalX = dx / len2;
        normalY = dy / len2;
      } else {
        normalX = 1;
        normalY = 0;
      }
    } else if (distance < radius) {
      penetration = radius - distance;
      const dx = position.x - closest.point.x;
      const dy = position.y - closest.point.y;
      const len2 = Math.sqrt(dx * dx + dy * dy);
      if (len2 > EPSILON4) {
        normalX = dx / len2;
        normalY = dy / len2;
      } else {
        normalX = 1;
        normalY = 0;
      }
    } else {
      return EMPTY_COLLISION;
    }
    return {
      collided: true,
      penetration,
      normal: {
        x: normalX,
        y: normalY
      },
      closestPoint: closest.point
    };
  }
  /**
   * @zh 检测圆与所有障碍物的碰撞
   * @en Detect collision between circle and all obstacles
   *
   * @param position - @zh 圆心位置 @en Circle center position
   * @param radius - @zh 圆半径 @en Circle radius
   * @param obstacles - @zh 障碍物列表 @en List of obstacles
   * @returns @zh 最严重的碰撞结果 @en Most severe collision result
   */
  detectCollisions(position, radius, obstacles) {
    let worstCollision = EMPTY_COLLISION;
    let maxPenetration = 0;
    for (const obstacle of obstacles) {
      const collision = this.detectCollision(position, radius, obstacle);
      if (collision.collided && collision.penetration > maxPenetration) {
        maxPenetration = collision.penetration;
        worstCollision = collision;
      }
    }
    return worstCollision;
  }
  /**
   * @zh 解决碰撞，返回修正后的位置
   * @en Resolve collision, return corrected position
   *
   * @param position - @zh 当前位置 @en Current position
   * @param radius - @zh 半径 @en Radius
   * @param obstacles - @zh 障碍物列表 @en List of obstacles
   * @returns @zh 修正后的位置 @en Corrected position
   */
  resolveCollision(position, radius, obstacles) {
    const result = {
      x: position.x,
      y: position.y
    };
    const maxIterations = 4;
    for (let iter = 0; iter < maxIterations; iter++) {
      const collision = this.detectCollisions(result, radius, obstacles);
      if (!collision.collided) {
        break;
      }
      const pushDistance = (collision.penetration + this.config.safetyMargin) * this.config.responseFactor;
      result.x += collision.normal.x * pushDistance;
      result.y += collision.normal.y * pushDistance;
    }
    return result;
  }
  /**
   * @zh 验证速度是否会导致碰撞，返回安全速度
   * @en Validate velocity won't cause collision, return safe velocity
   *
   * @param position - @zh 当前位置 @en Current position
   * @param velocity - @zh 目标速度 @en Target velocity
   * @param radius - @zh 半径 @en Radius
   * @param obstacles - @zh 障碍物列表 @en List of obstacles
   * @param deltaTime - @zh 时间步长 @en Time step
   * @returns @zh 安全速度 @en Safe velocity
   */
  validateVelocity(position, velocity, radius, obstacles, deltaTime) {
    const speed = Math.sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
    if (speed < EPSILON4) {
      return velocity;
    }
    const newPos = {
      x: position.x + velocity.x * deltaTime,
      y: position.y + velocity.y * deltaTime
    };
    const collision = this.detectCollisions(newPos, radius, obstacles);
    if (!collision.collided) {
      return velocity;
    }
    const dotProduct = velocity.x * collision.normal.x + velocity.y * collision.normal.y;
    if (dotProduct >= 0) {
      return velocity;
    }
    const slideVelocity = {
      x: velocity.x - dotProduct * collision.normal.x,
      y: velocity.y - dotProduct * collision.normal.y
    };
    const slideSpeed = Math.sqrt(slideVelocity.x * slideVelocity.x + slideVelocity.y * slideVelocity.y);
    if (slideSpeed < speed * 0.1) {
      const perpDir1 = {
        x: -collision.normal.y,
        y: collision.normal.x
      };
      const perpDir2 = {
        x: collision.normal.y,
        y: -collision.normal.x
      };
      const dot1 = velocity.x * perpDir1.x + velocity.y * perpDir1.y;
      const dot22 = velocity.x * perpDir2.x + velocity.y * perpDir2.y;
      const chosenDir = dot1 >= dot22 ? perpDir1 : perpDir2;
      return {
        x: chosenDir.x * speed,
        y: chosenDir.y * speed
      };
    }
    return slideVelocity;
  }
  /**
   * @zh 检测两个代理之间的碰撞
   * @en Detect collision between two agents
   *
   * @param posA - @zh 代理 A 位置 @en Agent A position
   * @param radiusA - @zh 代理 A 半径 @en Agent A radius
   * @param posB - @zh 代理 B 位置 @en Agent B position
   * @param radiusB - @zh 代理 B 半径 @en Agent B radius
   * @returns @zh 碰撞结果 @en Collision result
   */
  detectAgentCollision(posA, radiusA, posB, radiusB) {
    const dx = posB.x - posA.x;
    const dy = posB.y - posA.y;
    const distSq = dx * dx + dy * dy;
    const combinedRadius = radiusA + radiusB;
    if (distSq >= combinedRadius * combinedRadius) {
      return EMPTY_COLLISION;
    }
    const distance = Math.sqrt(distSq);
    const penetration = combinedRadius - distance;
    let normalX, normalY;
    if (distance > EPSILON4) {
      normalX = -dx / distance;
      normalY = -dy / distance;
    } else {
      normalX = 1;
      normalY = 0;
    }
    return {
      collided: true,
      penetration,
      normal: {
        x: normalX,
        y: normalY
      },
      closestPoint: {
        x: posA.x + normalX * radiusA,
        y: posA.y + normalY * radiusA
      }
    };
  }
  /**
   * @zh 解决代理之间的碰撞
   * @en Resolve collision between agents
   *
   * @param posA - @zh 代理 A 位置 @en Agent A position
   * @param radiusA - @zh 代理 A 半径 @en Agent A radius
   * @param posB - @zh 代理 B 位置 @en Agent B position
   * @param radiusB - @zh 代理 B 半径 @en Agent B radius
   * @returns @zh 修正后的位置 [A, B] @en Corrected positions [A, B]
   */
  resolveAgentCollision(posA, radiusA, posB, radiusB) {
    const collision = this.detectAgentCollision(posA, radiusA, posB, radiusB);
    if (!collision.collided) {
      return [
        posA,
        posB
      ];
    }
    const halfPush = (collision.penetration + this.config.safetyMargin) * 0.5 * this.config.responseFactor;
    return [
      {
        x: posA.x + collision.normal.x * halfPush,
        y: posA.y + collision.normal.y * halfPush
      },
      {
        x: posB.x - collision.normal.x * halfPush,
        y: posB.y - collision.normal.y * halfPush
      }
    ];
  }
};
__name(_CollisionResolver, "CollisionResolver");
var CollisionResolver = _CollisionResolver;
function createCollisionResolver(config) {
  return new CollisionResolver(config);
}
__name(createCollisionResolver, "createCollisionResolver");

export {
  DEFAULT_ORCA_CONFIG,
  DEFAULT_AGENT_PARAMS,
  linearProgram2,
  linearProgram3,
  solveORCALinearProgram,
  createObstacleVertices,
  buildObstacleVertices,
  ensureCCW,
  ORCASolver,
  createORCASolver,
  KDTree,
  createKDTree,
  EMPTY_COLLISION,
  DEFAULT_COLLISION_CONFIG,
  CollisionResolver,
  createCollisionResolver
};
//# sourceMappingURL=chunk-3VEX32JO.js.map