如何构建闭合3D路径上的连续矩阵变换序列？

需求与问题背景 #

我需要生成一组4x4空间变换矩阵，让物体沿闭合3D路径移动时实现平滑变换。核心要求是：

• 物体始终沿路径向前指向
• 最小化绕前向轴的不必要滚转
• 路径首尾的朝向需平滑衔接，初始朝向无特定要求

初始实现及首尾不连续问题 #

我最初通过推导变换坐标系的轴向量来构建变换矩阵xforms：遍历路径上的三元组点窗口，基于行进方向构建矩阵，并通过增量更新up（y轴参考）实现运动平滑，但首尾变换存在明显的不连续问题。

初始实现代码 #

let mut up = Y_AXIS;
let mut xforms = Vec::new();
let mut coords = Paths3::new();

for (&prev, &curr, &next) in lissa_path.iter().circular_tuple_windows() {
    let forward = (next - prev).unit();
    let right = up.cross(forward).unit();
    up = forward.cross(right).unit();

    let mut m = M44::identity();
    m.set_x_axis(right)
        .set_y_axis(up)
        .set_z_axis(forward)
        .set_translate(curr);

    let arrow_len = 0.2;
    coords.push(arrow(curr, curr + right.scale(0.5 * arrow_len)));
    coords.push(arrow(curr, curr + up.scale(arrow_len)));
    // coords.push(arrow(curr, curr + forward.scale(arrow_len)));

    xforms.push(m);
}

问题可视化 #

下图展示了生成的局部坐标系：x轴为较短臂，y轴为较长臂（z轴省略），红色圆圈标注了首尾变换的不连续问题：

Frenet标架尝试：解决首尾不连续但引入扭转 #

我尝试通过近似切线和法向量构建Frenet标架，该方法解决了首尾不连续问题，但引入了大量额外扭转，与《RMF论文》中的问题描述一致。

Frenet标架实现代码 #

fn get_xforms(path: &[V3]) -> Vec<M44> {
    let mut xforms = Vec::new();
    for (&prev, &curr, &next) in path.iter().circular_tuple_windows() {
        let tangent = (next - prev).unit();
        let t1 = (curr - prev).unit();
        let t2 = (next - curr).unit();
        let normal = (t2 - t1).unit();
        let binormal = tangent.cross(normal).unit();

        let mut m = M44::identity();
        m.set_x_axis(normal)
            .set_y_axis(binormal)
            .set_z_axis(tangent)
            .set_translate(curr);

        xforms.push(m);
    }
    xforms
}

Frenet标架可视化 #

下图为生成的Frenet标架坐标系，展示了法向量和副法向量：

我原本计划研究《RMF论文》实现图(f)中的理想效果：

最终解决方案：两次遍历的扭转梯度修正 #

最终我未采用RMF论文方法，而是通过两次遍历实现了满意效果：先用原始方法生成初始变换矩阵，再通过第二次遍历为每个变换应用扭转梯度，弥补首尾坐标系的差异。轴角旋转矩阵的计算参考了相关方法。

最终实现代码 #

fn get_xforms(path: &[V3]) -> Vec<M44> {
    let mut xforms = Vec::new();
    let mut up = Y_AXIS;

    for (&prev, &curr, &next) in path.iter().circular_tuple_windows() {
        let forward = (next - prev).unit();
        let right = up.cross(forward).unit();
        up = forward.cross(right).unit();

        let mut m = M44::identity();
        m.set_x_axis(right)
            .set_y_axis(up)
            .set_z_axis(forward)
            .set_translate(curr);

        xforms.push(m);
    }
    xforms
}

fn twist_xforms(xforms: &[M44]) -> Vec<M44> {
    let first_y = xforms[0].get_y_axis();
    let last_y = xforms.last().unwrap().get_y_axis();
    let angle = last_y.dot(first_y).acos();
    let mut result = Vec::new();

    for (i, xform) in xforms.iter().enumerate() {
        let frac = i as f64 / xforms.len() as f64;

        let right = xform.get_x_axis();
        let up = xform.get_y_axis();
        let forward = xform.get_z_axis();
        let translate = xform.get_translate();

        let mut m = M44::identity();
        m.set_x_axis(right).set_y_axis(up).set_z_axis(forward);
        m *= M44::rotate_axis_angle(forward, frac * angle);
        m.set_translate(translate);
        result.push(m);
    }

    result
}

效果可视化 #

下图展示了改进后的变换效果，首尾衔接平滑且无多余扭转：

内容的提问来源于stack exchange，提问作者strobot