1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
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;
}
};
|
