diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp index c1ee3288..911f5a0d 100644 --- a/libs/geom/include/psemek/geom/quaternion.hpp +++ b/libs/geom/include/psemek/geom/quaternion.hpp @@ -226,14 +226,21 @@ namespace psemek::geom // v0 & v1 assumed to be normalized template - quaternion shortest_arc(vector const & v0, vector const & v1) + quaternion shortest_arc(vector const & v0, vector const & v1, T const eps = T{1e-6}) { auto axis = cross(v0, v1); + auto d = dot(v0, v1); + + if (d + T{1} < eps) + axis = ort(v0); + quaternion result; + result[0] = axis[0]; result[1] = axis[1]; result[2] = axis[2]; - result[3] = T{1} + dot(v0, v1); + result[3] = T{1} + d; + result.coords = normalized(result.coords); return result; }