@esengine/pathfinding 13.2.0 → 13.3.1

package/dist/{chunk-JTZP55BJ.js → chunk-3VEX32JO.js}

@@ -8,7 +8,8 @@ var DEFAULT_ORCA_CONFIG = {
   defaultTimeHorizon: 2,
   defaultTimeHorizonObst: 1,
   timeStep: 1 / 60,
-  epsilon: 1e-5
+  epsilon: 1e-5,
+  yAxisDown: false
 };
 var DEFAULT_AGENT_PARAMS = {
   radius: 0.5,
@@ -175,6 +176,9 @@ function linearProgram3(lines, numObstLines, beginLine, radius, result) {
       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
@@ -183,13 +187,38 @@ function linearProgram3(lines, numObstLines, beginLine, radius, result) {
   }
 }
 __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);
-  if (lineFail < lines.length) {
+  let feasible = lineFail >= lines.length;
+  let violatedConstraints = 0;
+  if (!feasible) {
     linearProgram3(lines, numObstLines, lineFail, maxSpeed, result);
+    violatedConstraints = lines.length - lineFail;
   }
-  return result;
+  if (numObstLines > 0 && !verifyObstacleConstraints(lines, numObstLines, result)) {
+    feasible = false;
+    violatedConstraints = Math.max(violatedConstraints, 1);
+  }
+  return {
+    velocity: result,
+    feasible,
+    violatedConstraints
+  };
 }
 __name(solveORCALinearProgram, "solveORCALinearProgram");
 
@@ -251,11 +280,15 @@ function createObstacleVertices(vertices, startId = 0) {
   return obstacleVertices;
 }
 __name(createObstacleVertices, "createObstacleVertices");
-function buildObstacleVertices(obstacles) {
+function buildObstacleVertices(obstacles, options = {}) {
+  const { yAxisDown = false } = options;
   const allVertices = [];
   let nextId = 0;
   for (const obstacle of obstacles) {
-    const vertices = createObstacleVertices(obstacle.vertices, nextId);
+    const ccwVertices = ensureCCW([
+      ...obstacle.vertices
+    ], yAxisDown);
+    const vertices = createObstacleVertices(ccwVertices, nextId);
     allVertices.push(...vertices);
     nextId += vertices.length;
   }
@@ -320,11 +353,31 @@ var _ORCASolver = class _ORCASolver {
    * @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);
+    const obstacleVertices = buildObstacleVertices(obstacles, {
+      yAxisDown: this.config.yAxisDown
+    });
     const numObstLines = this.createObstacleORCALines(agent, obstacleVertices, orcaLines);
     this.createAgentORCALines(agent, neighbors, deltaTime, orcaLines);
-    return solveORCALinearProgram(orcaLines, numObstLines, agent.maxSpeed, agent.preferredVelocity);
+    const result = solveORCALinearProgram(orcaLines, numObstLines, agent.maxSpeed, agent.preferredVelocity);
+    return {
+      ...result,
+      numLines: orcaLines.length
+    };
   }
   /**
    * @zh 创建代理间的 ORCA 约束线
@@ -918,6 +971,325 @@ function createKDTree() {
 }
 __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,
@@ -930,6 +1302,10 @@ export {
   ORCASolver,
   createORCASolver,
   KDTree,
-  createKDTree
+  createKDTree,
+  EMPTY_COLLISION,
+  DEFAULT_COLLISION_CONFIG,
+  CollisionResolver,
+  createCollisionResolver
 };
-//# sourceMappingURL=chunk-JTZP55BJ.js.map
+//# sourceMappingURL=chunk-3VEX32JO.js.map