3D Rotation Algorithm about arbitrary axis with C/C++ code

When an object is to be rotated about an axis that is not parallel to one of the coordinate axes, we need to perform some additional transformations. In this case, we also need rotations to align the axis with a selected coordinate axis and to bring the axis back to its original orientation. Given the specifications for the rotation axis and the rotation angle, we can accomplish the required rotation in five steps.

1. Translate the object so that the rotation axis passes through the coordinate origin.
2. Rotate the object so that the axis of rotation coincides with one of the coordinate axes.
3. Perform the specified rotation about that coordinate axis.
4. Apply inverse rotations to bring the rotation axis back to its original orientation.
5. Apply the inverse translation to bring the rotation axis back to its original position.
   

Let (u, v, w) be a vector that specify axis about which the object is to be rotated. Suppose the vector passes through a point (a, b, c). Then the general composite matrix for the rotation can be written as:

 

Here L is magnitude of the axis vector. C source code for the arbitrary rotation is given below. Here (a, b, c) is assumed to be origin for simplicity.
Source Code

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84

#include <iostream> #include <cmath>   using namespace std;   typedef struct {     float x;     float y;     float z; }Point; Point points;   float rotationMatrix[4][4]; float inputMatrix[4][1] = {0.0, 0.0, 0.0, 0.0}; float outputMatrix[4][1] = {0.0, 0.0, 0.0, 0.0};   void showPoint(){     cout<<"("<<outputMatrix[0][0]<<","<<outputMatrix[1][0]<<","<<outputMatrix[2][0]<<")"<<endl; }   void multiplyMatrix() {     for(int i = 0; i < 4; i++ ){         for(int j = 0; j < 1; j++){             outputMatrix[i][j] = 0;             for(int k = 0; k < 4; k++){                 outputMatrix[i][j] += rotationMatrix[i][k] * inputMatrix[k][j];             }         }     } } void setUpRotationMatrix(float angle, float u, float v, float w) {     float L = (u*u + v * v + w * w);     angle = angle * M_PI / 180.0; //converting to radian value     float u2 = u * u;     float v2 = v * v;     float w2 = w * w;       rotationMatrix[0][0] = (u2 + (v2 + w2) * cos(angle)) / L;     rotationMatrix[0][1] = (u * v * (1 - cos(angle)) - w * sqrt(L) * sin(angle)) / L;     rotationMatrix[0][2] = (u * w * (1 - cos(angle)) + v * sqrt(L) * sin(angle)) / L;     rotationMatrix[0][3] = 0.0;       rotationMatrix[1][0] = (u * v * (1 - cos(angle)) + w * sqrt(L) * sin(angle)) / L;     rotationMatrix[1][1] = (v2 + (u2 + w2) * cos(angle)) / L;     rotationMatrix[1][2] = (v * w * (1 - cos(angle)) - u * sqrt(L) * sin(angle)) / L;     rotationMatrix[1][3] = 0.0;       rotationMatrix[2][0] = (u * w * (1 - cos(angle)) - v * sqrt(L) * sin(angle)) / L;     rotationMatrix[2][1] = (v * w * (1 - cos(angle)) + u * sqrt(L) * sin(angle)) / L;     rotationMatrix[2][2] = (w2 + (u2 + v2) * cos(angle)) / L;     rotationMatrix[2][3] = 0.0;       rotationMatrix[3][0] = 0.0;     rotationMatrix[3][1] = 0.0;     rotationMatrix[3][2] = 0.0;     rotationMatrix[3][3] = 1.0; }   int main() {     float angle;     float u, v, w;     cout<<"Enter the initial point you want to transform:";     cin>>points.x>>points.y>>points.z;     inputMatrix[0][0] = points.x;     inputMatrix[1][0] = points.y;     inputMatrix[2][0] = points.z;     inputMatrix[3][0] = 1.0;       cout<<"Enter axis vector: ";     cin>>u>>v>>w;       cout<<"Enter the rotating angle in degree: ";     cin>>angle;       setUpRotationMatrix(angle, u, v, w);     multiplyMatrix();     showPoint();       return 0; }