计算旋转矩阵以将向量 (1,1,1) 与向量对齐

Question

我想得到从向量 [1,1,1] 计算向量 [x,y,z] 的旋转矩阵 R：

R*[1,1,1]=[x,y,z]

知道 x,y,z，我尝试计算旋转矩阵，就像这里所说的：https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d

下面我给你看代码：

#include <stdio.h>
#include <math.h>
#define pi 3.14149265359

typedef struct float3_
{
    float x,y,z;
} float3;


float len(float* v)
{
    return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
}

float dot(float* v1,float* v2)
{
    return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
}

void cross(float* v1,float* v2, float* v3)
{
    v3[0]=v1[1]*v2[2]-v2[1]*v1[2];
    v3[1]=v1[2]*v2[0]-v2[2]*v1[0];
    v3[2]=v1[0]*v2[1]-v2[0]*v1[1];
}


void normal(float* v) //Normalization
{
    float temp=len(v);
    if (temp==0)
        temp=1;
    v[0]/=temp;
    v[1]/=temp;
    v[2]/=temp;
}


void matrix_product(float a[3][3], float b[3][3], float c[3][3])
{
    int i,j,k,sum;
    for( i = 0; i < 3; i++ )
    {
    for( j = 0; j < 3; j++ )
     {
        c[i][j] = 0;
        for( k = 0; k < 3; k++ )
              c[i][j] = c[i][j] + a[i][k] * b[k][j];
     }
    }
}



void prod_matrix_scalar(float m[3][3], float k, float r[3][3])
{
    int i,j=0;
    for (i=0;i<3;i++)
        for(j=0;j<3;j++)
        r[i][j]=m[i][j]*k;
}


void sum_matrix(float m[3][3], float n[3][3], float r[3][3])
{
    int i,j=0;
    for (i=0;i<3;i++)
        for(j=0;j<3;j++)
        r[i][j]=m[i][j]+n[i][j];
}


void vector2rotation_matrix (float vector[3],float R[3][3]) //myfunction
{
    float vid[3]={1,1,1};
    float v[3];

    printf("Vector vector:");
    print_vector(vector);
    printf("V vid:");
    print_vector(vid);

    printf("\nlen(vector)=%f\n",norm(vector));
    printf("\nlen(vid)=%f\n\n",norm(vid));

    //Normalization
    normal(vector);
    normal(vid);

    printf("Vector vector normalized:");
    print_vector(vector);
    printf("Vector vid normalized:");
    print_vector(vid);
    printf("\nlen(vector normalized)=%f\n",norm(vector));
    printf("\nlen(vid normalized)=%f\n",norm(vid));

    //cross product v=(1,1,1)x(vector)
    cross(vid,vector, v);
    printf("Vector v:");
    print_vector(v);
    printf("\nlen(v)=%f\n",norm(v));

    //angle between vectors
    float alfa=acos(dot(vid,vector));
    printf("Alfa angle between vectors: %f rad\n",alfa);

    //s
    float s = len(v)*sin(alfa);
    printf("s: %f\n",s);

    //c
    float c = dot(vid,vector)*cos(alfa);
    printf("c: %f\n",c);

    //Matrices
    float Id[3][3]={1,0,0,0,1,0,0,0,1};
    float Vx[3][3]={0,-v[2],v[1],v[2],0,-v[0],-v[1],v[0],0};
    float Vx2[3][3];
    float Vx2k[3][3];
    float Ris[3][3];
    printf("\nMatrixIdentity I:\n");
    print_matrix(Id);
    printf("\n\nMatrixVx:\n");
    print_matrix(Vx);

    //Vx^2
    matrix_product(Vx,Vx,Vx2);
    printf("\n\nMatrixVx2:\n");
    print_matrix(Vx2);

    //k=(1-c)/(s^2)
    float k=(1-c)/(s*s);
    printf("\nk=%f\n",k);

    //V^2*k
    prod_matrix_scalar(Vx2,k,Vx2k);
    printf("\n\nMatrix Vx2*k:\n");
    print_matrix(Vx2k);

    //R= Id + Vx + Vx2k
    sum_matrix(Id,Vx,Ris);
    sum_matrix(Ris,Vx2k,R);

    printf("\n\nMatrix*1 1 1=vector:\n");
    print_matrix(R);

    float vris[3];
    vector_rotation(R,vid,vris);

    printf("\n\nStart vector: ");
    print_vector(vector);
    printf("Matrix*1,1,1= ");
    print_vector(vris);
}



void print_matrix(float matrix[3][3])
{
    int i,j;
  for (i=0; i<3; i++)
  {
    printf("\n");
    for (j=0; j<3; j++)
      printf("%f ", matrix[i][j]);
  }

}

void print_vector(float v[3])
{
    int i=0;
    printf("[");
    for(i=0; i<3; i++)
        printf(" %f ",v[i]);
    printf("]\n");
}

void vector_rotation(float rotation_matrix[3][3], float vector[3], float result[3])
{
    int i,k;
    for( i = 0; i < 3; i++ )
    {
        result[i] = 0;
        for( k = 0; k < 3; k++ )
              result[k] = result[k] + rotation_matrix[i][k] * vector[k];
     }
}

int main()
{
    float v[3]={2,4,5};
    float v1[3]={1,1,1};
    normal(v1);
    float r[3];
    float matrix[3][3];
    vector2rotation_matrix(v,matrix);
}

但如果我尝试运行我的函数，最终结果 (R*(1,1,1)) 不是起始向量。谁能帮帮我？

Answer 1

您的代码中存在误解。这部分是错误的：

float s = len(v)*sin(alfa);
float c = dot(vid,vector)*cos(alfa);

注意，2 个单位向量（归一化向量）的点积等于 2 个单位向量之间的夹角的余弦值和由 2 个单位向量的叉积给出的向量的长度（大小）相等两个单位向量之间夹角的正弦值。

换句话说，如果alpha是v1和v2之间的角度，并且v1和v2的长度是1，那么满足以下条件：

• dot(v1, v2) == cos(alpha)
• len(cross(v1, v2)) == sin(alpha)

在以下行之后，您的向量 vector 和 vid 被归一化（单位向量）：

normal(vector);
normal(vid);

这意味着您必须像这样更改您的代码：

cross(vid, vector, v);

float s = len(v);
float c = dot(vid, vector);

您的代码实际上所做的不是计算角度的正弦和余弦。它计算正弦和余弦的 2 的幂。

此外，正如下面评论中提到的，在vector_rotation 中应该是result[i] = 而不是result[k] =：

result[i] = 0;
for( k = 0; k < 3; k++ )
    result[i] = result[k] + rotation_matrix[i][k] * vector[k];