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

Question

我想获得从 vector [1,1,1] 计算 vector [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)) 不是起始 vector 。谁能帮帮我？

Answer 1

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

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

请注意，2 个单位 vector (归一化 vector )的点积等于 2 个单位 vector 之间夹角的余弦与由 2 个单位 vector 的叉积给出的 vector 长度(幅度)相等2 个单位 vector 之间夹角的正弦值。

换句话说，如果alpha是v1和v2的夹角和v1的长度，则v2为1，则满足:

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

在以下几行之后，您的 vector vector 和 vid 被归一化(单位 vector ):

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