Проблемы с реализацией формулы вращения Родригеса в переполнении стека

Я пытаюсь реализовать функцию, которая берет два геометрических вектора в трехмерном пространстве и возвращает матрицу вращения, которая поворачивает первый вектор ко второму вектору. В настоящее время моя функция использует формулу вращения Родригеса для создания матрицы, но моя реализация этой формулы дает неправильный ответ для некоторых входных данных. Я проверил математику вручную для одного теста, который дал неверный результат, и моя работа дала тот же результат.

Вот код для моей функции:

Matrix3d rotation_matrix(Vector3d vector0, Vector3d vector1)
{
vector0.normalize();
vector1.normalize();
// vector orthogonal to both inputs
Vector3d u = vector0.cross(vector1);
if (!u.norm())
{
if (vector0 == vector1)
return Matrix3d::Identity();
// return rotation matrix that represents 180 degree rotation
Matrix3d m1;
m1 << -1, 0, 0,
0,-1, 0,
0, 0, 1;
return m1;
}

/* For the angle between both inputs:
* 1) The sine is the magnitude of their cross product.
* 2) The cosine equals their dot product.
*/
// sine must be calculated using original cross product
double sine = u.norm();
double cosine = vector0.dot(vector1);
u.normalize();

double ux = u[0];
double uy = u[1];
double uz = u[2];
Matrix3d cross_product_matrix;
cross_product_matrix << 0, -uz, uy,
uz,   0,-ux,
-uy,  ux,  0;

Matrix3d part1 = Matrix3d::Identity();
Matrix3d part2 = cross_product_matrix * sine;
Matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);
return part1 + part2 + part3;
}

Я использую библиотеку Eigen C ++ для линейной алгебры (доступна здесь):
http://eigen.tuxfamily.org/index.php?title=Main_Page

Любая помощь приветствуется. Благодарю.