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

Question

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

这里有两种明显的伪代码方法...

轴角

这很合理，但在角度计算和轴角转换矩阵中会调用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点积得到angle_sin和angle_cos来避免计算角度。

两个矩阵之间的差异

这里是另一种方法，它从向量构造了两个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;
}

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

编辑：这个问题的目的不是为了微调和基准测试每种方法。相反，我想知道是否存在一些完全不同并且更优越的方法，我不知道这些方法。

---

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

- ideasman42

---

@legends2k，就我所知不是这样的，我在寻找比轴角法更好的替代方法时发现了这个（它经过充分测试，即使在退化情况下也能正常工作）。 - ideasman42

如果您编写m1.transpose(); matrix = m2 * m1;并返回矩阵，难道不应该获得相同的结果吗？这将节省一个转置操作。 - Tesseract

@ideasman42：好的，明白了。假设A和B是两个矩阵，它们之间有一个矩阵X用于表示它们的差异，那么 AX = B ⇒ X = A⁻¹B。由于纯旋转可以使用正交矩阵来表示，所以它的转置就是它的逆矩阵。如果你对 m1 进行转置然后与 m2 相乘，我认为第二次转置是多余的。 - legends2k

@legends2k：在旋转中，旋转矩阵首先出现，而被旋转的矩阵/向量其次。因此，它是 XA = B ⇒ X = BA⁻¹。 - Tesseract

@SpiderPig，你是对的，去掉了冗余的转置。 - ideasman42

@SpiderPig 这取决于您是否使用行向量或列向量。OP正在使用列向量，因此您的形式是适当的。我使用了这种约定给出答案，但是使用行向量约定进行了评论。无论哪种方式，第二次转置都是多余的。 - legends2k

Answer 1

你发布的两种方法都可以进行优化。

方法一

不要使用acos来查找两个向量之间的角度，更好的方法是完全避免查找角度。如何做到？ Rodrigues的轴角公式仅需要sin θ、cos θ和1-cos θ，因此查找实际角度是多余的。

我们知道v1和v2是单位向量；由于|v1|=|v2|=1，因此v1 · v2 = |v1| |v2| cos θ直接给出cos θ，找到1-cos θ并不昂贵。 v1 × v2 = |v1| |v2| sin θ n = sin θ n，其中n是垂直于v1和v2的单位向量，找到|v1 × v2|即向量积的大小将直接给出sin θ。

现在我们有了sin θ和cos θ，我们可以直接使用Rodrigues公式构建旋转矩阵；这里是一个简单的实现（尽管该页面声称使用四元数数学，但其实是轴角到矩阵转换公式）。

方法二

在你构造两个正交矩阵作为框架之后，可以避免进行第二次转置。下面是证明。

假设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更快。所以我真的建议你在这两种方法之间进行基准测试（加上一些非平凡负载），然后得出结论。

- legends2k

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;
}

- ideasman42

---

你的回答对我非常有帮助。但是，你能解释一下当你进行180度旋转时，在共线情况下会发生什么吗？我看到你使用ortho_v3_v3()函数创建了一个新的轴，但是这个轴是如何选择的呢？我不理解axis_dominant_v3_single()函数，以及switch块。如果你能提供一些见解，那就太好了，谢谢。 - undefined

我的意思是，你不能只是围绕与v1垂直的任何轴旋转吗？ - undefined

对于180度的旋转，你可以围绕任何垂直轴进行旋转。 - undefined

是的，我想知道你的代码是做什么的：你有这个函数，它提到了“主轴”？ - undefined

如果你对如何获得正交轴有疑问，最好单独提出一个关于这个问题的提问。 - undefined

这并不完全是关于一般的正交轴的问题。更多的是关于你的x>y、x>z条件和开关块。 - undefined