diff --git a/clifford/tools/g3c/__init__.py b/clifford/tools/g3c/__init__.py index 420d41eb..5a82a7af 100644 --- a/clifford/tools/g3c/__init__.py +++ b/clifford/tools/g3c/__init__.py @@ -205,94 +205,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() diff --git a/clifford/tools/g3c/rotor_parameterisation.py b/clifford/tools/g3c/rotor_parameterisation.py index eab7f405..40b85870 100644 --- a/clifford/tools/g3c/rotor_parameterisation.py +++ b/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 diff --git a/test/test_g3c_tools.py b/test/test_g3c_tools.py index c32c5c35..3245e8de 100644 --- a/test/test_g3c_tools.py +++ b/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):