I ~think~ exp and log are correctly implemented now? Tests are good f…

…or certain things, will add more. tolerance is still kinda high for interpolation though and there's this weird thing where I have to keep normalizing the motors

src/PGA3D.jl

@@ -2,8 +2,8 @@ module PGA3D
 
 using StaticArrays
 using LinearAlgebra
-import LinearAlgebra: dot, norm, ⋅, adjoint, cross
-import Base: +, -, *, /, ==, zero, one, abs, convert, getindex, length, size, isapprox, isequal, sqrt
+import LinearAlgebra: dot, norm, ⋅, adjoint, cross, normalize
+import Base: +, -, *, /, ==, zero, one, abs, convert, getindex, length, size, isapprox, isequal, sqrt, exp, log
 
 include("abstract_types.jl")
 include("vector3D.jl")

src/element_construction.jl

@@ -61,20 +61,26 @@ function motor_logarithm(m_ununitized::Motor3D)
     (; theta_hat, theta_hat_perp, mu, nu)
 end
 =#
-function motor_log(m_ununitized::Motor3D)
+function Base.log(m::Motor3D)
     # https://arxiv.org/abs/2206.07496 section 8.2
-    m = unitize(m_ununitized)
-    if m[4] ≈ 1
+    #m = unitize(m_ununitized)
+    if abs(m[4]) ≈ 1
         return Line3D(0, 0, 0, m[5], m[6], m[7])
     else
         a = 1 / (1 - m[4] * m[4])
         b = acos(m[4]) * sqrt(a)
-        c = a * m[3] * (1 - m[4] * b)
-        return Line3D(b * m[1], b * m[2], b * m[3], c * m[3] + b * m[5], c * m[2] + b * m[6], c * m[1] + b * m[7])
+        c = a * m[8] * (1 - m[4] * b)
+        return Line3D(
+            b * m[1],
+            b * m[2],
+            b * m[3],
+            c * m[1] + b * m[5],
+            c * m[2] + b * m[6],
+            c * m[3] + b * m[7])
     end
 end
 
-function line_exp(axis::Line3D)
+function Base.exp(axis::Line3D)
     # https://arxiv.org/abs/2206.07496 section 8.2
     l = axis[1] * axis[1] + axis[2] * axis[2] + axis[3] * axis[3]
     if l ≈ 0
@@ -85,7 +91,15 @@ function line_exp(axis::Line3D)
         c = cos(a)
         s = sin(a) / a
         t = m / l * (c - s)
-        return Motor3D(s * axis[1], s * axis[2], s * axis[3], c, s * axis[1] + t * axis[6], s * axis[2] + t * axis[5], s * axis[3] + t * axis[4], m * s)
+        return Motor3D(
+            s * axis[1],
+            s * axis[2],
+            s * axis[3],
+            c,
+            s * axis[4] + t * axis[1],
+            s * axis[5] + t * axis[2],
+            s * axis[6] + t * axis[3],
+            m * s)
     end
 end
 

src/motor3D.jl

@@ -52,6 +52,13 @@ weight_norm(a::Motor3D) = norm(SA[a[1], a[2], a[3], a[4]])
 bulk_norm(a::Motor3D) = norm(SA[a[5], a[6], a[7], a[8]])
 
 unitize(a::Motor3D) = Motor3D(internal_vec(a) .* (1 / weight_norm(a)))
+function LinearAlgebra.normalize(m::Motor3D)
+    A = 1 / weight_norm(m)
+    B = (m[4] * m[8] - (m[1] * m[5] + m[2] * m[6] + m[3] * m[7])) * A * A * A
+    Motor3D(A * m[1], A * m[2], A * m[3], A * m[4],
+        A * m[5] + B * m[1], A * m[6] + B * m[2], A * m[7] + B * m[3], A * m[8] - B * m[4])
+
+end
 
 reverse(a::Motor3D) = Motor3D(-a[1], -a[2], -a[3], a[4], -a[5], -a[6], -a[7], a[8])
 anti_reverse(a::Motor3D) = reverse(a)

test/test_transformations.jl

@@ -60,7 +60,7 @@ using PGA3D, Test, SafeTestsets, Logging, PrettyPrinting, StaticArrays, Random
     @safetestset "Motor to and from TransformMatrix" begin
         using PGA3D, Test, SafeTestsets, Logging, PrettyPrinting, StaticArrays, Random, LinearAlgebra
         motor_identity = identity_motor()
-        for i in 1:250
+        for i in 1:1000
             testfrom = Point3D(randn(3)...)
             testto = Point3D(randn(3)...)
             testline = line_fromto(testfrom, testto)
@@ -101,4 +101,35 @@ using PGA3D, Test, SafeTestsets, Logging, PrettyPrinting, StaticArrays, Random
             @test testmatrixinv3 ≈ testmatrixinv
         end
     end
+
+    @safetestset "Motor log and bivector exp" begin
+        using PGA3D, Test, SafeTestsets, Logging, PrettyPrinting, StaticArrays, Random, LinearAlgebra
+        motor_identity = identity_motor()
+        for i in 1:1000
+            testfrom = Point3D(randn(3)...)
+            testto = Point3D(randn(3)...)
+            testline = line_fromto(testfrom, testto)
+            testangle = rand()
+            testdisp = rand()
+            testmotor = normalize(motor_screw(testline, testangle, testdisp))
+
+            testbv = log(testmotor)
+
+            testmotorexp = normalize(exp(testbv))
+
+            @test testmotorexp ≈ testmotor || testmotorexp ≈ -testmotor
+
+            testbvha = Line3D(internal_vec(testbv) .* 0.5)
+            testmotorexpha = normalize(exp(testbvha))
+            testmotorexpha2 = normalize(testmotorexpha * testmotorexpha)
+
+
+            #@info "testmotor: $testmotor"
+            #@info "testmotorexpha: $testmotorexpha"
+            #@info "testmotorexpha2: $testmotorexpha2"
+            @test isapprox(testmotorexpha2, testmotor; atol=0.1) || isapprox(testmotorexpha2, -testmotor; atol=0.1)
+
+            ≈
+        end
+    end
 end