Rodrigues任意轴旋转实现错误排查：360度旋转未归位

我需要将候选向量v(vx, vy, vz)绕始于向量s(sx,sy,sz)、止于向量e(ex, ey, ez)的任意轴旋转theta度，坐标系原点为o(ox, oy, oz)。我用以下代码实现Rodrigues旋转，但结果不正确：

实现代码 #

public class RotoTranslation
{
    private readonly Vec3 origin;
    private readonly double cosTheta;
    private readonly double sinTheta;
    private readonly Vec3 axis;
    private readonly Matrix3x3 rotationMatrix;
    private readonly Matrix3x3 translationMatrix;
    private readonly Matrix3x3 invTranslationMatrix;
    public RotoTranslation(
                Vec3 origin, 
                Vec3 start, 
                Vec3 end, 
                double angle_rad)
    {
        this.origin = origin;
        this.axis = Vec3.Normalize(end - start);
        this.cosTheta = Math.Cos(angle_rad);
        this.sinTheta = Math.Sin(angle_rad);

        Matrix3x3 uOuter = 
            Vec3.OuterProduct_mat(axis, axis);
        Matrix3x3 uCross = 
            new Matrix3x3(0.0f, -axis.z, axis.y,                   
                        axis.z, 0.0f, -axis.x,                
                        -axis.y, axis.x, 0.0f);

        rotationMatrix = 
             cosTheta * Matrix3x3.Identity()
             + (1.0f - cosTheta) * uOuter
             + sinTheta * uCross;

        translationMatrix = 
             Matrix3x3.CreateTranslation(-origin);
        rotationMatrix = 
             rotationMatrix * translationMatrix;
        invTranslationMatrix = 
            Matrix3x3.CreateTranslation(origin);
    }
    public Vec3 RotateVector(Vec3 vector)
    {
        Vec3 transformedVector = 
            Vec3.TransformNormal(
                vector - origin, 
                rotationMatrix);
        return Vec3.Transform(
                transformedVector, 
                invTranslationMatrix);
    }
}

测试代码 #

[TestMethod]
public void TestMethod1()
{
    Vec3 origin = new Vec3(1, 1, 1);
    Vec3 start = new Vec3(1, 1, 1);
    Vec3 end = new Vec3(4, 4, 4);
    Vec3 candidate = new Vec3(3,3,3);

    double degrees = 360;
    degrees = degrees * (Math.PI / 180.0);

    RotoTranslation rot = 
        new RotoTranslation(origin, 
                            start, 
                            end, 
                            degrees);

    Vec3 rotated = rot.RotateVector(candidate);

    Assert.AreEqual(candidate[0], rotated[0]);
    Assert.AreEqual(candidate[1], rotated[1]);
    Assert.AreEqual(candidate[2], rotated[2]);
}

测试发现：将向量绕轴旋转360度后，它没有回到初始位置，不符合预期。要求必须使用Matrix3x3，不能使用Matrix4x4（增广矩阵）。

问题排查与修复 #

核心问题分析 #

1. 3x3矩阵无法表示平移操作：Matrix3x3.CreateTranslation是错误用法，3x3矩阵仅能处理线性变换（旋转、缩放等），平移属于仿射变换，无法通过3x3矩阵实现，强行使用会导致变换逻辑失效。
2. 旋转中心混淆：代码错误地将坐标系原点origin作为平移中心，而绕任意轴旋转的平移中心应该是旋转轴上的任意点（比如轴的起点s）。
3. 矩阵乘法顺序错误：错误地将旋转矩阵与平移矩阵相乘，变换顺序逻辑混乱，导致旋转结果偏离预期。
4. 浮点数类型不匹配：代码中混用float（0.0f）和double类型，可能引发精度问题。
5. 测试用例未考虑浮点数精度：直接用Assert.AreEqual比较浮点数，容易因精度误差导致断言失败。

修复后的代码 #

实现类

public class RotoTranslation
{
    private readonly Vec3 axisPoint; // 旋转轴上的点，作为平移中心
    private readonly double cosTheta;
    private readonly double sinTheta;
    private readonly Vec3 axis;
    private readonly Matrix3x3 rotationMatrix;

    public RotoTranslation(Vec3 axisStart, Vec3 axisEnd, double angle_rad)
    {
        axisPoint = axisStart;
        axis = Vec3.Normalize(axisEnd - axisStart);
        cosTheta = Math.Cos(angle_rad);
        sinTheta = Math.Sin(angle_rad);

        Matrix3x3 uOuter = Vec3.OuterProduct_mat(axis, axis);
        Matrix3x3 uCross = new Matrix3x3(
            0.0, -axis.z, axis.y,
            axis.z, 0.0, -axis.x,
            -axis.y, axis.x, 0.0
        );

        // 正确计算Rodrigues旋转矩阵
        rotationMatrix = cosTheta * Matrix3x3.Identity()
                         + (1.0 - cosTheta) * uOuter
                         + sinTheta * uCross;
    }

    public Vec3 RotateVector(Vec3 vector)
    {
        // 1. 手动平移向量到以旋转轴点为原点的局部坐标系
        Vec3 localVector = vector - axisPoint;
        // 2. 应用3x3旋转矩阵（使用普通向量变换，而非法线变换）
        Vec3 rotatedLocal = Vec3.Transform(localVector, rotationMatrix);
        // 3. 手动平移回原坐标系
        return rotatedLocal + axisPoint;
    }
}

测试代码

[TestMethod]
public void TestMethod1()
{
    Vec3 start = new Vec3(1, 1, 1);
    Vec3 end = new Vec3(4, 4, 4);
    Vec3 candidate = new Vec3(3,3,3);

    double degrees = 360;
    double angle_rad = degrees * (Math.PI / 180.0);

    RotoTranslation rot = new RotoTranslation(start, end, angle_rad);
    Vec3 rotated = rot.RotateVector(candidate);

    // 加入浮点数精度阈值，避免精度误差导致断言失败
    Assert.AreEqual(candidate[0], rotated[0], 1e-6);
    Assert.AreEqual(candidate[1], rotated[1], 1e-6);
    Assert.AreEqual(candidate[2], rotated[2], 1e-6);
}

修复说明 #

• 移除错误的3x3平移矩阵：改用向量加减手动实现平移，符合仅使用Matrix3x3的要求（旋转用3x3矩阵，平移用向量运算）。
• 修正旋转中心：以旋转轴的起点作为平移中心，符合绕任意轴旋转的标准流程。
• 统一浮点数类型：全部使用double类型，避免类型不匹配引发的精度问题。
• 处理浮点数精度：测试断言加入精度阈值，解决浮点数运算的精度误差问题。

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