Improve handling of sqrt for tiny values

quaternion.c

@@ -12,6 +12,7 @@ extern "C" {
 #endif
 
 #include <stdio.h>
+#include <float.h>
 
 #include "quaternion.h"
 
@@ -40,7 +41,12 @@ quaternion_create_from_euler_angles(double alpha, double beta, double gamma) {
 quaternion
 quaternion_sqrt(quaternion q)
 {
-  double absolute = quaternion_absolute(q);
+  double absolute = quaternion_norm(q);  // pre-square-root
+  if(absolute<=DBL_MIN) {
+      quaternion r = {0.0, 0.0, 0.0, 0.0};
+      return r;
+  }
+  absolute = sqrt(absolute);
   if(fabs(absolute+q.w)<_QUATERNION_EPS*absolute) {
     quaternion r = {0.0, sqrt(absolute), 0.0, 0.0};
     return r;

test/test_quaternion.py

@@ -4,6 +4,7 @@
 import os
 import operator
 
+import math
 import numpy as np
 import quaternion
 from numpy import *
@@ -550,10 +551,17 @@ def test_quaternion_sqrt(Qs):
     sqrt_precision = 2.e-15
     for q in Qs[Qs_finitenonzero]:
         assert allclose(q.sqrt() * q.sqrt(), q, rtol=sqrt_precision)
+        # Ensure that non-unit quaternions are handled correctly
         for s in [1, -1, 2, -2, 3.4, -3.4]:
             for r in [1, quaternion.x, quaternion.y, quaternion.z]:
                 srq = s*r*q
-                assert allclose(srq.sqrt() * srq.sqrt(), srq, rtol=sqrt_precision), (s,r,q,srq,srq.sqrt(),srq.sqrt()*srq.sqrt())
+                assert allclose(srq.sqrt() * srq.sqrt(), srq, rtol=sqrt_precision)
+    # Ensure that inputs close to zero are handled gracefully
+    sqrt_dbl_min = math.sqrt(np.finfo(float).tiny)
+    assert quaternion.quaternion(0, 0, 0, 2e-8*sqrt_dbl_min).sqrt() == quaternion.quaternion(0, 0, 0, 0)
+    assert quaternion.quaternion(0, 0, 0, 0.9999*sqrt_dbl_min).sqrt() == quaternion.quaternion(0, 0, 0, 0)
+    assert quaternion.quaternion(0, 0, 0, 1e-16*sqrt_dbl_min).sqrt() == quaternion.quaternion(0, 0, 0, 0)
+    assert quaternion.quaternion(0, 0, 0, 1.1*sqrt_dbl_min).sqrt() != quaternion.quaternion(0, 0, 0, 0)
 
 
 def test_quaternion_log_exp(Qs):