In function : rodrigues_to_matrix, the ct 's formula should be : double ct = (a == 0.0) ? 0.5f : (1.0f - std::cos(a)) / (a * a);