Merge pull request #58 from pygae/general_logarithm

Added proper general conformal log with decomposition a la Hestenes …

clifford/tools/g3c/__init__.py

@@ -204,94 +204,6 @@ def interpret_multivector_as_object(mv):
     else:
         return -1
 
-
-def general_exp(x, order=9):
-    """
-    This implements the series expansion of e**mv where mv is a multivector
-    The parameter order is the maximum order of the taylor series to use
-    """
-
-    result = 1.0
-    if (order == 0):
-        return result
-
-    # scale by power of 2 so that its norm is < 1
-    max_val = int(np.max(np.abs(x.value)))
-    scale=1
-    if max_val > 1:
-        max_val <<= 1
-    while max_val:
-        max_val >>= 1
-        scale <<= 1
-
-    scaled = x * (1.0 / scale)
-
-    # taylor approximation
-    tmp = 1.0
-    for i in range(1, order):
-        tmp = tmp*scaled * (1.0 / i)
-        result += tmp
-
-    # undo scaling
-    while scale > 1:
-        result *= result
-        scale >>= 1
-    return result
-
-
-
-def general_logarithm(V):
-    """
-    This implements the logarithm of a TRS rotor to a bivector
-    Ie. any translation rotation and scaling rotor
-    """
-    epsilon = 10**(-6)
-    R = V(e123).normal()
-    RV = R*V
-
-    # Extract the scaling
-    tanh_gamma_2 = -RV[e45]/RV[0]
-    gamma = 2*np.arctanh(tanh_gamma_2)
-
-    if abs(gamma) < epsilon:
-        gamma_dash = 1
-    else:
-        gamma_dash = gamma/(np.exp(gamma)-1)
-    S = (np.cosh(gamma/2) + np.sinh(gamma/2)*(ninf^no)).normal()
-    R = V(e123).normal()
-    T = (V*~S*~R).normal()
-    t = -2*(eo.lc(T))(1)
-    if abs(R[0] - 1) < epsilon:  # No rotation
-        biv_log = -gamma_dash*t*einf/2 + gamma*(eo^einf)/2
-        return biv_log
-    elif abs(gamma) < epsilon:  # No scaling component
-            phi = np.arccos(float(V[0]))  # scalar
-            phi2 = phi * phi  # scalar
-            # Notice: np.sinc(pi * x)/(pi x)
-            phi_sinc = np.sinc(phi / np.pi)  # scalar
-            phiP = ((V(2) * ninf) | ep) / (phi_sinc)
-            t_normal_n = -((phiP * V(4)) / (phi2 * phi_sinc))
-            t_perpendicular_n = -(phiP * (phiP * V(2))(2)) / (phi2 * phi_sinc)
-            return phiP + t_normal_n + t_perpendicular_n
-    else:  # Definitely have rotation and scaling
-        I = -R(2) / abs(R(2))
-        phi = 2 * np.arctan2(abs(R(2)), R[0])
-        if abs(t) > epsilon:  # Translation too, full triple whammy
-            tIoverI = (t.lc(I) * ~I)
-            A = 1 - (np.exp(gamma)) * (R * R)
-            A_rev = ~A
-            A_inv = A_rev/((A*A_rev)[0])
-            Tv = (1 + ninf * A_inv * tIoverI*0.5 ).normal()
-            t_perp = ((t ^ I) * ~I)
-            biv_log_perp = -gamma_dash * t_perp * einf/2
-            biv_log_par = Tv * (-I * phi / 2 - gamma * (no ^ ninf)/2 ) * ~Tv
-            biv_log = biv_log_perp + biv_log_par
-            return biv_log
-        else:  # No translation, just rotation and scaling
-            biv_log = -I * phi/2 - gamma * (no ^ ninf) / 2
-            return biv_log
-
-
 def get_circle_in_euc(circle):
     """ Extracts all the normal stuff for a circle """
     Ic = (circle^ninf).normal()

clifford/tools/g3c/rotor_parameterisation.py

@@ -23,6 +23,113 @@
 I3_val = I3.value
 
 
+def dorst_sinh(A):
+    """
+    sinh of a bivector as given in square root and logarithm of rotors
+    by Dorst and Valkenburg
+    """
+    A2 = (A * A)[0]
+    if A2 > 0:
+        root_A2 = np.sqrt(A2)
+        return (np.sinh(root_A2) / root_A2) * A
+    elif abs(A2) < 0.000001:
+        return +A
+    else:
+        root_A2 = np.sqrt(-A2)
+        return (np.sin(root_A2) / root_A2) * A
+
+
+def dorst_atanh2(s, c):
+    """
+    Atanh2 of a bivector as given in square root and logarithm of rotors
+    by Dorst and Valkenburg
+    """
+    s2 = (s * s)[0]
+    if s2 > 0:
+        root_s2 = np.sqrt(s2)
+        return (np.arcsinh(root_s2) / root_s2) * s
+    elif abs(s2) < 0.000001:
+        return +s
+    else:
+        root_s2 = np.sqrt(-s2)
+        return (np.arctan2(root_s2, c) / root_s2) * s
+
+
+def decompose_bivector(F):
+    """
+    Takes a bivector and decomposes it into 2 commuting blades
+    From Hesternes and Sobzyk GA to GC p81 and with corrections
+    by Anthony Lasenby
+    """
+    c1 = F
+    F2 = F * F
+    if F2 == 0:
+        return +F, 0*e1
+    c2 = 0.5 * F2(4)
+    c1_2 = (c1 * c1)[0]
+    c2_2 = (c2 * c2)[0]
+    lambs = np.roots([1, -c1_2, c2_2])
+    F1 = (c1 * c2 - lambs[0] * c1) / (lambs[1] - lambs[0])
+    F2 = (c1 * c2 - lambs[1] * c1) / (lambs[0] - lambs[1])
+    return F1, F2
+
+
+def general_exp(x, order=9):
+    """
+    This implements the series expansion of e**mv where mv is a multivector
+    The parameter order is the maximum order of the taylor series to use
+    """
+
+    result = 1.0
+    if (order == 0):
+        return result
+
+    # scale by power of 2 so that its norm is < 1
+    max_val = int(np.max(np.abs(x.value)))
+    scale=1
+    if max_val > 1:
+        max_val <<= 1
+    while max_val:
+        max_val >>= 1
+        scale <<= 1
+
+    scaled = x * (1.0 / scale)
+
+    # taylor approximation
+    tmp = 1.0
+    for i in range(1, order):
+        tmp = tmp*scaled * (1.0 / i)
+        result += tmp
+
+    # undo scaling
+    while scale > 1:
+        result *= result
+        scale >>= 1
+    return result
+
+
+def general_logarithm(R):
+    """
+    Takes a general conformal rotor and returns the log
+    From square root and loagrithm of rotors by Leo Dorst
+    and Robert Valkenburg
+    """
+    F = 2 * (R(4) - R[0]) * R(2)
+    S1, S2 = decompose_bivector(F)
+
+    def dorst_cosh(S):
+        s2 = (S * S)[0]
+        if abs(s2) < 0.000001:
+            return (R ** 2)[0]
+        else:
+            return -((R ** 2)(2) * (S / s2))[0]
+
+    C1 = dorst_cosh(S2)
+    C2 = dorst_cosh(S1)
+
+    return -0.5 * (dorst_atanh2(S1, C1) + dorst_atanh2(S2, C2))
+
+
 def full_conformal_biv_params_to_biv(biv_params):
     """
     Converts the bivector parameters for a general TRS rotor into
@@ -94,92 +201,6 @@ def residual_cost(biv_params):
     return TRS_biv_params_to_rotor(res.x).clean(0.00001).normal()
 
 
-def general_exp(x, order=9):
-    """
-    This implements the series expansion of e**mv where mv is a multivector
-    The parameter order is the maximum order of the taylor series to use
-    """
-
-    result = 1.0
-    if (order == 0):
-        return result
-
-    # scale by power of 2 so that its norm is < 1
-    max_val = int(np.max(np.abs(x.value)))
-    scale=1
-    if max_val > 1:
-        max_val <<= 1
-    while max_val:
-        max_val >>= 1
-        scale <<= 1
-
-    scaled = x * (1.0 / scale)
-
-    # taylor approximation
-    tmp = 1.0
-    for i in range(1, order):
-        tmp = tmp*scaled * (1.0 / i)
-        result += tmp
-
-    # undo scaling
-    while scale > 1:
-        result *= result
-        scale >>= 1
-    return result
-
-
-def general_logarithm(V):
-    """
-    This implements the logarithm of a TRS rotor to a bivector
-    Ie. any translation rotation and scaling rotor
-    """
-    epsilon = 10**(-6)
-    R = V(e123).normal()
-    RV = R*V
-
-    # Extract the scaling
-    tanh_gamma_2 = -RV[e45]/RV[0]
-    gamma = 2*np.arctanh(tanh_gamma_2)
-
-    if abs(gamma) < epsilon:
-        gamma_dash = 1
-    else:
-        gamma_dash = gamma/(np.exp(gamma)-1)
-    S = (np.cosh(gamma/2) + np.sinh(gamma/2)*(ninf^no)).normal()
-    R = V(e123).normal()
-    T = (V*~S*~R).normal()
-    t = -2*(eo.lc(T))(1)
-    if abs(R[0] - 1) < epsilon:  # No rotation
-        biv_log = -gamma_dash*t*einf/2 + gamma*(eo^einf)/2
-        return biv_log
-    elif abs(gamma) < epsilon:  # No scaling component
-            phi = np.arccos(float(V[0]))  # scalar
-            phi2 = phi * phi  # scalar
-            # Notice: np.sinc(pi * x)/(pi x)
-            phi_sinc = np.sinc(phi / np.pi)  # scalar
-            phiP = ((V(2) * ninf) | ep) / (phi_sinc)
-            t_normal_n = -((phiP * V(4)) / (phi2 * phi_sinc))
-            t_perpendicular_n = -(phiP * (phiP * V(2))(2)) / (phi2 * phi_sinc)
-            return phiP + t_normal_n + t_perpendicular_n
-    else:  # Definitely have rotation and scaling
-        I = -R(2) / abs(R(2))
-        phi = 2 * np.arctan2(abs(R(2)), R[0])
-        if abs(t) > epsilon:  # Translation too, full triple whammy
-            tIoverI = (t.lc(I) * ~I)
-            A = 1 - (np.exp(gamma)) * (R * R)
-            A_rev = ~A
-            A_inv = A_rev/((A*A_rev)[0])
-            Tv = (1 + ninf * A_inv * tIoverI*0.5 ).normal()
-            t_perp = ((t ^ I) * ~I)
-            biv_log_perp = -gamma_dash * t_perp * einf/2
-            biv_log_par = Tv * (-I * phi / 2 - gamma * (no ^ ninf)/2 ) * ~Tv
-            biv_log = biv_log_perp + biv_log_par
-            return biv_log
-        else:  # No translation, just rotation and scaling
-            biv_log = -I * phi/2 - gamma * (no ^ ninf) / 2
-            return biv_log
-
-
 @numba.njit
 def val_exp(B_val):
     """
@@ -226,10 +247,14 @@ def interpolate_TR_rotors(R_n_plus_1, R_n, interpolation_fraction):
     return R_n_lambda
 
 
-def interpolate_TRS_rotors(R_n_plus_1, R_n, interpolation_fraction):
+def interpolate_rotors(R_n_plus_1, R_n, interpolation_fraction):
     """
-    Interpolates TR type rotors
-    Leo Dorst GA for Computer Science
+    Interpolates all conformal type rotors
+    Mesh Vertex Pose and Position Interpolation using Geometric Algebra
+    Rich Wareham and Joan Lasenby
+    and
+    Square Root and Logarithm of Rotors
+    Leo Dorst and Robert Valkenburg
     """
     if interpolation_fraction < np.finfo(float).eps:
         return R_n

test/test_g3c_tools.py

@@ -6,7 +6,7 @@
 from clifford.g3c import *
 from clifford.tools.g3c import *
 from clifford.tools.g3c.rotor_parameterisation import ga_log, ga_exp, general_exp, general_logarithm,\
-    interpolate_TRS_rotors
+    interpolate_rotors
 from clifford.tools.g3c.rotor_estimation import *
 from clifford.tools.g3c.object_clustering import *
 from clifford.tools.g3c.scene_simplification import *
@@ -33,19 +33,15 @@ class TestGeneralLogarithm(unittest.TestCase):
 
     def test_general_logarithm_rotation(self):
         # Check we can reverse rotations
-        for i in range(5):
-            phi = 2 * (np.random.rand() - 1) * np.pi
-            m = random_euc_mv().normal()
-            n = random_euc_mv().normal()
-            biv = - (m ^ n).normal() * phi /2
-            R = general_exp(biv).normal()
+        for i in range(50):
+            R = random_rotation_rotor()
             biv_2 = general_logarithm(R)
             biv_3 = ga_log(R)
-            np.testing.assert_almost_equal(biv.value, biv_2.value, 3)
+            np.testing.assert_almost_equal(biv_2.value, biv_3.value, 3)
 
     def test_general_logarithm_translation(self):
         # Check we can reverse translation
-        for i in range(5):
+        for i in range(50):
             t = random_euc_mv()
             biv = ninf * t /2
             R = general_exp(biv).normal()
@@ -124,6 +120,21 @@ def test_general_logarithm_TRS(self):
             C3 = (V_rebuilt * C1 * ~V_rebuilt).normal()
             np.testing.assert_almost_equal(C2.value, C3.value)
 
+    def test_general_logarithm_conformal(self):
+
+        object_generators = [random_point_pair, random_line, random_circle, random_plane]
+        # object_generators = [random_sphere]
+
+        for obj_gen in object_generators:
+            print(obj_gen.__name__)
+            for i in range(10000):
+                X = obj_gen()
+                Y = obj_gen()
+                R = rotor_between_objects(X, Y)
+                biv = general_logarithm(R)
+                R_recon = general_exp(biv).normal()
+                np.testing.assert_almost_equal(R.value, R_recon, 3)
+
 
 class ConformalArrayTests(unittest.TestCase):