1 files changed, 45 insertions, 0 deletions
diff --git a/forceScheme.cpp b/forceScheme.cpp
new file mode 100644
index 0000000..3683c10
--- /dev/null
+++ b/forceScheme.cpp
@@ -0,0 +1,45 @@
+#include "mp.h"
+
+#include <algorithm>
+#include <vector>
+
+typedef arma::uvec V;
+
+arma::mat mp::forceScheme(const arma::mat &D,
+        arma::mat &Y,
+        size_t maxIter,
+        double tol,
+        double fraction)
+{
+    
+    arma::uword n = (arma::uword) Y.n_rows;
+    V i(n), j(n);
+    for (arma::uword k = 0; k < n; k++)
+        i[k] = j[k] = k;
+
+    double prev_delta_sum = 1. / 0.;
+    for (size_t iter = 0; iter < maxIter; iter++) {
+        double delta_sum = 0;
+
+        arma::shuffle(i);
+        for (V::iterator a = i.begin(); a != i.end(); a++) {
+            arma::shuffle(j);
+            for (V::iterator b = j.begin(); b != j.end(); b++) {
+                if (*a == *b)
+                    continue;
+
+                arma::rowvec direction(Y.row(*b) - Y.row(*a));
+                double d2 = std::max(arma::norm(direction, 2), mp::EPSILON);
+                double delta = (D(*a, *b) - d2) / fraction;
+                delta_sum += fabs(delta);
+                Y.row(*b) += delta * (direction / d2);
+            }
+        }
+
+        if (fabs(prev_delta_sum - delta_sum) < tol)
+            break;
+        prev_delta_sum = delta_sum;
+    }
+
+    return Y;
+}