Calculate Rotation Matrix to align Vector (1,1,1) to a vector

Question

I want to get rotation matrix R that calculate a vector [x,y,z] from vector [1,1,1]:

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

knowing x,y,z, I try to calcute rotation matrix like said here: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d

Under here I'll show you the code:

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

But if I try to run my function the final result (R*(1,1,1)) is not the starting vector. Can anyone help me?

Answer 1

There is a missunderstanding in your code. This part is wrong:

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

Note, the dot product of 2 unit vectors (normalized vectors) is equal the cosine of the angle between the 2 unit vectors and the length (magnitude) of the vector which is given by the cross product of 2 unit vectors is equal the sine of the angle between the 2 unit vectors.

In other words, if alpha is the angle between v1 and v2 and the length of v1 and v2 is 1, then the following is fulfilled:

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

After the following lines your vetors vector and vid are normalized (unit vectors):

normal(vector);
normal(vid);

This means you have to change youre code like this:

cross(vid, vector, v);

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

What your code actually does is not calculating the sine and the cosine of the angle. It calculates the power of 2 of the sine and the cosine.

Further, as mentioned in the comment below, in vector_rotation it should be result[i] = instead of result[k] =:

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