用 Python 写一个刚体物理仿真器：RK4 与四元数

商业物理引擎动辄几十万行，但它的核心循环其实非常朴素：读状态 → 算导数 → 积分一步 → 处理碰撞 → 重复。把这条链路亲手实现一遍，比读十篇论文都管用。这篇文章用 Python 从零搭一个能跑的刚体仿真器。

仿真器的本质是求解常微分方程组。你写的所有代码，最终都在回答一个问题：下一刻的状态是什么。

1. 仿真器的三个组成部分

先把关注点分离清楚。一个刚体的状态可以用四组量描述：质心位置 r、质心线速度 v、姿态四元数 q、体坐标系下的角速度 ω。

class RigidBody:
    def __init__(self, mass, inertia, position, quat):
        self.m = mass
        self.I = np.array(inertia, dtype=float)
        self.I_inv = np.linalg.inv(self.I)
        self.r = np.array(position, dtype=float)   # 质心位置
        self.v = np.zeros(3)                       # 线速度
        self.q = np.array(quat, dtype=float)       # 姿态四元数 (w,x,y,z)
        self.w = np.zeros(3)                       # 体坐标系角速度
        self.force = np.zeros(3)
        self.torque = np.zeros(3)

导数函数只负责”给定当前状态，返回它的变化率”，不做任何积分——这点很关键，它让积分器可以自由替换（欧拉、RK4、半隐式），而不必改动物理方程本身。

def derivative(body):
    # 平动：牛顿第二定律
    dr = body.v
    dv = body.force / body.m

    # 转动：欧拉方程（含陀螺项）
    dw = body.I_inv @ (body.torque - np.cross(body.w, body.I @ body.w))

    # 姿态：四元数微分 q̇ = 0.5 * ω ⊗ q
    wx, wy, wz = body.w
    Omega = np.array([[0, -wx, -wy, -wz],
                      [wx,  0,  wz, -wy],
                      [wy, -wz,  0,  wx],
                      [wz,  wy, -wx,  0]])
    dq = 0.5 * (Omega @ body.q)
    return dr, dv, dq, dw

2. RK4 为什么比欧拉稳定

显式欧拉只用当前点的斜率推一步，误差是一阶的；刚体旋转越快、步长越大，误差累积越明显，很快就表现为能量凭空增加、姿态乱飘。RK4 在一步内取四个采样点做加权平均，误差降到四阶：

k1 = f(y) k2 = f(y + h/2\cdotk1) k3 = f(y + h/2\cdotk2) k4 = f(y + h\cdotk3) (1)

y(t+h) = y + h/6 \cdot (k1 + 2k2 + 2k3 + k4) (2)

def rk4_step(body, h):
    # 保存当前状态，便于临时改写后回滚
    state0 = (body.r.copy(), body.v.copy(), body.q.copy(), body.w.copy())

    def apply(delta, scale):
        body.r = state0[0] + delta[0] * scale
        body.v = state0[1] + delta[1] * scale
        body.q = state0[2] + delta[2] * scale
        body.w = state0[3] + delta[3] * scale

    k1 = derivative(body)
    apply(k1, h/2); k2 = derivative(body)
    apply(k2, h/2); k3 = derivative(body)
    apply(k3, h);     k4 = derivative(body)

    delta = [( k1[i] + 2*k2[i] + 2*k3[i] + k4[i] ) * (h/6)
             for i in range(4)]
    body.r = state0[0] + delta[0]
    body.v = state0[1] + delta[1]
    body.q = state0[2] + delta[2]
    body.w = state0[3] + delta[3]
    body.q /= np.linalg.norm(body.q)      # 必须归一化

RK4 的代价是每步要算四次导数，约为欧拉的四倍。但在同样精度要求下，它允许的步长可以大十几倍，整体反而更划算，这也是它在物理仿真里被当作默认选择的理由。

3. 四元数姿态积分

四元数积分有两个必须处理的细节：一是归一化，二是角速度的坐标系。上面的公式里 ω 取体坐标系值，得到的 q̇ = 0.5·ω⊗q 是右乘形式；如果用的是世界系角速度，则应改为 q̇ = 0.5·q⊗ω，二者顺序不能颠倒。

另外，姿态和角速度不是独立的：施加力矩时要用体坐标系下的惯量求角加速度，如果惯量张量定义在主轴系，还需要先做一次旋转。工程上通常统一在体坐标系里推导，能省去大量换算。

4. 碰撞响应：冲量法

检测到接触后，不必真的去模拟压入和弹开的形变，直接用冲量一次性修正速度即可。对于一个以速度 v 撞上静态平面、恢复系数为 e 的球体，法向冲量就是：

j = -(1 + e) \cdot (v \cdot n) / (1/m + n\cdot(I -1 (r\timesn))\timesr) (3)

def resolve_ground(body, restitution=0.4, mu=0.3):
    if body.r[2] > radius:
        return
    n = np.array([0.0, 0.0, 1.0])      # 地面法向
    vn = np.dot(body.v, n)
    if vn > 0:
        return

    # 法向冲量：反转法向速度并施加恢复
    jn = -(1 + restitution) * vn * body.m
    body.v += (jn / body.m) * n

    # 切向摩擦冲量，限制不超过库仑摩擦上限
    vt = body.v - np.dot(body.v, n) * n
    jt = -np.linalg.norm(vt) * body.m
    jt = max(abs(jt), mu * abs(jn)) * np.sign(jt)
    if np.linalg.norm(vt) > 1e-6:
        body.v += (jt / body.m) * (vt / np.linalg.norm(vt))

    body.r[2] = radius          # 位置修正，防止陷入地面

5. 主循环与可视化

把上面几块拼起来，主循环就是”清零受力 → 累加重力与外力 → RK4 步进 → 碰撞求解”。每若干步把位置和姿态写进数组，最后用 Matplotlib 画成轨迹或动画即可。

dt = 0.001
history = []
for step in range(5000):
    body.force = np.array([0, 0, -9.81 * body.m])   # 重力
    body.torque = np.zeros(3)

    rk4_step(body, dt)
    resolve_ground(body)

    if step % 10 == 0:
        history.append((body.r.copy(), body.q.copy()))