Got the variation calculation fixed and working.

2 files changed, 46 insertions, 16 deletions

diff --git a/stokes.hpp b/stokes.hpp
index 43a69e3..6a00637 100644
--- a/stokes.hpp
+++ b/stokes.hpp
@@ -21,32 +21,37 @@ Vector<Scalar> variation(const Vector<Scalar>& z, const Vector<Scalar>& z´, con
   Vector<Scalar> ż = zDot(z, dH);
   double ż² = ż.squaredNorm();
 
-  double Reż·z´² = real(pow(ż.dot(z´), 2));
+  double Reż·z´ = real(ż.dot(z´));
 
   Matrix<Scalar> dż = (dH.conjugate() - (dH.dot(z) / z²) * z.conjugate()) * z.adjoint() / z²;
   Matrix<Scalar> dżc = -ddH + (ddH * z.conjugate()) * z.transpose() / z²
-    + (z.dot(dH) / z²) * (Matrix<Scalar>::Identity(ddH.rows(), ddH.cols()) - z.conjugate() * z.transpose() / z²);
+    + (z.dot(dH) / z²) * (
+        Matrix<Scalar>::Identity(ddH.rows(), ddH.cols()) - z.conjugate() * z.transpose() / z²
+      );
 
-  Vector<Scalar> dLdz = - 0.5 * (
-        ż.dot(z´) * (dżc * z´) + z´.dot(ż) * (dż * z´.conjugate())
-        - (dż * ż.conjugate() + dżc * ż) * Reż·z´² / ż²
+  Vector<Scalar> dLdz = - Reż·z´ * (
+        dżc * z´ + dż * z´.conjugate() - (dż * ż.conjugate() + dżc * ż) * Reż·z´ / ż²
       ) / (ż² * z´²);
 
   Vector<Scalar> ż´ = -(ddH * z´).conjugate() + ((ddH * z´).dot(z) / z²) * z.conjugate() + (
-        dH.dot(z) * z´.conjugate() + dH.dot(z´) * z.conjugate() - (dH.dot(z) * (z´.dot(z) + z.dot(z´)) / z²) * z.conjugate()
+        dH.dot(z) * z´.conjugate() + dH.dot(z´) * z.conjugate() - (
+          dH.dot(z) * (z´.dot(z) + z.dot(z´)) / z²
+        ) * z.conjugate()
       ) / z²;
 
-  Vector<Scalar> ddtdLdz´ = -0.5 * (
-      ż´.dot(z´) * ż.conjugate() + ż.dot(z´´) * ż.conjugate() + ż.dot(z´) * ż´.conjugate()
-      - (
-        Reż·z´² * z´´.conjugate() + real(2.0 * ż.dot(z´) * (ż.dot(z´´) + ż´.dot(z´))) * z´.conjugate()
-        - Reż·z´² * (z´´.dot(z´) + z´.dot(z´´)) * z´.conjugate() / z´²
-        ) / z´²
-      - (
-        (ż.dot(ż´) + ż´.dot(ż)) / ż² + (z´´.dot(z´) + z´.dot(z´´)) / z´²
-        ) * (
-        ż.dot(z´) * ż.conjugate() - Reż·z´² / z´² * z´.conjugate()
+  double dReż·z´ = real(ż.dot(z´´) + ż´.dot(z´));
+
+  Vector<Scalar> ddtdLdz´ = - (
+      Reż·z´ * (
+        ż´.conjugate() - (
+          Reż·z´ * z´´.conjugate() + dReż·z´ * z´.conjugate()
+          - (Reż·z´ / z´²) * (z´´.dot(z´) + z´.dot(z´´)) * z´.conjugate() 
+          ) / z´²
         )
+      + dReż·z´ * (ż.conjugate() - Reż·z´ / z´² * z´.conjugate())
+      - Reż·z´ * (
+        (ż.dot(ż´) + ż´.dot(ż)) / ż² + (z´´.dot(z´) + z´.dot(z´´)) / z´²
+        ) * (ż.conjugate() - Reż·z´ / z´² * z´.conjugate())
       ) / (ż² * z´²);
 
   return dLdz - ddtdLdz´;
diff --git a/stokes_test.cpp b/stokes_test.cpp
new file mode 100644
index 0000000..7d96c8c
--- /dev/null
+++ b/stokes_test.cpp
@@ -0,0 +1,25 @@
+#include "stokes.hpp"
+
+using namespace std::complex_literals;
+
+int main() {
+
+  Vector<std::complex<double>> z(2), dz(2), ddz(2), dH(2);
+  Matrix<std::complex<double>> ddH(2, 2);
+
+  z << -0.75067 - 0.190132 * 1i, -0.625994 + 0.665987 * 1i;
+  dz << -0.0636149 - 0.469166 * 1i, 0.820037 - 0.449064 * 1i;
+  ddz << 0.55777 + 0.730164 * 1i, 0.361959 - 0.463245 * 1i;
+  dH << 0.967613 - 0.907519 * 1i, 0.712336 - 0.649056 * 1i;
+
+  ddH << -0.371925 - 0.280788 * 1i, -0.163888 - 0.141297 * 1i,
+         -0.163888 - 0.141297 * 1i, 0.230969 + 0.942449 * 1i;
+
+  std::cout << zDot(z, dH) << std::endl << std::endl;
+
+  std::cout << segmentCost(z, dz, dH) << std::endl << std::endl;
+
+  std::cout << variation(z, dz, ddz, dH, ddH) << std::endl;
+
+  return 0;
+}