1 files changed, 13 insertions, 12 deletions

diff --git a/topology.cpp b/topology.cpp
index 9e755c0..e89e3eb 100644
--- a/topology.cpp
+++ b/topology.cpp
@@ -44,11 +44,6 @@ Vector normalize(const Vector& x) {
 }
 
 class ConstraintModel {
-protected:
-  Vector minusE(const Vector& H) const {
-    return H - Vector::Constant(M, E);
-  }
-
 public:
   Vector x0;
   Real E;
@@ -67,7 +62,7 @@ public:
     Tensor t(M, N, N);
     Tensor ∂∂H = t.setConstant(0);
     Matrix ∂H  = Matrix::Zero(M, N);
-    Vector H   = minusE(Vector::Zero(M));
+    Vector H   = Vector::Zero(M);
     return {H, ∂H, ∂∂H};
   }
 
@@ -75,6 +70,12 @@ public:
     return v.head(N).dot(x0) / N;
   }
 
+  Real cost(const Vector& v) const {
+    Vector H;
+    std::tie(H, std::ignore, std::ignore) = H_∂H_∂∂H(v.head(N));
+    return 0.5 * (H - Vector::Constant(M, E)).squaredNorm();
+  }
+
   std::tuple<Vector, Matrix> ∂L_∂∂L(const Vector& v) const {
     Vector x = v.head(N);
     Real ω₀ = v(N);
@@ -84,7 +85,7 @@ public:
 
     Vector ∂L∂x = x0 + ω₀ * x + ∂H∂x.transpose() * ω₁;
     Real ∂L∂ω₀ = 0.5 * (x.squaredNorm() - N);
-    Vector ∂L∂ω₁ = H;
+    Vector ∂L∂ω₁ = H - Vector::Constant(M, E);
 
     Vector ∂L(N + 1 + M);
     ∂L << ∂L∂x, ∂L∂ω₀, ∂L∂ω₁;
@@ -102,13 +103,13 @@ public:
     return {∂L, ∂∂L};
   }
 
-  Vector newtonMethod(const Vector& v0, Real γ = 1) const {
+  Vector newtonMethod(const Vector& v0, Real γ = 1, Real ε = 1e-12) const {
     Vector v = v0;
 
     Vector ∂L;
     Matrix ∂∂L;
 
-    while (std::tie(∂L, ∂∂L) = ∂L_∂∂L(v), ∂L.squaredNorm() > 1e-10) {
+    while (std::tie(∂L, ∂∂L) = ∂L_∂∂L(v), ∂L.squaredNorm() > ε) {
       v -= γ * ∂∂L.partialPivLu().solve(∂L);
       v.head(N) = normalize(v.head(N)); // might as well stay on the sphere
     }
@@ -137,7 +138,7 @@ public:
   std::tuple<Vector, Matrix, Tensor> H_∂H_∂∂H(const Vector& x) const override {
     Tensor ∂∂H = J * x;
     Matrix ∂H = ∂∂H * x / 2;
-    Vector H = minusE(∂H * x / 6);
+    Vector H = ∂H * x / 6;
     return {H, ∂H, ∂∂H};
   }
 };
@@ -162,7 +163,7 @@ public:
   std::tuple<Vector, Matrix, Tensor> H_∂H_∂∂H(const Vector& x) const override {
     Tensor ∂∂H = J;
     Matrix ∂H = ∂∂H * x;
-    Vector H = minusE(∂H * x / 2);
+    Vector H = ∂H * x / 2;
     return {H, ∂H, ∂∂H};
   }
 };
@@ -225,7 +226,7 @@ int main(int argc, char* argv[]) {
       }
       ExtensiveQuadratic S(N, M, E, r);
       Vector v = S.newtonMethod(v0, γ);
-      std::cout << S.overlap(v) << std::endl;
+      std::cout << S.overlap(v) << " " << S.cost(v) << std::endl;
     }
   } else if (!count) {
     Vector x = Vector::Zero(N);