diff --git a/src/quaternion.hh b/src/quaternion.hh
index 89af3afc24bf82f22e812286c7d30457a9a3e5f0..218df072645136d588e1ddfbdcb63a1eee19c9b4 100644
--- a/src/quaternion.hh
+++ b/src/quaternion.hh
@@ -310,7 +310,7 @@ public:
         // Compute the geodesical distance between a and b on SO(3)
         // Due to numerical dirt, diff[3] may be larger than 1. 
         // In that case, use 1 instead of diff[3].
-        T dist = 2*std::acos( diff[3]);
+        T dist = 2*std::acos( std::min(diff[3],1.0) );
 
         T invSinc = 1/sincHalf(dist);