1 #ifndef FIELD_MATH_H_
2 #define FIELD_MATH_H_
3
4 #ifdef WITH_CUDA
5 # include <glm/glm.hpp>
6 #endif
7 #include <Eigen/Core>
8 #include <Eigen/Dense>
9 #include <algorithm>
10 #include <vector>
11
12 namespace qflow {
13
14 using namespace Eigen;
15
16 struct DEdge
17 {
DEdgeqflow::DEdge18 DEdge()
19 : x(0), y(0)
20 {}
DEdgeqflow::DEdge21 DEdge(int _x, int _y) {
22 if (_x > _y)
23 x = _y, y = _x;
24 else
25 x = _x, y = _y;
26 }
operator <qflow::DEdge27 bool operator<(const DEdge& e) const {
28 return (x < e.x) || (x == e.x && y < e.y);
29 }
operator ==qflow::DEdge30 bool operator==(const DEdge& e) const {
31 return x == e.x && y == e.y;
32 }
operator !=qflow::DEdge33 bool operator!=(const DEdge& e) const {
34 return x != e.x || y != e.y;
35 }
36 int x, y;
37 };
38
get_parents(std::vector<std::pair<int,int>> & parents,int j)39 inline int get_parents(std::vector<std::pair<int, int>>& parents, int j) {
40 if (j == parents[j].first) return j;
41 int k = get_parents(parents, parents[j].first);
42 parents[j].second = (parents[j].second + parents[parents[j].first].second) % 4;
43 parents[j].first = k;
44 return k;
45 }
46
get_parents_orient(std::vector<std::pair<int,int>> & parents,int j)47 inline int get_parents_orient(std::vector<std::pair<int, int>>& parents, int j) {
48 if (j == parents[j].first) return parents[j].second;
49 return (parents[j].second + get_parents_orient(parents, parents[j].first)) % 4;
50 }
51
fast_acos(double x)52 inline double fast_acos(double x) {
53 double negate = double(x < 0.0f);
54 x = std::abs(x);
55 double ret = -0.0187293f;
56 ret *= x;
57 ret = ret + 0.0742610f;
58 ret *= x;
59 ret = ret - 0.2121144f;
60 ret *= x;
61 ret = ret + 1.5707288f;
62 ret = ret * std::sqrt(1.0f - x);
63 ret = ret - 2.0f * negate * ret;
64 return negate * (double)M_PI + ret;
65 }
66
signum(double value)67 inline double signum(double value) { return std::copysign((double)1, value); }
68
69 /// Always-positive modulo function (assumes b > 0)
modulo(int a,int b)70 inline int modulo(int a, int b) {
71 int r = a % b;
72 return (r < 0) ? r + b : r;
73 }
74
rotate90_by(const Vector3d & q,const Vector3d & n,int amount)75 inline Vector3d rotate90_by(const Vector3d &q, const Vector3d &n, int amount) {
76 return ((amount & 1) ? (n.cross(q)) : q) * (amount < 2 ? 1.0f : -1.0f);
77 }
78
rshift90(Vector2i shift,int amount)79 inline Vector2i rshift90(Vector2i shift, int amount) {
80 if (amount & 1) shift = Vector2i(-shift.y(), shift.x());
81 if (amount >= 2) shift = -shift;
82 return shift;
83 }
84
compat_orientation_extrinsic_index_4(const Vector3d & q0,const Vector3d & n0,const Vector3d & q1,const Vector3d & n1)85 inline std::pair<int, int> compat_orientation_extrinsic_index_4(const Vector3d &q0,
86 const Vector3d &n0,
87 const Vector3d &q1,
88 const Vector3d &n1) {
89 const Vector3d A[2] = {q0, n0.cross(q0)};
90 const Vector3d B[2] = {q1, n1.cross(q1)};
91
92 double best_score = -std::numeric_limits<double>::infinity();
93 int best_a = 0, best_b = 0;
94
95 for (int i = 0; i < 2; ++i) {
96 for (int j = 0; j < 2; ++j) {
97 double score = std::abs(A[i].dot(B[j]));
98 if (score > best_score) {
99 best_a = i;
100 best_b = j;
101 best_score = score;
102 }
103 }
104 }
105
106 if (A[best_a].dot(B[best_b]) < 0) best_b += 2;
107
108 return std::make_pair(best_a, best_b);
109 }
110
compat_orientation_extrinsic_4(const Vector3d & q0,const Vector3d & n0,const Vector3d & q1,const Vector3d & n1)111 inline std::pair<Vector3d, Vector3d> compat_orientation_extrinsic_4(const Vector3d &q0,
112 const Vector3d &n0,
113 const Vector3d &q1,
114 const Vector3d &n1) {
115 const Vector3d A[2] = {q0, n0.cross(q0)};
116 const Vector3d B[2] = {q1, n1.cross(q1)};
117
118 double best_score = -std::numeric_limits<double>::infinity();
119 int best_a = 0, best_b = 0;
120
121 for (int i = 0; i < 2; ++i) {
122 for (int j = 0; j < 2; ++j) {
123 double score = std::abs(A[i].dot(B[j]));
124 if (score > best_score + 1e-6) {
125 best_a = i;
126 best_b = j;
127 best_score = score;
128 }
129 }
130 }
131
132 const double dp = A[best_a].dot(B[best_b]);
133 return std::make_pair(A[best_a], B[best_b] * signum(dp));
134 }
135
middle_point(const Vector3d & p0,const Vector3d & n0,const Vector3d & p1,const Vector3d & n1)136 inline Vector3d middle_point(const Vector3d &p0, const Vector3d &n0, const Vector3d &p1,
137 const Vector3d &n1) {
138 /* How was this derived?
139 *
140 * Minimize \|x-p0\|^2 + \|x-p1\|^2, where
141 * dot(n0, x) == dot(n0, p0)
142 * dot(n1, x) == dot(n1, p1)
143 *
144 * -> Lagrange multipliers, set derivative = 0
145 * Use first 3 equalities to write x in terms of
146 * lambda_1 and lambda_2. Substitute that into the last
147 * two equations and solve for the lambdas. Finally,
148 * add a small epsilon term to avoid issues when n1=n2.
149 */
150 double n0p0 = n0.dot(p0), n0p1 = n0.dot(p1), n1p0 = n1.dot(p0), n1p1 = n1.dot(p1),
151 n0n1 = n0.dot(n1), denom = 1.0f / (1.0f - n0n1 * n0n1 + 1e-4f),
152 lambda_0 = 2.0f * (n0p1 - n0p0 - n0n1 * (n1p0 - n1p1)) * denom,
153 lambda_1 = 2.0f * (n1p0 - n1p1 - n0n1 * (n0p1 - n0p0)) * denom;
154
155 return 0.5f * (p0 + p1) - 0.25f * (n0 * lambda_0 + n1 * lambda_1);
156 }
157
position_floor_4(const Vector3d & o,const Vector3d & q,const Vector3d & n,const Vector3d & p,double scale_x,double scale_y,double inv_scale_x,double inv_scale_y)158 inline Vector3d position_floor_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
159 const Vector3d &p, double scale_x, double scale_y,
160 double inv_scale_x, double inv_scale_y) {
161 Vector3d t = n.cross(q);
162 Vector3d d = p - o;
163 return o + q * std::floor(q.dot(d) * inv_scale_x) * scale_x +
164 t * std::floor(t.dot(d) * inv_scale_y) * scale_y;
165 }
166
compat_position_extrinsic_4(const Vector3d & p0,const Vector3d & n0,const Vector3d & q0,const Vector3d & o0,const Vector3d & p1,const Vector3d & n1,const Vector3d & q1,const Vector3d & o1,double scale_x,double scale_y,double inv_scale_x,double inv_scale_y,double scale_x_1,double scale_y_1,double inv_scale_x_1,double inv_scale_y_1)167 inline std::pair<Vector3d, Vector3d> compat_position_extrinsic_4(
168 const Vector3d &p0, const Vector3d &n0, const Vector3d &q0, const Vector3d &o0,
169 const Vector3d &p1, const Vector3d &n1, const Vector3d &q1, const Vector3d &o1, double scale_x,
170 double scale_y, double inv_scale_x, double inv_scale_y, double scale_x_1, double scale_y_1,
171 double inv_scale_x_1, double inv_scale_y_1) {
172 Vector3d t0 = n0.cross(q0), t1 = n1.cross(q1);
173 Vector3d middle = middle_point(p0, n0, p1, n1);
174 Vector3d o0p =
175 position_floor_4(o0, q0, n0, middle, scale_x, scale_y, inv_scale_x, inv_scale_y);
176 Vector3d o1p =
177 position_floor_4(o1, q1, n1, middle, scale_x_1, scale_y_1, inv_scale_x_1, inv_scale_y_1);
178
179 double best_cost = std::numeric_limits<double>::infinity();
180 int best_i = -1, best_j = -1;
181
182 for (int i = 0; i < 4; ++i) {
183 Vector3d o0t = o0p + (q0 * (i & 1) * scale_x + t0 * ((i & 2) >> 1) * scale_y);
184 for (int j = 0; j < 4; ++j) {
185 Vector3d o1t = o1p + (q1 * (j & 1) * scale_x_1 + t1 * ((j & 2) >> 1) * scale_y_1);
186 double cost = (o0t - o1t).squaredNorm();
187
188 if (cost < best_cost) {
189 best_i = i;
190 best_j = j;
191 best_cost = cost;
192 }
193 }
194 }
195
196 return std::make_pair(
197 o0p + (q0 * (best_i & 1) * scale_x + t0 * ((best_i & 2) >> 1) * scale_y),
198 o1p + (q1 * (best_j & 1) * scale_x_1 + t1 * ((best_j & 2) >> 1) * scale_y_1));
199 }
200
position_round_4(const Vector3d & o,const Vector3d & q,const Vector3d & n,const Vector3d & p,double scale_x,double scale_y,double inv_scale_x,double inv_scale_y)201 inline Vector3d position_round_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
202 const Vector3d &p, double scale_x, double scale_y,
203 double inv_scale_x, double inv_scale_y) {
204 Vector3d t = n.cross(q);
205 Vector3d d = p - o;
206 return o + q * std::round(q.dot(d) * inv_scale_x) * scale_x +
207 t * std::round(t.dot(d) * inv_scale_y) * scale_y;
208 }
209
position_floor_index_4(const Vector3d & o,const Vector3d & q,const Vector3d & n,const Vector3d & p,double,double,double inv_scale_x,double inv_scale_y)210 inline Vector2i position_floor_index_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
211 const Vector3d &p, double /* unused */, double /* unused */,
212 double inv_scale_x, double inv_scale_y) {
213 Vector3d t = n.cross(q);
214 Vector3d d = p - o;
215 return Vector2i((int)std::floor(q.dot(d) * inv_scale_x),
216 (int)std::floor(t.dot(d) * inv_scale_y));
217 }
218
compat_position_extrinsic_index_4(const Vector3d & p0,const Vector3d & n0,const Vector3d & q0,const Vector3d & o0,const Vector3d & p1,const Vector3d & n1,const Vector3d & q1,const Vector3d & o1,double scale_x,double scale_y,double inv_scale_x,double inv_scale_y,double scale_x_1,double scale_y_1,double inv_scale_x_1,double inv_scale_y_1,double * error)219 inline std::pair<Vector2i, Vector2i> compat_position_extrinsic_index_4(
220 const Vector3d &p0, const Vector3d &n0, const Vector3d &q0, const Vector3d &o0,
221 const Vector3d &p1, const Vector3d &n1, const Vector3d &q1, const Vector3d &o1, double scale_x,
222 double scale_y, double inv_scale_x, double inv_scale_y, double scale_x_1, double scale_y_1,
223 double inv_scale_x_1, double inv_scale_y_1, double *error) {
224 Vector3d t0 = n0.cross(q0), t1 = n1.cross(q1);
225 Vector3d middle = middle_point(p0, n0, p1, n1);
226 Vector2i o0p =
227 position_floor_index_4(o0, q0, n0, middle, scale_x, scale_y, inv_scale_x, inv_scale_y);
228 Vector2i o1p = position_floor_index_4(o1, q1, n1, middle, scale_x_1, scale_y_1, inv_scale_x_1,
229 inv_scale_y_1);
230
231 double best_cost = std::numeric_limits<double>::infinity();
232 int best_i = -1, best_j = -1;
233
234 for (int i = 0; i < 4; ++i) {
235 Vector3d o0t =
236 o0 + (q0 * ((i & 1) + o0p[0]) * scale_x + t0 * (((i & 2) >> 1) + o0p[1]) * scale_y);
237 for (int j = 0; j < 4; ++j) {
238 Vector3d o1t = o1 + (q1 * ((j & 1) + o1p[0]) * scale_x_1 +
239 t1 * (((j & 2) >> 1) + o1p[1]) * scale_y_1);
240 double cost = (o0t - o1t).squaredNorm();
241
242 if (cost < best_cost) {
243 best_i = i;
244 best_j = j;
245 best_cost = cost;
246 }
247 }
248 }
249 if (error) *error = best_cost;
250
251 return std::make_pair(Vector2i((best_i & 1) + o0p[0], ((best_i & 2) >> 1) + o0p[1]),
252 Vector2i((best_j & 1) + o1p[0], ((best_j & 2) >> 1) + o1p[1]));
253 }
254
coordinate_system(const Vector3d & a,Vector3d & b,Vector3d & c)255 inline void coordinate_system(const Vector3d &a, Vector3d &b, Vector3d &c) {
256 if (std::abs(a.x()) > std::abs(a.y())) {
257 double invLen = 1.0f / std::sqrt(a.x() * a.x() + a.z() * a.z());
258 c = Vector3d(a.z() * invLen, 0.0f, -a.x() * invLen);
259 } else {
260 double invLen = 1.0f / std::sqrt(a.y() * a.y() + a.z() * a.z());
261 c = Vector3d(0.0f, a.z() * invLen, -a.y() * invLen);
262 }
263 b = c.cross(a);
264 }
265
rotate_vector_into_plane(Vector3d q,const Vector3d & source_normal,const Vector3d & target_normal)266 inline Vector3d rotate_vector_into_plane(Vector3d q, const Vector3d &source_normal,
267 const Vector3d &target_normal) {
268 const double cosTheta = source_normal.dot(target_normal);
269 if (cosTheta < 0.9999f) {
270 if (cosTheta < -0.9999f) return -q;
271 Vector3d axis = source_normal.cross(target_normal);
272 q = q * cosTheta + axis.cross(q) +
273 axis * (axis.dot(q) * (1.0 - cosTheta) / axis.dot(axis));
274 }
275 return q;
276 }
277
Travel(Vector3d p,const Vector3d & dir,double & len,int & f,VectorXi & E2E,MatrixXd & V,MatrixXi & F,MatrixXd & NF,std::vector<MatrixXd> & triangle_space,double * tx=0,double * ty=0)278 inline Vector3d Travel(Vector3d p, const Vector3d &dir, double &len, int &f, VectorXi &E2E,
279 MatrixXd &V, MatrixXi &F, MatrixXd &NF,
280 std::vector<MatrixXd> &triangle_space, double *tx = 0, double *ty = 0) {
281 Vector3d N = NF.col(f);
282 Vector3d pt = (dir - dir.dot(N) * N).normalized();
283 int prev_id = -1;
284 int count = 0;
285 while (len > 0) {
286 count += 1;
287 Vector3d t1 = V.col(F(1, f)) - V.col(F(0, f));
288 Vector3d t2 = V.col(F(2, f)) - V.col(F(0, f));
289 Vector3d N = NF.col(f);
290 // printf("point dis: %f\n", (p - V.col(F(1, f))).dot(N));
291 int edge_id = f * 3;
292 double max_len = 1e30;
293 bool found = false;
294 int next_id, next_f;
295 Vector3d next_q;
296 Matrix3d m, n;
297 m.col(0) = t1;
298 m.col(1) = t2;
299 m.col(2) = N;
300 n = m.inverse();
301 MatrixXd &T = triangle_space[f];
302 VectorXd coord = T * Vector3d(p - V.col(F(0, f)));
303 VectorXd dirs = (T * pt);
304
305 double lens[3];
306 lens[0] = -coord.y() / dirs.y();
307 lens[1] = (1 - coord.x() - coord.y()) / (dirs.x() + dirs.y());
308 lens[2] = -coord.x() / dirs.x();
309 for (int fid = 0; fid < 3; ++fid) {
310 if (fid + edge_id == prev_id) continue;
311
312 if (lens[fid] >= 0 && lens[fid] < max_len) {
313 max_len = lens[fid];
314 next_id = E2E[edge_id + fid];
315 next_f = next_id;
316 if (next_f != -1) next_f /= 3;
317 found = true;
318 }
319 }
320 if (!found) {
321 printf("error...\n");
322 exit(0);
323 }
324 // printf("status: %f %f %d\n", len, max_len, f);
325 if (max_len >= len) {
326 if (tx && ty) {
327 *tx = coord.x() + dirs.x() * len;
328 *ty = coord.y() + dirs.y() * len;
329 }
330 p = p + len * pt;
331 len = 0;
332 return p;
333 }
334 p = V.col(F(0, f)) + t1 * (coord.x() + dirs.x() * max_len) +
335 t2 * (coord.y() + dirs.y() * max_len);
336 len -= max_len;
337 if (next_f == -1) {
338 if (tx && ty) {
339 *tx = coord.x() + dirs.x() * max_len;
340 *ty = coord.y() + dirs.y() * max_len;
341 }
342 return p;
343 }
344 pt = rotate_vector_into_plane(pt, NF.col(f), NF.col(next_f));
345 f = next_f;
346 prev_id = next_id;
347 }
348 return p;
349 }
TravelField(Vector3d p,Vector3d & pt,double & len,int & f,VectorXi & E2E,MatrixXd & V,MatrixXi & F,MatrixXd & NF,MatrixXd & QF,MatrixXd & QV,MatrixXd & NV,std::vector<MatrixXd> & triangle_space,double * tx=0,double * ty=0,Vector3d * dir_unfold=0)350 inline Vector3d TravelField(Vector3d p, Vector3d &pt, double &len, int &f, VectorXi &E2E,
351 MatrixXd &V, MatrixXi &F, MatrixXd &NF, MatrixXd &QF, MatrixXd &QV,
352 MatrixXd &NV, std::vector<MatrixXd> &triangle_space, double *tx = 0,
353 double *ty = 0, Vector3d *dir_unfold = 0) {
354 Vector3d N = NF.col(f);
355 pt = (pt - pt.dot(N) * N).normalized();
356 int prev_id = -1;
357 int count = 0;
358 std::vector<Vector3d> Ns;
359
360 auto FaceQFromVertices = [&](int f, double tx, double ty) {
361 const Vector3d &n = NF.col(f);
362 const Vector3d &q_1 = QV.col(F(0, f)), &q_2 = QV.col(F(1, f)), &q_3 = QV.col(F(2, f));
363 const Vector3d &n_1 = NV.col(F(0, f)), &n_2 = NV.col(F(1, f)), &n_3 = NV.col(F(2, f));
364 Vector3d q_1n = rotate_vector_into_plane(q_1, n_1, n);
365 Vector3d q_2n = rotate_vector_into_plane(q_2, n_2, n);
366 Vector3d q_3n = rotate_vector_into_plane(q_3, n_3, n);
367 auto orient = compat_orientation_extrinsic_4(q_1n, n, q_2n, n);
368 Vector3d q = (orient.first * tx + orient.second * ty).normalized();
369 orient = compat_orientation_extrinsic_4(q, n, q_3n, n);
370 q = (orient.first * (tx + ty) + orient.second * (1 - tx - ty)).normalized();
371 return q;
372 };
373
374 auto BestQFromGivenQ = [&](const Vector3d &n, const Vector3d &q, const Vector3d &given_q) {
375 Vector3d q_1 = n.cross(q);
376 double t1 = q.dot(given_q);
377 double t2 = q_1.dot(given_q);
378 if (fabs(t1) > fabs(t2)) {
379 if (t1 > 0.0)
380 return Vector3d(q);
381 else
382 return Vector3d(-q);
383 } else {
384 if (t2 > 0.0)
385 return Vector3d(q_1);
386 else
387 return Vector3d(-q_1);
388 }
389 };
390
391 while (len > 0) {
392 count += 1;
393 Vector3d t1 = V.col(F(1, f)) - V.col(F(0, f));
394 Vector3d t2 = V.col(F(2, f)) - V.col(F(0, f));
395 Vector3d N = NF.col(f);
396 Ns.push_back(N);
397 // printf("point dis: %f\n", (p - V.col(F(1, f))).dot(N));
398 int edge_id = f * 3;
399 double max_len = 1e30;
400 bool found = false;
401 int next_id = -1, next_f = -1;
402 Vector3d next_q;
403 Matrix3d m, n;
404 m.col(0) = t1;
405 m.col(1) = t2;
406 m.col(2) = N;
407 n = m.inverse();
408 MatrixXd &T = triangle_space[f];
409 VectorXd coord = T * Vector3d(p - V.col(F(0, f)));
410 VectorXd dirs = (T * pt);
411 double lens[3];
412 lens[0] = -coord.y() / dirs.y();
413 lens[1] = (1 - coord.x() - coord.y()) / (dirs.x() + dirs.y());
414 lens[2] = -coord.x() / dirs.x();
415 for (int fid = 0; fid < 3; ++fid) {
416 if (fid + edge_id == prev_id) continue;
417
418 if (lens[fid] >= 0 && lens[fid] < max_len) {
419 max_len = lens[fid];
420 next_id = E2E[edge_id + fid];
421 next_f = next_id;
422 if (next_f != -1) next_f /= 3;
423 found = true;
424 }
425 }
426 double w1 = (coord.x() + dirs.x() * max_len);
427 double w2 = (coord.y() + dirs.y() * max_len);
428 if (w1 < 0) w1 = 0.0f;
429 if (w2 < 0) w2 = 0.0f;
430 if (w1 + w2 > 1) {
431 double w = w1 + w2;
432 w1 /= w;
433 w2 /= w;
434 }
435
436 if (!found) {
437 printf("error...\n");
438 exit(0);
439 }
440 // printf("status: %f %f %d\n", len, max_len, f);
441 if (max_len >= len) {
442 if (tx && ty) {
443 *tx = w1;
444 *ty = w2;
445 }
446 Vector3d ideal_q = FaceQFromVertices(f, *tx, *ty);
447 *dir_unfold = BestQFromGivenQ(NF.col(f), ideal_q, *dir_unfold);
448 for (int i = Ns.size() - 1; i > 0; --i) {
449 *dir_unfold = rotate_vector_into_plane(*dir_unfold, Ns[i], Ns[i - 1]);
450 }
451 p = p + len * pt;
452 len = 0;
453 return p;
454 }
455 p = V.col(F(0, f)) + t1 * w1 + t2 * w2;
456 len -= max_len;
457 if (next_f == -1) {
458 if (tx && ty) {
459 *tx = w1;
460 *ty = w2;
461 }
462 Vector3d ideal_q = FaceQFromVertices(f, *tx, *ty);
463 *dir_unfold = BestQFromGivenQ(NF.col(f), ideal_q, *dir_unfold);
464 for (int i = Ns.size() - 1; i > 0; --i) {
465 *dir_unfold = rotate_vector_into_plane(*dir_unfold, Ns[i], Ns[i - 1]);
466 }
467 return p;
468 }
469 pt = rotate_vector_into_plane(pt, NF.col(f), NF.col(next_f));
470 // pt = BestQFromGivenQ(NF.col(next_f), QF.col(next_f), pt);
471 if (dir_unfold) {
472 *dir_unfold = BestQFromGivenQ(NF.col(next_f), QF.col(next_f), *dir_unfold);
473 }
474 f = next_f;
475 prev_id = next_id;
476 }
477
478 return p;
479 }
480
481 } // namespace qflow
482
483 #endif
484