More experiments

include/game/motor_parameterization.h

@@ -241,27 +241,38 @@ struct Cayley {
   template <typename T>
   bool operator()(const T *x, const T *delta, T *x_plus_delta) const {
     using vsr::cga::Scalar;
+    using vsr::cga::Vector;
+    using vsr::cga::Bivector;
     using vsr::cga::Motor;
     using vsr::cga::DualLine;
     using vsr::cga::DirectionTrivector;
 
-    // std::cout << "Cayley" << std::endl;
-
-    T a[8];
-    for (int i = 0; i < 8; ++i) {
-      a[i] = x[i] + delta[i];
-    }
+    // Scalar<T> one{1.0};
+    // Scalar<T> two{2.0};
+    // Scalar<T> half{0.5};
+    // Motor<T> eps{0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0};
+    // Bivector<T> B{delta[0], delta[1], delta[2]};
+    // Bivector<T> Bp = Vector<T>{delta[3], delta[4], delta[5]}.duale();
+    // Motor<T> oneplusB = (one + B);
+    // Motor<T> oneminusB = (one - B);
+    // Motor<T> C = (oneplusB / oneminusB) + (two * eps / oneminusB) * (Bp / oneminusB);
 
-    DualLine<T> B(delta);
-    Motor<T> BB = B * B;
-    Scalar<T> S = BB;
-    DirectionTrivector<T> S4 = BB;
     Scalar<T> one{1.0};
+    Scalar<T> two{2.0};
     Scalar<T> half{0.5};
-
-    Motor<T> C = (one + B + B + BB) * (one - S + S4) / ((one - S) * (one - S));
-
-    Motor<T> M = C * Motor<T>(x) * half;
+    Motor<T> eps{0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0};
+    Motor<T> Bp = Motor<T>{0.0, delta[5], -delta[4], delta[3],0.0,0.0,0.0,0.0};
+    Motor<T> oneplusB = Motor<T>{1.0, delta[0], delta[1], delta[2],0.0,0.0,0.0,0.0};
+    Motor<T> oneminusB = !Motor<T>{1.0, -delta[0], -delta[1], -delta[2],0.0,0.0,0.0,0.0};
+    Motor<T> C = (oneplusB * oneminusB) + (two * eps * oneminusB * Bp * oneminusB);
+
+    // DualLine<T> B(delta);
+    // Motor<T> BB = B * B;
+    // Scalar<T> S = BB;
+    // DirectionTrivector<T> S4 = BB;
+    // Motor<T> C = (one + B + B + BB) * (one - S + S4) / ((one - S) * (one - S));
+
+    Motor<T> M = C * Motor<T>(x); 
 
     for (int i = 0; i < 8; ++i) {
       x_plus_delta[i] = M[i];