已知俩向量A,B ,求解A ->B的变换矩阵(求俩向量变换矩阵):
一共搞到了俩种方案:
1.利用罗德里格斯旋转公式求解:
//源 100,0,0
//目标 400,400,400
//锚点 0,0,0
Eigen::Vector3d point_anchor = { 0,0,0 };
double vec_src[3] = { 100,0,0 };
double vec_dst[3] = { 400,400,400 };
double axis_rotation[3] = { 0 };
vtkMath::Cross(vec_src, vec_dst, axis_rotation);
double norm = vtkMath::Norm(axis_rotation);
axis_rotation[0] = axis_rotation[0] / norm;
axis_rotation[1] = axis_rotation[1] / norm;
axis_rotation[2] = axis_rotation[2] / norm;
double angle = acos(vtkMath::Dot(vec_src, vec_dst) / (vtkMath::Norm(vec_src) * vtkMath::Norm(vec_dst)));
angle = angle * 180 / 3.1415926;
double angle2 = vtkMath::AngleBetweenVectors(vec_src, vec_dst);
Eigen::Matrix3d N;
N << 0, -axis_rotation[2], axis_rotation[1],
axis_rotation[2], 0, -axis_rotation[0],
-axis_rotation[1], axis_rotation[0], 0;
Eigen::Matrix3d R;
Eigen::Matrix3d I;
I.Identity();
//罗德里格斯旋转公式
R = I + sin(angle) * N + (1 - cos(angle)) * N * N;
cout << R << angle;
Eigen::Matrix4d Matrix_Result;
Matrix_Result << R, -R * point_anchor + point_anchor,
0, 0, 0, 1;
//这个感觉没得到自己想要的,可能哪有小小的问题,请大佬们指正。
参考翻译自此博主:
已知两向量,计算能够使两向量对齐的刚体变换矩阵
2.利用Eigen 库内封装的函数求解:
直接上码
Eigen::Matrix3d rotMatrix;
Eigen::Vector3d vectorBefore(100, 0, 0); //A向量
Eigen::Vector3d vectorAfter(400, 400, 400); //B向量
rotMatrix = Eigen::Quaterniond::FromTwoVectors(vectorBefore, vectorAfter).toRotationMatrix();
//所求 rotMatrix 即为 A -> B 的变换矩阵
//要是像移动到某个位置,自己外加矩阵变换,
此函数 原理没有细看,粘一个解释吧:
/** Returns a quaternion representing a rotation between
* the two arbitrary vectors \a a and \a b. In other words, the built
* rotation represent a rotation sending the line of direction \a a
* to the line of direction \a b, both lines passing through the origin.
*
* \returns resulting quaternion
*
* Note that the two input vectors do \b not have to be normalized, and
* do not need to have the same norm.
*/