summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--stokes.hpp99
1 files changed, 99 insertions, 0 deletions
diff --git a/stokes.hpp b/stokes.hpp
new file mode 100644
index 0000000..b60ae0e
--- /dev/null
+++ b/stokes.hpp
@@ -0,0 +1,99 @@
+#include "stereographic.hpp"
+
+template <class Scalar>
+class Rope {
+ private:
+ std::vector<Vector<Scalar>> z;
+
+ template <int p>
+ Vector<Scalar> δz(const Tensor<Scalar, p>& J) const {
+ Vector<Scalar> dz(z.size());
+
+ for (unsigned i = 1; i < z.size() - 1; i++) {
+ Vector<Scalar> z12 = (z[i] + z[i - 1]) / 2.0;
+ Vector<Scalar> g;
+ std::tie(std::ignore, g, std::ignore) = stereographicHamGradHessHess(J, z12);
+
+ Vector<Scalar> Δ = z[i] - z[i - 1];
+
+ g.normalize();
+ Δ.normalize();
+
+ if (abs(arg(g.dot(Δ))) < M_PI / 2) {
+ dz[i] = g - Δ;
+ } else {
+ dz[i] = -g - Δ;
+ }
+ }
+
+ return dz;
+ }
+
+ template <int p>
+ void perturb(const Tensor<Scalar, p>& J, double δ) {
+ z += δ * this->δz(J);
+ }
+
+ void spread() {
+ std::vector<double> d(z.size() - 1);
+ double l = 0;
+
+ for (unsigned i= 0; i < z.size() - 1; i++) {
+ double Δ = (z[i + 1] - z[i]).norm();
+ d[i] = Δ;
+ l += Δ;
+ }
+
+ double a = 0;
+ unsigned pos = 0;
+
+ for (unsigned i = 1; i < z.size() - 1; i++) {
+ double b = i * l / (z.size() - 1);
+ while (b >= a) {
+ a += d[pos];
+ pos++;
+ }
+
+ z[i] = z[pos] - (a - b) * (z[pos] - z[pos - 1]).normalized();
+ }
+ }
+
+ public:
+ Rope(unsigned N, const Vector<Scalar>& z1, const Vector<Scalar>& z2) : z(N + 2) {
+ for (unsigned i = 0; i < N + 2; i++) {
+ z[i] = z1 + (z2 - z1) * ((double)i / (N + 1.0));
+ }
+ }
+
+ template <int p>
+ void step(const Tensor<Scalar, p>& J, double δ) {
+ this->perturb(J, δ);
+ this->spread();
+ }
+
+ template <int p>
+ double cost(const Tensor<Scalar, p>& J) const {
+ return this->δz(J).norm();
+ }
+
+ template <int p>
+ void relax(const Tensor<Scalar, p>& J, unsigned N, double δ) {
+ for (unsigned i = 0; i < N; i++) {
+ this->step(J, δ);
+ }
+ }
+
+ Rope<Scalar> interpolate() const {
+ Rope<Scalar> r(2 * z.size() - 1);
+
+ for (unsigned i = 0; i < z.size(); i++) {
+ r.z[2 * i] = z[i];
+ }
+
+ for (unsigned i = 0; i < z.size() - 1; i++) {
+ r.z[2 * i + 1] = (z[i] + z[i + 1]) / 2.0;
+ }
+
+ return r;
+ }
+};