三维向量绕轴旋转的c++实现

罗德里格的公式 ：Rodrigues旋转公式_渣渣-CSDN博客_rodrigues

使用OSG实现：

inline osg::Vec3d rotate(osg::Vec3d & p, osg::Vec3d & r, double a) const
{
	osg::Vec3d data1 = p * cos(a) + (r ^ p) * sin(a) + r * (p * r)  * (1 - cos(a));
	return data1;
}

测试一下：

osg::Vec3d p(1.414, 0, 0);
osg::Vec3d x(1.0, 0, 0);
osg::Vec3d y(0, 1.0, 0);
osg::Vec3d z(0, 0, 1.0);

double a = 45.0 * M_PI / 180.0;

osg::Vec3d r = z;
		
osg::Vec3d data1 = rotate(p, r, a);
printf("%.3f, %.3f, %.3f\n", data1.x(), data1.y(), data1.z());

r = osg::Vec3d(1, -1, 0);
r.normalize();
a = M_PI / 2;
data1 = rotate(data1, r, a);

printf("%.3f, %.3f, %.3f\n", data1.x(), data1.y(), data1.z());
return data1;

需要注意的是：旋转轴 r 应该是个单位向量；