Method funnelPath

packages/pathfinding/src/navmesh/NavMesh.ts:484–554

* @zh 使用漏斗算法优化路径 * @en Optimize path using funnel algorithm

(
        start: IPoint,
        end: IPoint,
        polygons: INavPolygon[]
    )

     * @en Optimize path using funnel algorithm
     */
    private funnelPath(
        start: IPoint,
        end: IPoint,
        polygons: INavPolygon[]
    ): IPoint[] {
        if (polygons.length <= 1) {
            return [start, end];
        }

        // Collect portals
        const portals: IPortal[] = [];

        for (let i = 0; i < polygons.length - 1; i++) {
            const portal = polygons[i].portals.get(polygons[i + 1].id);
            if (portal) {
                portals.push(portal);
            }
        }

        if (portals.length === 0) {
            return [start, end];
        }

        // Simple string pulling algorithm
        const path: IPoint[] = [start];

        let apex = start;
        let leftIndex = 0;
        let rightIndex = 0;
        let left = portals[0].left;
        let right = portals[0].right;

        for (let i = 1; i <= portals.length; i++) {
            const nextLeft = i < portals.length ? portals[i].left : end;
            const nextRight = i < portals.length ? portals[i].right : end;

            // Update right
            if (this.triArea2(apex, right, nextRight) <= 0) {
                if (apex === right || this.triArea2(apex, left, nextRight) > 0) {
                    right = nextRight;
                    rightIndex = i;
                } else {
                    path.push(left);
                    apex = left;
                    leftIndex = rightIndex = leftIndex;
                    left = right = apex;
                    i = leftIndex;
                    continue;
                }
            }

            // Update left
            if (this.triArea2(apex, left, nextLeft) >= 0) {
                if (apex === left || this.triArea2(apex, right, nextLeft) < 0) {
                    left = nextLeft;
                    leftIndex = i;
                } else {
                    path.push(right);