1 files changed, 28 insertions, 14 deletions

diff --git a/stokes.hpp b/stokes.hpp
index 0756dc3..3814cf4 100644
--- a/stokes.hpp
+++ b/stokes.hpp
@@ -22,9 +22,9 @@ Vector<Scalar> variation(const Vector<Scalar>& z, const Vector<Scalar>& z´, con
         Matrix<Scalar>::Identity(ddH.rows(), ddH.cols()) - z.conjugate() * z.transpose() / z²
       );
 
-  Vector<Scalar> dLdz = - Reż·z´ * (
+  Vector<Scalar> dLdz = - (
         dżc * z´ + dż * z´.conjugate() - (dż * ż.conjugate() + dżc * ż) * Reż·z´ / ż²
-      ) / (ż² * z´²);
+      ) / sqrt(ż² * z´²) / 2;
 
   Vector<Scalar> ż´ = -(ddH * z´).conjugate() + ((ddH * z´).dot(z) / z²) * z.conjugate() + (
         dH.dot(z) * z´.conjugate() + dH.dot(z´) * z.conjugate() - (
@@ -35,17 +35,16 @@ Vector<Scalar> variation(const Vector<Scalar>& z, const Vector<Scalar>& z´, con
   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´ * (
+      - 0.5 * (
         (ż.dot(ż´) + ż´.dot(ż)) / ż² + (z´´.dot(z´) + z´.dot(z´´)) / z´²
         ) * (ż.conjugate() - Reż·z´ / z´² * z´.conjugate())
-      ) / (ż² * z´²);
+      ) / sqrt(ż² * z´²) / 2;
 
   return dLdz - ddtdLdz´;
 }
@@ -55,6 +54,27 @@ class Rope {
   public:
     std::vector<Vector<Scalar>> z;
 
+    Rope(unsigned N) : z(N + 2) {}
+
+    template <int p>
+    Rope(unsigned N, const Vector<Scalar>& z1, const Vector<Scalar>& z2, const Tensor<Scalar, p>& J) : z(N + 2) {
+      Scalar H1, H2;
+      std::tie(H1, std::ignore, std::ignore) = hamGradHess(J, z1);
+      std::tie(H2, std::ignore, std::ignore) = hamGradHess(J, z2);
+
+      if (real(H1) > real(H2)) {
+        z.front() = z1;
+        z.back() = z2;
+      } else {
+        z.front() = z2;
+        z.back() = z1;
+      }
+
+      for (unsigned i = 0; i < N + 2; i++) {
+        z[i] = normalize(z.front() + (z.back() - z.front()) * ((double)i / (N + 1.0)));
+      }
+    }
+
     unsigned n() const {
       return z.size() - 2;
     }
@@ -89,12 +109,6 @@ class Rope {
       return sqrt(err);
     }
 
-    Rope(unsigned N, const Vector<Scalar>& z1, const Vector<Scalar>& z2) : z(N + 2) {
-      for (unsigned i = 0; i < N + 2; i++) {
-        z[i] = normalize(z1 + (z2 - z1) * ((double)i / (N + 1.0)));
-      }
-    }
-
     Vector<Scalar> dz(unsigned i) const {
       return z[i + 1] - z[i - 1];
     }
@@ -208,14 +222,14 @@ class Rope {
 
         Vector<Scalar> zD = zDot(z[i], dH);
 
-        c += 1.0 - pow(real(zD.dot(dz(i))), 2) / zD.squaredNorm() / dz(i).squaredNorm();
+        c += 1.0 - real(zD.dot(dz(i))) / zD.norm() / dz(i).norm();
       }
 
       return c;
     }
 
     Rope<Scalar> interpolate() const {
-      Rope<Scalar> r(2 * n() + 1, z[0], z[z.size() - 1]);
+      Rope<Scalar> r(2 * n() + 1);
 
       for (unsigned i = 0; i < z.size(); i++) {
         r.z[2 * i] = z[i];