View Single Post
Hassan2
Hassan2 is offline
#6
Jan24-13, 12:45 AM
P: 404
I hope you are familiar with the following programming language. The calculation is based on Euler–Rodrigues formula.
The matrix rotMat rotates every point of the object in 3D space by alpha radian around rotAx ( rotation axis). It can make the bars parallel.

public static Mat rotMat(Vect rotAx,double alpha)
{
double e1,e2,e3,e4;

e1=rotAx.el[0]*sin(alpha/2);
e2=rotAx.el[1]*sin(alpha/2);
e3=rotAx.el[2]*sin(alpha/2);
e4=cos(alpha/2);

Mat M=new Mat(3,3);
M.el[0][0]=pow(e1,2)-pow(e2,2)-pow(e3,2)+pow(e4,2);
M.el[0][1]=2*(e1*e2-e3*e4);
M.el[0][2]=2*(e1*e3+e2*e4);
M.el[1][0]=2*(e1*e2+e3*e4);
M.el[1][1]=-pow(e1,2)+pow(e2,2)-pow(e3,2)+pow(e4,2);
M.el[1][2]=2*(e2*e3-e1*e4);
M.el[2][0]=2*(e1*e3-e2*e4);
M.el[2][1]=2*(e2*e3+e1*e4);
M.el[2][2]=-pow(e1,2)-pow(e2,2)+pow(e3,2)+pow(e4,2);
return M;
}