Поворот вокруг вектора.

void rotateP3D(double p[4], double v0[4], double v1[4], double angle, double res[4]){
    
    double d = sqrt(pow(v1[1], 2) + pow(v1[2], 2));
 
    double T[4][4];
    double T_1[4][4];
    double Rx[4][4];
    double Ry[4][4];
    double Rv[4][4];
    double Rx_1[4][4];
    double Ry_1[4][4];
    double M[4][4];
    double M_1[4][4];
    double R[4][4];
 
    double cos_a = v1[2] / d;
    double cos_b = d;   //Тут вообще непонятно, а если d>1 ???
    double a = acos(cos_a);
    double b = acos(cos_b);
 
           //makeEmatr4 создает единичную матрицу 4х4
    makeEmatr4(T);
    makeEmatr4(T_1);
    makeEmatr4(Rx);
    makeEmatr4(Ry);
    makeEmatr4(Rv);
    makeEmatr4(Rx_1);
    makeEmatr4(Ry_1);   
 
    Rx[1][1] = cos(a);      Rx[1][2] = sin(a);      Rx[2][1] = -sin(a);     Rx[2][2] = cos(a);
    Ry[0][0] = cos(-b);     Ry[0][2] = -sin(-b);    Ry[2][0] = sin(-b);     Ry[2][2] = cos(-b);
    Rv[0][0] = cos(angle);  Rv[0][1] = sin(angle);  Rv[1][0] = -sin(angle); Rv[1][1] = cos(angle);
    Rx_1[1][1] = cos(-a);   Rx_1[1][2] = sin(-a);   Rx_1[2][1] = -sin(-a);  Rx_1[2][2] = cos(-a);
    Ry_1[0][0] = cos(b);    Ry_1[0][2] = -sin(b);   Ry_1[2][0] = sin(b);    Ry_1[2][2] = cos(b);
    
    T[3][0] = -v0[0]; T[3][1] = -v0[1]; T[3][2] = -v0[2];
    T_1[3][0] = v0[0]; T_1[3][1] = v0[1]; T_1[3][2] = v0[2];
 
           //matr4Mul перемножает матрицы 4х4
    matr4Mul(T, Rx, M);
    matr4Mul(M, Ry, M);
    matr4Mul(Ry_1, Rx_1, M_1);
    matr4Mul(M_1, T_1, M_1);
    matr4Mul(M, Rv, R);
    matr4Mul(R, M_1, R);
 
           //VM4Mul умножает вектор на матрицу 4х4
    VM4Mul(p,R,res);
}