C 计算旋转矩阵以将向量（1,1,1）与向量对齐

我想得到旋转矩阵R，它根据向量[1,1,1]计算向量[x，y，z]：

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

知道x，y，z，我试着计算旋转矩阵，就像这里说的：

在这里，我将向您展示代码：

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

#包括
#包括
#定义pi 3.14149265359
类型定义结构浮动3_
{
浮动x，y，z；
}浮动3；
浮动透镜（浮动*v）
{
返回sqrt（v[0]*v[0]+v[1]*v[1]+v[2]*v[2]）；
}
浮点数（浮点数*v1，浮点数*v2）
{
返回v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]；
}
无效交叉（浮动*v1，浮动*v2，浮动*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）//规范化
{
浮子温度=透镜（v）；
如果（温度==0）
温度=1；
v[0]/=温度；
v[1]/=温度；
v[2]/=温度；
}
无效矩阵_乘积（浮点a[3][3]、浮点b[3][3]、浮点c[3][3]）
{
int i，j，k，和；
对于（i=0；i<3；i++）
{
对于（j=0；j<3；j++）
{
c[i][j]=0；
对于（k=0；k<3；k++）
c[i][j]=c[i][j]+a[i][k]*b[k][j]；
}
}
}
void prod_矩阵_标量（float m[3][3]、float k、float r[3][3]）
{
int i，j=0；
对于（i=0；i而言，您的代码中存在错误理解。这部分是错误的：

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

注意，2个单位向量（归一化向量）的点积等于2个单位向量之间角度的余弦，2个单位向量的叉积给出的向量长度（幅度）等于2个单位向量之间角度的正弦

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


dot（v1，v2）=cos（alpha）
len（cross（v1，v2））==sin（alpha）

在以下几行之后，您的检查者
向量
和
视频
被标准化（单位向量）：

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

cross(vid, vector, v);

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



你的代码实际做的不是计算角度的正弦和余弦，而是计算正弦和余弦的2的幂



此外，如下面的注释所述，在
矢量旋转中，它应该是
结果[i]=
，而不是
结果[k]=
：

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

结果[i]=0；
对于（k=0；k<3；k++）
结果[i]=结果[k]+旋转矩阵[i][k]*向量[k]；
O/T 12位pi近似值的任何特殊原因？A
double
通常适用于多达17位。请注意
float len（float*v）{返回sqrt（v[0]*v[0]+v[1]*v[1]+v[2]*v[2]）；
添加并乘以
float
，转换为
double
，调用
sqrt（）
，在返回时将
双精度
转换为
浮点
。可能只需使用
sqrtf（）
。非常感谢！！我不明白为什么会有cos和sin！我发现向量旋转中的另一个错误：结果[k]=结果[k]+旋转矩阵[i][k]*向量[k]；结果[i]不是结果[k].现在成功了！！再次感谢你！
result[i] = 0;
for( k = 0; k < 3; k++ )
    result[i] = result[k] + rotation_matrix[i][k] * vector[k];