函数参数:vector:任意旋转轴的方向,用法向量表示(记得要先单位化后再传参),point:旋转轴上某一个点,t:旋转的角度(如90度:PI/2)
// Returns the rotation matrix around a vector placed at a point , rotate by angle t
Eigen::Matrix4f rot_mat(const Eigen::Vector3f& point, const Eigen::Vector3f& vector, const float t)
{
float u = vector(0);
float v = vector(1);
float w = vector(2);
float a = point(0);
float b = point(1);
float c = point(2);
Eigen::Matrix4f matrix;
matrix<<u*u + (v*v + w*w)*cos(t), u*v*(1 - cos(t)) - w*sin(t), u*w*(1 - cos(t)) + v*sin(t), (a*(v*v + w*w) - u*(b*v + c*w))*(1 - cos(t)) + (b*w - c*v)*sin(t),
u*v*(1 - cos(t)) + w*sin(t), v*v + (u*u + w*w)*cos(t), v*w*(1 - cos(t)) - u*sin(t), (b*(u*u + w*w) - v*(a*u + c*w))*(1 - cos(t)) + (c*u - a*w)*sin(t),
u*w*(1 - cos(t)) - v*sin(t), v*w*(1 - cos(t)) + u*sin(t), w*w + (u*u + v*v)*cos(t), (c*(u*u + v*v) - w*(a*u + b*v))*(1 - cos(t)) + (a*v - b*u)*sin(t),
0, 0, 0, 1;
return matrix;
}
附加两个链接:
[1]Rotation About an Arbitrary Axis in 3 Dimensions -- Glenn Murray
第一个是Glenn Murray的Google scholar主页。
第二个是Glenn Murray做的一个可以实时查看旋转效果的网站(包括构造出来的旋转矩阵)。