从两个 3D 矢量定义的旋转计算 3x3 旋转矩阵的有效方法

Question

虽然我找到了 2 个解决方案，但我很好奇是否有众所周知的方法来执行此操作，因为这似乎是一项相当常见的任务。

这里有 2 个明显的方法 psudo-code...

轴角

这很合乎逻辑，但是调用了两次sin 和一次cos（在角度计算和轴角度到矩阵的转换中）。

Matrix3x3 rotation_between_vectors_to_matrix(const Vector v1, const Vector v2)
{
    angle = v1.angle(v2);
    axis  = v1.cross(v2);

    /* maths for this is well known */
    Matrix3x3 matrix = axis_angle_to_matrix(axis, angle);

    return matrix;
}

编辑：最直接的功能是相当慢，但是正如这里的回复中所指出的那样：通过从分别为轴长和v1,v2 点积。

两个矩阵的区别

这是我发现的另一种方法，它从向量构造两个 3x3 矩阵并返回差值。

但是，这比可以优化（如上所述）的轴/角度计算要慢。

注意。这假设两个向量都是标准化的，矩阵是列优先的（OpenGL）。

Matrix3x3 rotation_between_vectors_to_matrix(const Vector v1, const Vector v2)
{
    Matrix3x3 m1, m2;

    axis = v1.cross(v2);
    axis.normalize();

    /* construct 2 matrices */
    m1[0] = v1;
    m2[0] = v2;

    m1[1] = axis;
    m2[1] = axis;

    m1[2] = m1[1].cross(m1[0]);
    m2[2] = m2[1].cross(m2[0]);

    /* calculate the difference between m1 and m2 */
    m1.transpose();

    Matrix3x3 matrix = m2 * m1;

    return matrix;
}

有没有更好的方法来执行这个计算？

编辑：这个问题的目的不是对每种方法进行微优化和基准测试。相反 - 我很好奇是否有一些我不知道的完全不同和更好的方法。

---

注意：我故意省略了对共线性向量（其中轴长度为零）的退化情况的检查，以保持示例简单。

Answer 1

您发布的两种方法都可以优化。

方法一

与其使用acos 来求两个向量之间的角度，不如完全避免求角度。如何？ Rodrigues 的axis-angle formula 只需要sin θ、cos θ 和1 - cos θ，因此查找实际角度是多余的。

我们知道v1和v2是单位向量； v1 · v2 = |v1| |v2| cos θ 因为|v1| = |v2| = 1，v1 · v2 直接给我们cos θ，找到1 - cos θ 成本不高。 v1 × v2 = |v1| |v2| sin θ n = sin θ n，其中n 是垂直于v1 和v2 的单位向量，找到|v1 × v2| 的叉积的大小将直接得到sin θ。

现在我们已经有了sin θ和cos θ，我们可以直接使用Rodrigues forumla形成旋转矩阵； here's a simple implementation（虽然页面声称使用四元数数学，但它是轴-角度到矩阵的转换公式）。

方法二

将两个正交帧构建为矩阵后，您可以避免进行第二次转置。这是证据。

说A 和B 是两个矩阵，因为你想从A 旋转到B 我们需要一些矩阵X 与A 相乘将得到B：

XA = B

X = BA⁻¹

这就是你所需要的；当你将X 预乘到A 时，你会得到B。但是，你找到的是Y

Y = AB⁻¹

YB = A

然后你转置Y 得到Y⁻¹ 即

Y⁻¹YB = Y⁻¹A

B = Y⁻¹A

您可以只执行上述仅涉及一次转置的方法，而不是进行两次反转（此处为转置）。

我仍然会说，如果不以优化形式对方法进行基准测试，我们不能说方法 2 比方法 1 快。所以我真的敦促您在两种方法之间进行基准测试（有一些非平凡的负载）然后得出结论。

Answer 2

轴角度法是最快的方法，这是我想出的用于高效轴/角度到 3x3 矩阵转换的 C 代码。

这也检查共线情况。

注意：如果您有自己的数学库，您可能可以在不包含任何相关函数的情况下让rotation_between_vecs_to_mat3 工作以确保完整性。

#include <math.h>
#include <float.h>


/* -------------------------------------------------------------------- */
/* Math Lib declarations */

static void unit_m3(float m[3][3]);
static float dot_v3v3(const float a[3], const float b[3]);
static float normalize_v3(float n[3]);
static void cross_v3_v3v3(float r[3], const float a[3], const float b[3]);
static void mul_v3_v3fl(float r[3], const float a[3], float f);
static void ortho_v3_v3(float p[3], const float v[3]);
static void axis_angle_normalized_to_mat3_ex(
        float mat[3][3], const float axis[3],
        const float angle_sin, const float angle_cos);


/* -------------------------------------------------------------------- */
/* Main function */
void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3]);

/**
 * Calculate a rotation matrix from 2 normalized vectors.
 *
 * v1 and v2 must be unit length.
 */
void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3])
{
    float axis[3];
    /* avoid calculating the angle */
    float angle_sin;
    float angle_cos;

    cross_v3_v3v3(axis, v1, v2);

    angle_sin = normalize_v3(axis);
    angle_cos = dot_v3v3(v1, v2);

    if (angle_sin > FLT_EPSILON) {
axis_calc:
        axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
    }
    else {
        /* Degenerate (co-linear) vectors */
        if (angle_cos > 0.0f) {
            /* Same vectors, zero rotation... */
            unit_m3(m);
        }
        else {
            /* Colinear but opposed vectors, 180 rotation... */
            ortho_v3_v3(axis, v1);
            normalize_v3(axis);
            angle_sin =  0.0f;  /* sin(M_PI) */
            angle_cos = -1.0f;  /* cos(M_PI) */
            goto axis_calc;
        }
    }
}


/* -------------------------------------------------------------------- */
/* Math Lib */

static void unit_m3(float m[3][3])
{
    m[0][0] = m[1][1] = m[2][2] = 1.0;
    m[0][1] = m[0][2] = 0.0;
    m[1][0] = m[1][2] = 0.0;
    m[2][0] = m[2][1] = 0.0;
}

static float dot_v3v3(const float a[3], const float b[3])
{
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

static void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
{
    r[0] = a[1] * b[2] - a[2] * b[1];
    r[1] = a[2] * b[0] - a[0] * b[2];
    r[2] = a[0] * b[1] - a[1] * b[0];
}

static void mul_v3_v3fl(float r[3], const float a[3], float f)
{
    r[0] = a[0] * f;
    r[1] = a[1] * f;
    r[2] = a[2] * f;
}

static float normalize_v3_v3(float r[3], const float a[3])
{
    float d = dot_v3v3(a, a);

    if (d > 1.0e-35f) {
        d = sqrtf(d);
        mul_v3_v3fl(r, a, 1.0f / d);
    }
    else {
        d = r[0] = r[1] = r[2] = 0.0f;
    }

    return d;
}

static float normalize_v3(float n[3])
{
    return normalize_v3_v3(n, n);
}

static int axis_dominant_v3_single(const float vec[3])
{
    const float x = fabsf(vec[0]);
    const float y = fabsf(vec[1]);
    const float z = fabsf(vec[2]);
    return ((x > y) ?
           ((x > z) ? 0 : 2) :
           ((y > z) ? 1 : 2));
}

static void ortho_v3_v3(float p[3], const float v[3])
{
    const int axis = axis_dominant_v3_single(v);

    switch (axis) {
        case 0:
            p[0] = -v[1] - v[2];
            p[1] =  v[0];
            p[2] =  v[0];
            break;
        case 1:
            p[0] =  v[1];
            p[1] = -v[0] - v[2];
            p[2] =  v[1];
            break;
        case 2:
            p[0] =  v[2];
            p[1] =  v[2];
            p[2] = -v[0] - v[1];
            break;
    }
}

/* axis must be unit length */
static void axis_angle_normalized_to_mat3_ex(
        float mat[3][3], const float axis[3],
        const float angle_sin, const float angle_cos)
{
    float nsi[3], ico;
    float n_00, n_01, n_11, n_02, n_12, n_22;

    ico = (1.0f - angle_cos);
    nsi[0] = axis[0] * angle_sin;
    nsi[1] = axis[1] * angle_sin;
    nsi[2] = axis[2] * angle_sin;

    n_00 = (axis[0] * axis[0]) * ico;
    n_01 = (axis[0] * axis[1]) * ico;
    n_11 = (axis[1] * axis[1]) * ico;
    n_02 = (axis[0] * axis[2]) * ico;
    n_12 = (axis[1] * axis[2]) * ico;
    n_22 = (axis[2] * axis[2]) * ico;

    mat[0][0] = n_00 + angle_cos;
    mat[0][1] = n_01 + nsi[2];
    mat[0][2] = n_02 - nsi[1];
    mat[1][0] = n_01 - nsi[2];
    mat[1][1] = n_11 + angle_cos;
    mat[1][2] = n_12 + nsi[0];
    mat[2][0] = n_02 + nsi[1];
    mat[2][1] = n_12 - nsi[0];
    mat[2][2] = n_22 + angle_cos;
}