Skip to content

Commit 3382ad6

Browse files
schochasticsschochastics
committed
refactored and reformatted
1 parent 0d7a81e commit 3382ad6

1 file changed

Lines changed: 162 additions & 119 deletions

File tree

‎src/stress.cpp‎

Lines changed: 162 additions & 119 deletions
Original file line numberDiff line numberDiff line change
@@ -1,190 +1,233 @@
11
#include <Rcpp.h>
2+
#include <cmath>
3+
#include <algorithm>
4+
25
using namespace Rcpp;
36

4-
// [[Rcpp::export]]
5-
double stress(NumericMatrix x, NumericMatrix W, NumericMatrix D) {
6-
double fct = 0;
7-
int n = x.nrow();
8-
for (int i = 0; i < (n - 1); ++i) {
9-
for (int j = (i + 1); j < n; ++j) {
10-
double denom = sqrt((x(i, 0) - x(j, 0)) * (x(i, 0) - x(j, 0)) +
11-
(x(i, 1) - x(j, 1)) * (x(i, 1) - x(j, 1)));
12-
fct += W(i, j) * (denom - D(i, j)) * (denom - D(i, j));
7+
namespace {
8+
inline double safe_inv_dist(double dx, double dy, double eps = 1e-10) {
9+
const double d2 = dx*dx + dy*dy;
10+
if (d2 <= eps) return 0.0;
11+
return 1.0 / std::sqrt(d2);
12+
}
13+
14+
inline double stress2d(const NumericMatrix& x,
15+
const NumericMatrix& W,
16+
const NumericMatrix& D) {
17+
double fct = 0.0;
18+
const int n = x.nrow();
19+
for (int i = 0; i < n - 1; ++i) {
20+
const double xi0 = x(i, 0), xi1 = x(i, 1);
21+
for (int j = i + 1; j < n; ++j) {
22+
const double dx = xi0 - x(j, 0);
23+
const double dy = xi1 - x(j, 1);
24+
const double dist = std::sqrt(dx*dx + dy*dy);
25+
const double r = dist - D(i, j);
26+
fct += W(i, j) * r * r;
27+
}
1328
}
29+
return fct;
1430
}
15-
return fct;
31+
}
32+
33+
// [[Rcpp::export]]
34+
double stress(NumericMatrix x, NumericMatrix W, NumericMatrix D) {
35+
return stress2d(x, W, D);
1636
}
1737

1838
// [[Rcpp::export]]
1939
NumericMatrix stress_major(NumericMatrix y, NumericMatrix W, NumericMatrix D,
2040
int iter, double tol) {
21-
int n = y.nrow();
22-
23-
NumericMatrix x(clone(y));
41+
const int n = y.nrow();
42+
NumericMatrix x = clone(y);
2443

2544
NumericVector wsum = rowSums(W);
2645

27-
double stress_old = stress(x, W, D);
28-
NumericMatrix xnew(n, 2); // out or in?
46+
double stress_old = stress2d(x, W, D);
47+
NumericMatrix xnew(n, 2);
48+
2949
for (int k = 0; k < iter; ++k) {
30-
std::fill(xnew.begin(), xnew.end(), 0);
50+
std::fill(xnew.begin(), xnew.end(), 0.0);
51+
3152
for (int i = 0; i < n; ++i) {
53+
const double xi0 = x(i, 0), xi1 = x(i, 1);
54+
double acc0 = 0.0, acc1 = 0.0;
55+
3256
for (int j = 0; j < n; ++j) {
33-
if (i != j) {
34-
double denom = sqrt((x(i, 0) - x(j, 0)) * (x(i, 0) - x(j, 0)) +
35-
(x(i, 1) - x(j, 1)) * (x(i, 1) - x(j, 1)));
36-
if (denom > 0.00001) {
37-
xnew(i, 0) +=
38-
W(i, j) * (x(j, 0) + D(i, j) * (x(i, 0) - x(j, 0)) / denom);
39-
xnew(i, 1) +=
40-
W(i, j) * (x(j, 1) + D(i, j) * (x(i, 1) - x(j, 1)) / denom);
41-
}
42-
}
57+
if (i == j) continue;
58+
59+
const double xj0 = x(j, 0), xj1 = x(j, 1);
60+
const double dx = xi0 - xj0;
61+
const double dy = xi1 - xj1;
62+
const double invd = safe_inv_dist(dx, dy, 1e-10);
63+
if (invd == 0.0) continue;
64+
65+
const double wij = W(i, j);
66+
const double dij = D(i, j);
67+
68+
acc0 += wij * (xj0 + dij * dx * invd);
69+
acc1 += wij * (xj1 + dij * dy * invd);
4370
}
44-
xnew(i, 0) = xnew(i, 0) / wsum[i];
45-
xnew(i, 1) = xnew(i, 1) / wsum[i];
71+
72+
const double denom = wsum[i];
73+
xnew(i, 0) = acc0 / denom;
74+
xnew(i, 1) = acc1 / denom;
75+
}
76+
77+
const double stress_new = stress2d(xnew, W, D);
78+
79+
80+
if (stress_old <= 0.0) {
81+
x = clone(xnew);
82+
break;
4683
}
47-
double stress_new = stress(xnew, W, D);
48-
double eps = (stress_old - stress_new) / stress_old;
4984

85+
const double eps = (stress_old - stress_new) / stress_old;
5086
if (eps <= tol) {
87+
88+
x = clone(xnew);
5189
break;
5290
}
91+
5392
stress_old = stress_new;
5493
x = clone(xnew);
5594
}
95+
5696
return x;
5797
}
5898

5999
// [[Rcpp::export]]
60100
NumericMatrix stress_radii(NumericMatrix y, NumericMatrix W, NumericMatrix D,
61101
NumericVector r, NumericVector tseq) {
62-
int n = y.nrow();
63-
int m = tseq.length();
102+
const int n = y.nrow();
103+
const int m = tseq.size();
64104

65-
NumericMatrix x(n, 2);
66-
for (int i = 0; i < n; ++i) {
67-
for (int d = 0; d < 2; ++d) {
68-
x(i, d) = y(i, d);
69-
}
70-
}
105+
NumericMatrix x = clone(y);
71106

72107
NumericVector wsum(n);
73108
for (int i = 0; i < n; ++i) {
74-
for (int j = 0; j < n; ++j) {
75-
wsum[i] += W(i, j);
76-
}
109+
double s = 0.0;
110+
for (int j = 0; j < n; ++j) s += W(i, j);
111+
wsum[i] = s;
77112
}
78113

79114
NumericVector rpow(n);
80115
for (int i = 0; i < n; ++i) {
81-
rpow[i] = 1 / (r[i] * r[i]);
116+
const double ri = r[i];
117+
rpow[i] = 1.0 / (ri * ri);
82118
}
83119

120+
NumericMatrix xnew(n, 2);
121+
84122
for (int s = 0; s < m; ++s) {
85-
double t = tseq[s];
86-
for (int k = 0; k < 1; ++k) {
87-
NumericMatrix xnew(n, 2); // out or in?
88-
for (int i = 0; i < n; ++i) {
89-
for (int j = 0; j < n; ++j) {
90-
if (i != j) {
91-
double bij = sqrt((x(i, 0) - x(j, 0)) * (x(i, 0) - x(j, 0)) +
92-
(x(i, 1) - x(j, 1)) * (x(i, 1) - x(j, 1)));
93-
double ai = sqrt(x(i, 0) * x(i, 0) + x(i, 1) * x(i, 1));
94-
if (ai < 0.0001) {
95-
ai = 0;
96-
} else {
97-
ai = 1 / ai;
98-
}
99-
if (bij < 0.0001) {
100-
bij = 0;
101-
} else {
102-
bij = 1 / bij;
103-
}
104-
xnew(i, 0) += (1 - t) * W(i, j) *
105-
(x(j, 0) + D(i, j) * (x(i, 0) - x(j, 0)) * bij) +
106-
t * rpow[i] * (r[i] * x(i, 0) * ai);
107-
xnew(i, 1) += (1 - t) * W(i, j) *
108-
(x(j, 1) + D(i, j) * (x(i, 1) - x(j, 1)) * bij) +
109-
t * rpow[i] * (r[i] * x(i, 1) * ai);
110-
}
111-
}
112-
xnew(i, 0) = xnew(i, 0) / ((1 - t) * wsum[i] + t * rpow[i]);
113-
xnew(i, 1) = xnew(i, 1) / ((1 - t) * wsum[i] + t * rpow[i]);
114-
}
115-
for (int i = 0; i < n; ++i) {
116-
x(i, 0) = xnew(i, 0);
117-
x(i, 1) = xnew(i, 1);
123+
const double t = tseq[s];
124+
std::fill(xnew.begin(), xnew.end(), 0.0);
125+
126+
for (int i = 0; i < n; ++i) {
127+
const double xi0 = x(i, 0), xi1 = x(i, 1);
128+
129+
const double inv_ai = safe_inv_dist(xi0, xi1, 1e-10);
130+
131+
double acc0 = 0.0, acc1 = 0.0;
132+
133+
for (int j = 0; j < n; ++j) {
134+
if (i == j) continue;
135+
136+
const double xj0 = x(j, 0), xj1 = x(j, 1);
137+
const double dx = xi0 - xj0;
138+
const double dy = xi1 - xj1;
139+
const double inv_bij = safe_inv_dist(dx, dy, 1e-10);
140+
141+
const double wij = W(i, j);
142+
const double dij = D(i, j);
143+
144+
if (inv_bij != 0.0) {
145+
acc0 += (1.0 - t) * wij * (xj0 + dij * dx * inv_bij);
146+
acc1 += (1.0 - t) * wij * (xj1 + dij * dy * inv_bij);
147+
}
148+
149+
acc0 += t * rpow[i] * (r[i] * xi0 * inv_ai);
150+
acc1 += t * rpow[i] * (r[i] * xi1 * inv_ai);
118151
}
152+
153+
const double denom = (1.0 - t) * wsum[i] + t * rpow[i];
154+
xnew(i, 0) = acc0 / denom;
155+
xnew(i, 1) = acc1 / denom;
119156
}
157+
158+
x = clone(xnew);
120159
}
160+
121161
return x;
122162
}
123163

124164
// [[Rcpp::export]]
125165
NumericMatrix stress_focus(NumericMatrix y, NumericMatrix W, NumericMatrix D,
126166
NumericMatrix Z, NumericVector tseq, int iter,
127167
double tol) {
128-
int n = y.nrow();
129-
int m = tseq.length();
168+
const int n = y.nrow();
169+
const int m = tseq.size();
130170

131-
NumericMatrix x(n, 2);
132-
for (int i = 0; i < n; ++i) {
133-
for (int d = 0; d < 2; ++d) {
134-
x(i, d) = y(i, d);
135-
}
136-
}
171+
NumericMatrix x = clone(y);
137172

138-
NumericVector wsum(n);
139-
NumericVector zsum(n);
173+
NumericVector wsum(n), zsum(n);
140174
for (int i = 0; i < n; ++i) {
175+
double sw = 0.0, sz = 0.0;
141176
for (int j = 0; j < n; ++j) {
142-
wsum[i] += W(i, j);
143-
zsum[i] += Z(i, j);
177+
sw += W(i, j);
178+
sz += Z(i, j);
144179
}
180+
wsum[i] = sw;
181+
zsum[i] = sz;
145182
}
146183

147-
double stress_oldW = stress(x, W, D);
148-
// double stress_oldZ = stress(x,Z,D);
149-
double stress_old = stress_oldW;
184+
double stress_old = stress2d(x, W, D);
185+
NumericMatrix xnew(n, 2);
186+
150187
for (int s = 0; s < m; ++s) {
151-
double t = tseq[s];
188+
const double t = tseq[s];
152189

153190
for (int k = 0; k < iter; ++k) {
154-
NumericMatrix xnew(n, 2); // out or in?
191+
std::fill(xnew.begin(), xnew.end(), 0.0);
192+
155193
for (int i = 0; i < n; ++i) {
194+
const double xi0 = x(i, 0), xi1 = x(i, 1);
195+
double acc0 = 0.0, acc1 = 0.0;
196+
156197
for (int j = 0; j < n; ++j) {
157-
if (i != j) {
158-
double denom = sqrt((x(i, 0) - x(j, 0)) * (x(i, 0) - x(j, 0)) +
159-
(x(i, 1) - x(j, 1)) * (x(i, 1) - x(j, 1)));
160-
if (denom > 0.00001) {
161-
denom = 1 / denom;
162-
} else {
163-
denom = 0;
164-
}
165-
xnew(i, 0) += ((1 - t) * W(i, j) + t * Z(i, j)) *
166-
(x(j, 0) + D(i, j) * (x(i, 0) - x(j, 0)) * denom);
167-
xnew(i, 1) += ((1 - t) * W(i, j) + t * Z(i, j)) *
168-
(x(j, 1) + D(i, j) * (x(i, 1) - x(j, 1)) * denom);
169-
}
198+
if (i == j) continue;
199+
200+
const double xj0 = x(j, 0), xj1 = x(j, 1);
201+
const double dx = xi0 - xj0;
202+
const double dy = xi1 - xj1;
203+
const double invd = safe_inv_dist(dx, dy, 1e-10);
204+
205+
const double wij = (1.0 - t) * W(i, j) + t * Z(i, j);
206+
if (invd == 0.0) continue;
207+
208+
const double dij = D(i, j);
209+
acc0 += wij * (xj0 + dij * dx * invd);
210+
acc1 += wij * (xj1 + dij * dy * invd);
170211
}
171-
xnew(i, 0) = xnew(i, 0) / ((1 - t) * wsum[i] + t * zsum[i]);
172-
xnew(i, 1) = xnew(i, 1) / ((1 - t) * wsum[i] + t * zsum[i]);
173-
}
174-
double stress_newW = stress(xnew, W, D);
175-
double stress_newZ = stress(xnew, Z, D);
176-
double stress_new = (1 - t) * stress_newW + t * stress_newZ;
177212

178-
for (int i = 0; i < n; ++i) {
179-
x(i, 0) = xnew(i, 0);
180-
x(i, 1) = xnew(i, 1);
181-
}
182-
double eps = (stress_old - stress_new) / stress_old;
183-
if (eps <= tol) {
184-
break;
213+
const double denom = (1.0 - t) * wsum[i] + t * zsum[i];
214+
xnew(i, 0) = acc0 / denom;
215+
xnew(i, 1) = acc1 / denom;
185216
}
217+
218+
const double stress_newW = stress2d(xnew, W, D);
219+
const double stress_newZ = stress2d(xnew, Z, D);
220+
const double stress_new = (1.0 - t) * stress_newW + t * stress_newZ;
221+
222+
x = clone(xnew);
223+
224+
if (stress_old <= 0.0) break;
225+
const double eps = (stress_old - stress_new) / stress_old;
226+
if (eps <= tol) break;
227+
186228
stress_old = stress_new;
187229
}
188230
}
231+
189232
return x;
190-
}
233+
}

0 commit comments

Comments
 (0)