每日算法 — 使用java实现平衡球：加速度受限运动规划与扫掠球体碰撞检测

平衡球（Labyrinth / 木球走迷宫）是最经典的体感/重力控制游戏之一：屏幕上有一颗钢球，玩家通过倾斜整个迷宫盘面来控制它滚动，绕过一个个洞，最终滚进目标洞。iOS 时代它是最早一批靠陀螺仪出圈的爆款，也是机器人学里”欠驱动系统控制”的绝佳教学案例。

它为什么值得写成算法题？因为它和大多数”寻路游戏”有本质区别：你不能直接决定球往哪走，你只能决定加速度。球的位置由加速度积分两次得到，这意味着：

1. 路径不能有直角转弯——球有惯性，必须提前减速；
2. 障碍物不是”格子被占”，而是”球心到墙的距离小于半径”——必须做连续碰撞检测，否则高速穿墙（tunneling）；
3. 洞是可穿过的目标，必须在正确速度下抵达，快了会冲过去。

本文用 Java 实现一个完整的平衡球求解器，核心是加速度受限的轨迹规划（用”速度上限曲面 + 时间最优梯形速度剖面”来保证不打滑过目标）和扫掠球体碰撞检测（用 capsule-vs-AABB 的解析求交，避免离散步进漏检）。这套方法可以直接迁移到 AGV 调度、磁悬浮定位等真实工程问题。

一、问题建模：为什么普通寻路算法在这里失效

1.1 从”格子寻路”到”动力学规划”

A* 在网格图上的代价是”格子数”，但平衡球的执行器输出是加速度 a，状态是 (p, v) 四维（位置 + 速度）。这意味着：

第二行是关键：控制量是加速度，不是速度，所以可达状态集是 |v| ≤ a_max · t 的锥形——这就是经典的双积分器（double integrator）可达集。

1.2 双积分器的可达集

对一维，状态 (p, v)，控制 a ∈ [-A, A]。从 (0, 0) 出发，在时间 T 内可达的速度集合是 |v| ≤ A·T，位置集合是 |p| ≤ A·T²/2（若要求末端速度为 0）。

推论：想让球在距离 D 处停下，最短时间 T = 2√(D/A)（加速一半、减速一半）。这条公式是平衡球规划的核心，因为它给出了”要留多长的刹车距离”的解析答案：

刹车距离 d_brake = v² / (2A)

任何时候，球到下一个必须停下的点（目标洞或墙）的距离若小于 v²/(2A)，就必须立刻全速反向减速——这就是速度上限曲面（velocity limit surface）：

v_max(p) = √(2 · A · dist(p, nearestObstacleOrGoal))

把它画在 (p, v) 平面上就是一条抛物线，可行域在抛物线下方。这条曲面把”动力学约束”变成了”几何约束”，后面的规划就轻松了。

1.3 为什么必须做扫掠碰撞检测

球半径 r，速度 v，时间步 dt。球一帧位移 |v|·dt。若 |v|·dt > 2r，离散检测会漏掉中间的墙——球”穿墙而过”。这在平衡球里是真实存在的问题：满速冲过窄通道时，一帧能移动 20+ 像素，而通道宽度可能只有 12 像素。

三种解决方式：

本文用 CCD 的解析解：球在 [t0, t1] 内做匀速直线运动时，与 AABB 的最早接触时刻有闭式解（求解”移动点与静止盒子的最近距离”）。一次函数调用，O(1)，无迭代。

二、算法设计

2.1 三层结构

① 宏观层：在"配置空间"（球心可行域）上跑 A*，得到一条折线参考路径
② 中层：把折线做"圆角化"（用圆弧替代尖角），使路径处处曲率 ≤ 允许上限
③ 微观层：沿弧长参数化路径做时间最优速度规划（梯形/双 S 型剖面）

为什么必须圆角化：尖角意味着速度方向瞬时反转，需要无穷大加速度，物理上不可实现。圆角半径由 r_min = v²/a_max 决定——速度越高，允许的最小转弯半径越大。这个关系把”路径几何”和”速度规划”耦合在一起，是平衡球最本质的难点。

解耦近似：工程上通常先按巡航速度 v_cruise 算出 r_min，用它做圆角化，再在圆角后的路径上做速度规划。若速度规划发现某段曲率要求的速度低于巡航速度，就把该段的 v_max 下调——这是一次”几何-速度”的迭代，通常两三轮就收敛。

2.2 配置空间构建

球是半径 r 的圆，墙是 AABB 矩形。球心不能进入的区域是”墙向外膨胀 r”后的矩形（Minkowski 和）。所以配置空间障碍 = 每个墙 AABB 的四边各向外扩 r。这一步把”球体 vs 矩形”降维成”点 vs 膨胀矩形”，判定从 O(1) 复杂几何变成矩形包含测试。

2.3 时间最优速度规划

沿弧长 s 从 0 到 L，给定每点的曲率 κ(s) 和速度上限：

v_limit(s) = min( v_global, √(a_max / max(κ(s), ε)) )

其中 √(a_max/κ) 来自向心加速度约束 a_centripetal = v²·κ ≤ a_max。

然后做两次扫描（经典的时间最优剖面算法）：

1. 前向扫描：从 s=0 出发，v(s+ds) = min(v_limit, √(v(s)² + 2·a_max·ds)) —— 保证加速能力不超限；
2. 后向扫描：从 s=L 且 v(L)=0 出发，v(s) = min(v(s), √(v(s+ds)² + 2·a_max·ds)) —— 保证刹车能力足够。

两次扫描的结果就是”在任意点都能停下”的最大可行速度包络。这是所有轨迹规划（包括自动驾驶的 Apollo EM Planner）的公共基石。

三、完整 Java 实现

3.1 项目结构

balance-ball/
├── src/main/java/com/example/ball/
│   ├── Vec2.java          二维向量
│   ├── Aabb.java          AABB 与"点-盒最近距离"
│   ├── World.java         关卡（墙 + 洞 + 目标）+ 配置空间膨胀
│   ├── SweptCcd.java      扫掠球体 CCD（解析解）
│   ├── Planner.java       A* 宏观路径 + 圆角化
│   ├── SpeedProfile.java  时间最优速度剖面
│   └── Main.java          求解演示 + 一致性测试
└── pom.xml

3.2 Vec2.java — 基础向量

package com.example.ball;

/** 不可变二维向量，同时承载位置与速度 */
public final class Vec2 {
    public final double x, y;
    public Vec2(double x, double y) { this.x = x; this.y = y; }

    public Vec2 add(Vec2 o)  { return new Vec2(x + o.x, y + o.y); }
    public Vec2 sub(Vec2 o)  { return new Vec2(x - o.x, y - o.y); }
    public Vec2 mul(double k){ return new Vec2(x * k, y * k); }
    public double dot(Vec2 o){ return x * o.x + y * o.y; }
    public double norm()     { return Math.hypot(x, y); }
    public double norm2()    { return x * x + y * y; }

    public Vec2 normalize() {
        double n = norm();
        return n < 1e-12 ? new Vec2(0, 0) : new Vec2(x / n, y / n);
    }

    /** 逆时针旋转 90 度 */
    public Vec2 perp() { return new Vec2(-y, x); }

    /** 从 a 指向 b 的单位向量 */
    public static Vec2 dir(Vec2 a, Vec2 b) { return b.sub(a).normalize(); }

    @Override public String toString() {
        return String.format("(%.2f, %.2f)", x, y);
    }
}

3.3 Aabb.java — 盒子与最近距离

package com.example.ball;

/**
 * 轴对齐包围盒（Axis-Aligned Bounding Box）。
 * 核心方法 distanceToPoint 是 CCD 与配置空间膨胀的共同基础：
 * 点 p 到盒子的最近距离（若 p 在盒内则返回 0）。
 */
public final class Aabb {

    public final double minX, minY, maxX, maxY;

    public Aabb(double minX, double minY, double maxX, double maxY) {
        this.minX = Math.min(minX, maxX);
        this.minY = Math.min(minY, maxY);
        this.maxX = Math.max(minX, maxX);
        this.maxY = Math.max(minY, maxY);
    }

    /**
     * 点到盒子的最短距离（点在盒内时为 0）。
     * 这是把"球 vs 盒"降维成"点 vs 膨胀盒"的关键：
     * 球心到盒子的距离 < r  ⟺  球与盒相交。
     */
    public double distanceToPoint(Vec2 p) {
        double dx = Math.max(Math.max(minX - p.x, 0), p.x - maxX);
        double dy = Math.max(Math.max(minY - p.y, 0), p.y - maxY);
        return Math.hypot(dx, dy);
    }

    /** 点是否在盒内（含边界） */
    public boolean contains(Vec2 p) {
        return p.x >= minX && p.x <= maxX && p.y >= minY && p.y <= maxY;
    }

    /** 把盒子向外膨胀 d（Minkowski 和，用于配置空间障碍） */
    public Aabb inflate(double d) {
        return new Aabb(minX - d, minY - d, maxX + d, maxY + d);
    }

    /** 盒子的四个角点（圆角化时会用到） */
    public Vec2[] corners() {
        return new Vec2[]{
            new Vec2(minX, minY), new Vec2(maxX, minY),
            new Vec2(maxX, maxY), new Vec2(minX, maxY)
        };
    }

    @Override public String toString() {
        return String.format("AABB[%.1f,%.1f → %.1f,%.1f]", minX, minY, maxX, maxY);
    }
}

3.4 SweptCcd.java — 扫掠球体连续碰撞检测

package com.example.ball;

import java.util.List;

/**
 * 连续碰撞检测（CCD）核心：球从 p0 匀速直线运动到 p1，求它最早接触某个 AABB 的时刻。
 *
 * 数学推导：
 *   设球心轨迹 c(t) = p0 + (p1 - p0)·t，t ∈ [0,1]，半径 r。
 *   与 AABB 相交 ⟺ distanceToPoint(c(t)) ≤ r。
 *   由于 AABB 是凸体，"球心到盒子的距离"作为 t 的函数在区间内有唯一极小值点，
 *   因此相交区间是 [t*, +∞) 的一个连续段。求 t* 只需在 [0,1] 上做三分搜索
 *   或直接用"膨胀盒 + 射线"的解析解。
 *
 * 本文采用解析解（更快更稳）：
 *   把 AABB 膨胀 r → 得到"禁止球心进入"的盒子 B'。
 *   轨迹进入 B' 的时刻用 slab 法（射线-盒子求交）解析求出。
 *   slab 法：对 x 与 y 两个轴分别求进入/离开参数区间，取交集。
 */
public final class SweptCcd {

    /** 一次碰撞结果：碰撞时刻（0~1）、法线方向（用于反弹） */
    public static final class Hit {
        public final double t;        // 首次碰撞的无量纲时刻 ∈ [0,1]
        public final Vec2 normal;     // 指向球心的单位法线（反弹方向）
        public final Aabb box;

        Hit(double t, Vec2 normal, Aabb box) { this.t = t; this.normal = normal; this.box = box; }
        @Override public String toString() {
            return String.format("Hit{t=%.4f, n=%s}", t, normal);
        }
    }

    private SweptCcd() {}

    /**
     * slab 法求线段与膨胀盒的最早进入参数。
     * 返回 [0,1] 内的最小 t；若无相交返回 Double.POSITIVE_INFINITY。
     *
     * slab 法原理（二维版）：
     *   对每个轴 a ∈ {x, y}：
     *     若 d[a] == 0：要求 p0[a] 落在 [min, max] 内，否则整体无交；
     *     否则 t1 = (min - p0[a]) / d[a]，t2 = (max - p0[a]) / d[a]，取小者为 enter、大者为 exit，
     *     与原区间求交：tMin = max(tMin, enter)，tMax = min(tMax, exit)。
     *   最后若 tMin ≤ tMax 且 tMin ≤ 1，则最早进入时刻为 max(tMin, 0)。
     */
    public static double firstEnterParam(Vec2 p0, Vec2 p1, Aabb box, double r) {
        Aabb inf = box.inflate(r);
        double dx = p1.x - p0.x, dy = p1.y - p0.y;

        double tMin = 0.0, tMax = 1.0;

        // ---- x 轴 slab ----
        if (Math.abs(dx) < 1e-12) {
            if (p0.x < inf.minX || p0.x > inf.maxX) return Double.POSITIVE_INFINITY;
        } else {
            double t1 = (inf.minX - p0.x) / dx;
            double t2 = (inf.maxX - p0.x) / dx;
            double enter = Math.min(t1, t2), exit = Math.max(t1, t2);
            tMin = Math.max(tMin, enter);
            tMax = Math.min(tMax, exit);
            if (tMin > tMax) return Double.POSITIVE_INFINITY;
        }

        // ---- y 轴 slab ----
        if (Math.abs(dy) < 1e-12) {
            if (p0.y < inf.minY || p0.y > inf.maxY) return Double.POSITIVE_INFINITY;
        } else {
            double t1 = (inf.minY - p0.y) / dy;
            double t2 = (inf.maxY - p0.y) / dy;
            double enter = Math.min(t1, t2), exit = Math.max(t1, t2);
            tMin = Math.max(tMin, enter);
            tMax = Math.min(tMax, exit);
            if (tMin > tMax) return Double.POSITIVE_INFINITY;
        }

        if (tMin > 1.0 || tMax < 0.0) return Double.POSITIVE_INFINITY;
        return Math.max(tMin, 0.0);
    }

    /**
     * 沿轨迹扫掠，找所有墙中最早的接触。
     * 若 t 已经在盒子内部（球心已在膨胀盒内），firstEnterParam 返回 0——需要特判防卡死。
     */
    public static Hit earliestHit(Vec2 p0, Vec2 p1, List<Aabb> walls, double r) {
        double bestT = Double.POSITIVE_INFINITY;
        Aabb bestBox = null;

        for (Aabb w : walls) {
            double t = firstEnterParam(p0, p1, w, r);
            if (t < bestT) { bestT = t; bestBox = w; }
        }
        if (bestBox == null) return null;

        // 计算接触法线：球心在接触点处，从盒子最近点指向球心的方向
        Vec2 contact = p0.add(p1.sub(p0).mul(bestT));
        Vec2 normal = normalFromBox(bestBox, contact, r);
        return new Hit(bestT, normal, bestBox);
    }

    /**
     * 求接触法线：找到盒子上离球心最近的点 q，法线 = normalize(contact - q)。
     * 若球心正好在盒子内部（退化情形），退化为"从盒子中心指向球心"。
     */
    private static Vec2 normalFromBox(Aabb box, Vec2 contact, double r) {
        double qx = clamp(contact.x, box.minX, box.maxX);
        double qy = clamp(contact.y, box.minY, box.maxY);
        Vec2 q = new Vec2(qx, qy);
        Vec2 n = contact.sub(q);
        if (n.norm() < 1e-9) {
            // 球心在盒内：用盒中心到球心的方向
            Vec2 c = new Vec2((box.minX + box.maxX) * 0.5, (box.minY + box.maxY) * 0.5);
            n = contact.sub(c);
            if (n.norm() < 1e-9) n = new Vec2(1, 0);
        }
        return n.normalize();
    }

    private static double clamp(double v, double lo, double hi) {
        return v < lo ? lo : (v > hi ? hi : v);
    }
}

3.5 World.java — 关卡与配置空间

package com.example.ball;

import java.util.ArrayList;
import java.util.List;

/**
 * 平衡球世界：边界墙、内部障碍、洞（陷阱）、目标洞。
 * 关键概念：**配置空间（C-space）障碍 = 墙膨胀球半径 r**。
 * 球心不得进入膨胀后的盒子，这样"球 vs 墙"就统一成"点 vs 膨胀盒"。
 */
public final class World {

    public final double width, height;    // 世界尺寸
    public final double ballRadius;       // 球半径
    public final double aMax;             // 最大加速度（由盘面倾角上限决定）
    public final List<Aabb> walls;        // 原始墙体
    public final List<Aabb> cspaceWalls;  // 膨胀后的配置空间障碍
    public final List<Vec2> holes;        // 洞（掉下去就算失败）
    public final double holeRadius;
    public final Vec2 target;             // 目标洞位置
    public final Vec2 start;

    public World(double width, double height, double ballRadius, double aMax,
                 List<Aabb> walls, List<Vec2> holes, double holeRadius,
                 Vec2 target, Vec2 start) {
        this.width = width; this.height = height;
        this.ballRadius = ballRadius; this.aMax = aMax;
        this.walls = walls;
        this.holes = holes;
        this.holeRadius = holeRadius;
        this.target = target;
        this.start = start;

        // ① 墙体统一收集成"边界 + 内部障碍"四个矩形
        List<Aabb> all = new ArrayList<>(walls);
        double t = 8.0;   // 边界厚度
        all.add(new Aabb(-t, -t, width + t, 0));          // 上边界
        all.add(new Aabb(-t, height, width + t, height + t)); // 下边界
        all.add(new Aabb(-t, -t, 0, height + t));         // 左边界
        all.add(new Aabb(width, -t, width + t, height + t)); // 右边界
        this.walls.clear();
        this.walls.addAll(all);

        // ② 配置空间膨胀：球心不得进入膨胀盒
        this.cspaceWalls = new ArrayList<>(all.size());
        for (Aabb w : all) this.cspaceWalls.add(w.inflate(ballRadius));
    }

    /** 点（球心）是否处于自由空间 */
    public boolean isFree(Vec2 p) {
        if (p.x < 0 || p.y < 0 || p.x > width || p.y > height) return false;
        for (Aabb w : cspaceWalls) if (w.contains(p)) return false;
        for (Vec2 h : holes)
            if (p.sub(h).norm() < holeRadius + ballRadius) return false;   // 洞也是障碍
        return true;
    }

    /** 球心到最近障碍（墙 + 洞）的距离，用于速度上限曲面 */
    public double clearance(Vec2 p) {
        double best = Math.min(Math.min(p.x, width - p.x), Math.min(p.y, height - p.y));
        for (Aabb w : walls) best = Math.min(best, w.distanceToPoint(p) - ballRadius);
        for (Vec2 h : holes) best = Math.min(best, p.sub(h).norm() - holeRadius - ballRadius);
        return Math.max(best, 0.0);
    }

    /** 速度上限曲面：v_max(p) = √(2·a_max·clearance(p))，保证在任何点都能刹停 */
    public double speedLimit(Vec2 p) {
        return Math.sqrt(2.0 * aMax * clearance(p));
    }
}

3.6 SpeedProfile.java — 时间最优速度剖面

package com.example.ball;

/**
 * 沿路径的时间最优速度规划（两次扫描法）。
 *
 * 输入：离散化的路径点序列（弧长 s 间隔 ds）、每点曲率 κ、全局速度上限。
 * 输出：每点的可行速度 v[i]，以及总耗时。
 *
 * 算法：
 *   ① 每点局部速度上限 v_lim[i] = min(v_global, √(a_max/κ[i]))  ← 向心加速度约束
 *   ② 前向扫描：v[i] = min(v_lim[i], √(v[i-1]² + 2·a_max·ds))
 *   ③ 后向扫描：v[i] = min(v[i],     √(v[i+1]² + 2·a_max·ds))
 *   两次扫描后，任意点都能刹停，且任意点都能加速到该速度。
 */
public final class SpeedProfile {

    public final double[] s;     // 弧长
    public final double[] v;     // 规划速度
    public final double[] kappa; // 曲率
    public final double totalTime;

    private SpeedProfile(double[] s, double[] v, double[] kappa, double totalTime) {
        this.s = s; this.v = v; this.kappa = kappa; this.totalTime = totalTime;
    }

    public static SpeedProfile plan(double[] s, double[] kappa, double aMax,
                                    double vGlobal, double vEnd) {
        int n = s.length;
        double ds = n > 1 ? s[1] - s[0] : 1.0;
        double[] vLim = new double[n];
        double[] v = new double[n];

        // ① 局部速度上限：向心加速度约束 v²κ ≤ aMax ⟹ v ≤ √(aMax/κ)
        for (int i = 0; i < n; i++) {
            double k = Math.abs(kappa[i]);
            double vCentri = (k < 1e-9) ? Double.POSITIVE_INFINITY
                                        : Math.sqrt(aMax / k);
            vLim[i] = Math.min(vGlobal, vCentri);
            v[i] = vLim[i];
        }

        // ② 前向扫描：受"最大加速度"限制（v² = v0² + 2as）
        v[0] = 0.0;                                  // 起点静止
        for (int i = 1; i < n; i++) {
            double reachable = Math.sqrt(v[i - 1] * v[i - 1] + 2.0 * aMax * ds);
            v[i] = Math.min(vLim[i], reachable);
        }

        // ③ 后向扫描：受"最大减速度"限制（保证每点都能在该点刹停到 vEnd）
        v[n - 1] = Math.min(v[n - 1], vEnd);
        for (int i = n - 2; i >= 0; i--) {
            double reachable = Math.sqrt(v[i + 1] * v[i + 1] + 2.0 * aMax * ds);
            v[i] = Math.min(v[i], reachable);
        }

        // ④ 累计时间：每段 dt = 2·ds / (v[i] + v[i+1])（梯形速度积分的逆运算）
        double total = 0;
        for (int i = 0; i < n - 1; i++) {
            double vAvg = 0.5 * (v[i] + v[i + 1]);
            if (vAvg < 1e-6) vAvg = 1e-6;            // 防除零
            total += ds / vAvg;
        }
        return new SpeedProfile(s, v, kappa, total);
    }

    /** 打印剖面摘要，便于观察"梯形/双梯形"形状 */
    public void printSummary(int every) {
        System.out.println("  弧长 s      曲率 κ      速度 v      刹车距离 v²/(2a)");
        for (int i = 0; i < s.length; i += every) {
            System.out.printf("  %8.2f  %9.4f  %9.2f  %12.2f%n",
                    s[i], kappa[i], v[i],
                    v[i] * v[i] / (2.0 * Math.max(1e-9, 1.0)));  // 仅示意
        }
    }

    public double[] velocities() { return v; }
    public double totalTime()    { return totalTime; }
}

3.7 Planner.java — A* 宏观路径 + 圆角化

package com.example.ball;

import java.util.*;

/**
 * 宏观路径规划：在配置空间网格上跑 A*，然后做圆角化。
 *
 * 为什么用 A* 而不是 RRT：平衡球的障碍是矩形（规则几何），
 * 网格 A* 在 8 邻域下得到的折线经过圆角化后质量足够，且最短性有保证。
 * RRT 适合高维/复杂障碍，这里属于杀鸡用牛刀。
 */
public final class Planner {

    private final World world;
    private final double cell;    // 网格边长

    public Planner(World world, double cellSize) {
        this.world = world;
        this.cell = cellSize;
    }

    /** A* 结果：折线路径点序列（含起点与终点） */
    public List<Vec2> planGridPath() {
        int gw = (int) Math.ceil(world.width / cell);
        int gh = (int) Math.ceil(world.height / cell);

        int sx = (int) (world.start.x / cell),  sy = (int) (world.start.y / cell);
        int tx = (int) (world.target.x / cell), ty = (int) (world.target.y / cell);

        double[] gScore = new double[gw * gh];
        Arrays.fill(gScore, Double.POSITIVE_INFINITY);
        int[] prev = new int[gw * gh];
        Arrays.fill(prev, -1);
        boolean[] closed = new boolean[gw * gh];

        PriorityQueue<int[]> open = new PriorityQueue<>(
                Comparator.comparingDouble(a -> gScore[a[0] * gh + a[1]] + a[2]));
        // 队列元素：{gx, gy, 未经修改的 f 用于排序，实际 f 在比较器里算}

        int sIdx = sx * gh + sy;
        gScore[sIdx] = 0;
        open.add(new int[]{sx, sy, 0});

        int[] dx = {1, -1, 0, 0, 1, 1, -1, -1};
        int[] dy = {0, 0, 1, -1, 1, -1, 1, -1};

        while (!open.isEmpty()) {
            int[] cur = open.poll();
            int cx = cur[0], cy = cur[1];
            int cIdx = cx * gh + cy;
            if (closed[cIdx]) continue;
            closed[cIdx] = true;
            if (cx == tx && cy == ty) break;

            for (int d = 0; d < 8; d++) {
                int nx = cx + dx[d], ny = cy + dy[d];
                if (nx < 0 || ny < 0 || nx >= gw || ny >= gh) continue;
                Vec2 wp = new Vec2((nx + 0.5) * cell, (ny + 0.5) * cell);
                if (!world.isFree(wp)) continue;

                double step = (d >= 4) ? cell * Math.sqrt(2) : cell;
                double ng = gScore[cIdx] + step;
                int nIdx = nx * gh + ny;
                if (ng < gScore[nIdx]) {
                    gScore[nIdx] = ng;
                    prev[nIdx] = cIdx;
                    double h = Math.hypot(tx - nx, ty - ny) * cell;
                    open.add(new int[]{nx, ny, (int) ((ng + h) * 1000)});
                }
            }
        }

        // 回溯：从终点沿 prev 走回起点
        if (prev[tx * gh + ty] == -1 && !(sx == tx && sy == ty)) return null;
        List<Vec2> rev = new ArrayList<>();
        int idx = tx * gh + ty;
        while (idx != -1) {
            int x = idx / gh, y = idx % gh;
            rev.add(new Vec2((x + 0.5) * cell, (y + 0.5) * cell));
            idx = prev[idx];
        }
        rev.add(world.start);
        Collections.reverse(rev);
        rev.add(world.target);
        return rev;
    }

    /**
     * 折线圆角化：对每个中间顶点，用一段与两边相切的圆弧替代。
     * 半径按"圆心到两边的垂距"给定，此处直接用巡航速度算出的 r_min。
     *
     * 输出为密集采样点序列（后续速度规划需要稠密弧长参数化）。
     */
    public static List<Vec2> fillet(List<Vec2> poly, double rMin, double sampleStep) {
        if (poly.size() < 3) return poly;
        List<Vec2> out = new ArrayList<>();
        out.add(poly.get(0));

        for (int i = 1; i < poly.size() - 1; i++) {
            Vec2 a = poly.get(i - 1), b = poly.get(i), c = poly.get(i + 1);
            Vec2 u = Vec2.dir(b, a);       // 从 b 指向 a
            Vec2 w = Vec2.dir(b, c);       // 从 b 指向 c
            double cosTheta = u.dot(w);
            if (cosTheta < -0.9999) {      // 反向（尖角 180 度）——无法圆角，原样保留
                out.add(b);
                continue;
            }
            double theta = Math.acos(Math.max(-1.0, Math.min(1.0, cosTheta)));
            if (theta > Math.PI - 1e-3) {  // 两个方向几乎相同，无需圆角
                out.add(b);
                continue;
            }
            // 切点到顶点的距离：t = r / tan(theta/2)
            double half = theta * 0.5;
            double tanHalf = Math.tan(half);
            if (tanHalf < 1e-9) { out.add(b); continue; }
            double t = rMin / tanHalf;
            double lenA = a.sub(b).norm(), lenC = c.sub(b).norm();
            // 切点不能超过相邻边长度的一半，否则半径要缩小
            double maxT = Math.min(lenA, lenC) * 0.5;
            if (t > maxT) { t = maxT; }
            double r = t * tanHalf;

            Vec2 p1 = b.add(u.mul(t));     // 入切点
            Vec2 p2 = b.add(w.mul(t));     // 出切点

            // 圆心：沿角平分线方向，距离 r/sin(half)
            Vec2 bis = u.add(w).normalize();
            double distC = r / Math.sin(half);
            Vec2 center = b.add(bis.mul(distC));

            // 沿圆弧从 p1 到 p2 采样
            double a0 = Math.atan2(p1.y - center.y, p1.x - center.x);
            double a1 = Math.atan2(p2.y - center.y, p2.x - center.x);
            double dA = a1 - a0;
            while (dA > Math.PI)  dA -= 2 * Math.PI;
            while (dA < -Math.PI) dA += 2 * Math.PI;
            double arcLen = Math.abs(dA) * r;
            int steps = Math.max(2, (int) Math.ceil(arcLen / sampleStep));
            for (int k = 0; k <= steps; k++) {
                double ang = a0 + dA * (k / (double) steps);
                out.add(new Vec2(center.x + r * Math.cos(ang),
                                 center.y + r * Math.sin(ang)));
            }
        }
        out.add(poly.get(poly.size() - 1));
        return out;
    }

    /** 计算离散点序列的弧长与曲率（三点外接圆曲率） */
    public static double[][] arcLengthAndCurvature(List<Vec2> pts) {
        int n = pts.size();
        double[] s = new double[n], k = new double[n];
        for (int i = 1; i < n; i++) s[i] = s[i - 1] + pts.get(i).sub(pts.get(i - 1)).norm();

        for (int i = 1; i < n - 1; i++) {
            Vec2 p0 = pts.get(i - 1), p1 = pts.get(i), p2 = pts.get(i + 1);
            double a = p1.sub(p0).norm(), b = p2.sub(p1).norm(), c = p2.sub(p0).norm();
            double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
            // 外接圆半径 R = abc / (4·S)，曲率 = 1/R = 4S/(abc)
            k[i] = (Math.abs(2 * cross) / (a * b * c + 1e-12));
        }
        k[0] = k.length > 1 ? k[1] : 0;
        k[n - 1] = k.length > 1 ? k[n - 2] : 0;
        return new double[][]{s, k};
    }
}

3.8 Main.java — 完整求解演示

package com.example.ball;

import java.util.*;

public final class Main {

    public static void main(String[] args) {
        // ===== 关卡定义：一个带窄通道与两个陷阱洞的 40×30 迷宫 =====
        List<Aabb> walls = new ArrayList<>();
        walls.add(new Aabb(0, 8, 26, 12));        // 横向隔墙（下端留缺口）
        walls.add(new Aabb(26, 0, 30, 18));       // 竖向隔墙
        walls.add(new Aabb(10, 18, 14, 30));      // 下方立柱

        Vec2 start  = new Vec2(3.0, 20.0);
        Vec2 target = new Vec2(35.0, 25.0);
        List<Vec2> holes = List.of(new Vec2(18.0, 22.0), new Vec2(22.0, 6.0));

        double ballR = 1.5, aMax = 20.0;
        World world = new World(40.0, 30.0, ballR, aMax, walls, holes, 1.2, target, start);

        System.out.println("世界：" + world.width + "×" + world.height
                + "，球半径 " + world.ballRadius + "，最大加速度 " + world.aMax);
        System.out.println("起点 " + start + "  →  目标 " + target);
        System.out.println("陷阱洞：" + holes);

        // ===== ① 宏观 A* 路径 =====
        Planner planner = new Planner(world, 0.5);
        long t0 = System.nanoTime();
        List<Vec2> poly = planner.planGridPath();
        long t1 = System.nanoTime();
        if (poly == null) { System.out.println("宏观无解：目标不可达。"); return; }
        System.out.printf("%n① A* 宏观路径：%d 个折线点（%.1f ms）%n",
                poly.size(), (t1 - t0) / 1e6);

        // ===== ② 圆角化：以巡航速度算最小转弯半径 =====
        double vCruise = 8.0;                        // 巡航速度 8 m/s
        double rMin = vCruise * vCruise / aMax;      // r_min = v²/a_max = 3.2
        List<Vec2> smooth = Planner.fillet(poly, rMin, 0.4);
        System.out.printf("② 圆角化后：%d 个采样点，最小转弯半径 %.2f%n", smooth.size(), rMin);

        // ===== ③ 速度剖面规划 =====
        double[][] sk = Planner.arcLengthAndCurvature(smooth);
        double[] s = sk[0], kappa = sk[1];
        double totalLen = s[s.length - 1];
        SpeedProfile prof = SpeedProfile.plan(s, kappa, aMax, vCruise, 2.0);
        System.out.printf("③ 路径总长 %.1f，规划总耗时 %.2f s%n", totalLen, prof.totalTime());

        // 输出前 20 个采样点摘要
        prof.printSummary(Math.max(1, s.length / 20));

        // ===== ④ 扫掠 CCD 校验：整条路径不得穿墙 =====
        System.out.println("\n④ 连续碰撞检测（沿平滑路径逐段扫掠）...");
        int hits = 0;
        for (int i = 0; i + 1 < smooth.size(); i++) {
            SweptCcd.Hit h = SweptCcd.earliestHit(
                    smooth.get(i), smooth.get(i + 1), world.walls, ballR);
            if (h != null && h.t < 1.0) {
                hits++;
                if (hits <= 5)
                    System.out.printf("   段 %d %s→%s 碰撞：%s%n",
                            i, smooth.get(i), smooth.get(i + 1), h);
            }
        }
        System.out.printf("   碰撞段数：%d / %d%n",
                hits, smooth.size() - 1);

        // ===== ⑤ 速度上限曲面采样演示 =====
        System.out.println("\n⑤ 速度上限曲面 v_max(p) = √(2·a_max·clearance(p)) 采样：");
        double[][] probes = {
            {3.0, 20.0}, {13.0, 13.0}, {28.0, 15.0}, {35.0, 25.0}, {18.0, 22.0}
        };
        for (double[] pr : probes) {
            Vec2 p = new Vec2(pr[0], pr[1]);
            double clear = world.clearance(p);
            double vmax = world.speedLimit(p);
            System.out.printf("   p=(%5.1f,%5.1f)  clearance=%6.2f  v_max=%7.2f%s%n",
                    p.x, p.y, clear, vmax,
                    world.isFree(p) ? "" : "   ← 位于障碍内！");
        }

        // ===== ⑥ CCD 的"防穿墙"对照实验 =====
        System.out.println("\n⑥ 对照实验：一帧内高速穿过薄墙");
        Aabb thin = new Aabb(10, 10, 11, 20);       // 1 单位厚的薄墙
        Vec2 a = new Vec2(5.0, 15.0);
        Vec2 b = new Vec2(16.0, 15.0);              // 一帧跨越 11 单位
        SweptCcd.Hit ccdHit = SweptCcd.earliestHit(a, b, List.of(thin), ballR);
        System.out.println("   起点 " + a + " → 终点 " + b + "（中点 " + 
                a.add(b).mul(0.5) + " 在墙的另一侧）");
        System.out.println("   离散检测（只看端点）：" 
                + (thin.distanceToPoint(a) < ballR || thin.distanceToPoint(b) < ballR
                   ? "会漏检" : "漏检！两端都在墙外"));
        System.out.println("   CCD 结果：" + ccdHit + "  ← 成功捕获穿墙");
        System.out.printf("   实际接触点在 t=%.3f，即 %s 处%n",
                ccdHit.t, a.add(b.sub(a).mul(ccdHit.t)));
    }
}

3.9 pom.xml

<?xml version="1.0" encoding="UTF-8"?>
<project xmlns="http://maven.apache.org/POM/4.0.0">
  <modelVersion>4.0.0</modelVersion>
  <groupId>com.example</groupId>
  <artifactId>balance-ball</artifactId>
  <version>1.0.0</version>
  <properties>
    <maven.compiler.source>17</maven.compiler.source>
    <maven.compiler.target>17</maven.compiler.target>
  </properties>
  <build>
    <plugins>
      <plugin>
        <groupId>org.apache.maven.plugins</groupId>
        <artifactId>maven-jar-plugin</artifactId>
        <configuration>
          <archive><manifest><mainClass>com.example.ball.Main</mainClass></manifest></archive>
        </configuration>
      </plugin>
    </plugins>
  </build>
</project>

构建与运行：

mvn -q clean package
java -jar target/balance-ball-1.0.0.jar

四、复杂度与关键数值

几个必须记住的解析公式：

• 最短时间（距离 D 内从静止到静止）：T = 2√(D/a_max)
• 刹车距离：d = v²/(2·a_max)
• 最小转弯半径：r_min = v²/a_max（向心加速度约束）
• 速度上限曲面：v_max(p) = √(2·a_max·clearance(p))

以 a_max = 20、v = 8 为例：刹车距离 3.2、最小转弯半径 3.2。这两个数字就是平衡球”手感”的物理来源——它们决定了玩家必须提前多远开始减速、弯道最多能跑多快。

五、常见错误与边界情形

5.1 忘了配置空间膨胀

直接用原始墙做 A，得到的路径会让球”擦着墙走”，物理执行时球体前半部分嵌入墙里。膨胀半径必须正好是球半径*，多一分路径过保守，少一分穿模。

5.2 圆角半径没有受”相邻边长度”限制

t = r / tan(θ/2) 是切点到顶点的距离。若 t 超过相邻边长的一半，两个相邻顶点的圆角会互相吃掉，产生自交路径。本文在 Planner.fillet 里用 maxT = min(lenA, lenC) * 0.5 做了钳制——这是必须的。

5.3 速度剖面只做一次前向扫描

只做前向扫描等于”只考虑加速能力”，不考虑”能否刹停”。典型症状：规划出的速度在路径末端仍然很高，球冲过目标洞。必须再加一遍后向扫描。

5.4 CCD 的退化情形

当球心恰好落在膨胀盒内部时，firstEnterParam 会返回 0（因为起点已在盒内），导致”每段都碰撞”，模拟直接卡死。工程上必须加一个”已在穿透状态时优先推出”的复苏逻辑（本文在 normalFromBox 里给了法线，反弹时可用）。

5.5 洞不是障碍而是”必须精确穿过的目标”

目标洞与陷阱洞在运动学上完全相同（都是圆形凹坑），差别只在语义：碰到陷阱洞 = 失败，碰到目标洞 = 成功。所以正确做法是把所有洞都作为障碍处理，而在”到达目标洞附近且速度足够低”时切换为成功判定——速度必须足够低，否则球会从洞上滚过去。

六、小结

平衡球的算法结构揭示了”欠驱动系统”的通用解题范式：

1. 把动力学约束转成几何约束——速度上限曲面 v_max(p) = √(2·a·clearance) 是整套方案的核心枢纽；
2. 宏观路径与微观速度解耦——A* 求几何、剖面求时间，两者通过”最小转弯半径 r_min = v²/a“这条公式耦合；
3. 连续碰撞检测不可省——扫掠球体的 slab 解析解把”每帧多次子步进”的成本压成一次 O(1) 计算，这是所有高速物理游戏的必备件。

这三条不仅适用于平衡球。把它们套到 AGV 仓储机器人、无人车局部规划、甚至机械臂末端轨迹生成上，你会发现骨架几乎一模一样——几何给方向，动力学给速度，碰撞检测保安全。