Line data Source code
1 : /*
2 : * Copyright (C) 2016 Intel Corporation. All rights reserved.
3 : *
4 : * This file is free software: you can redistribute it and/or modify it
5 : * under the terms of the GNU General Public License as published by the
6 : * Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This file is distributed in the hope that it will be useful, but
10 : * WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 : * See the GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License along
15 : * with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : /*
19 : * This comment section explains the basic idea behind the implementation.
20 : *
21 : * Vectors difference notation
22 : * ===========================
23 : * Let v and w be vectors. For readability purposes, unless explicitly
24 : * otherwise noted, the notation vw will be used to represent w - v.
25 : *
26 : * Relationship between a vector and a triangle in 3d space
27 : * ========================================================
28 : * Vector in the area of a triangle
29 : * --------------------------------
30 : * Let T = (a, b, c) be a triangle, where a, b and c are also vectors and
31 : * linearly independent. A vector inside that triangle can be written as one of
32 : * its vertices plus the sum of the positively scaled vectors from that vertex
33 : * to the other ones. Taking a as the first vertex, a vector p in the area
34 : * formed by T can be written as:
35 : *
36 : * p = a + w_ab * ab + w_ac * ac
37 : *
38 : * It's fairly easy to see that if p is in the area formed by T, then w_ab >= 0
39 : * and w_ac >= 0. That vector p can also be written as:
40 : *
41 : * p = b + w_ba * ba + w_bc * bc
42 : *
43 : * It's easy to check that the triangle formed by (a + w_ab * ab, b + w_ba *
44 : * ba, p) is similar to T and, with the correct algebraic manipulations, we can
45 : * come to the conclusion that:
46 : *
47 : * w_ba = 1 - w_ab - w_ac
48 : *
49 : * Since we know that w_ba >= 0, then w_ab + w_ac <= 1. Thus:
50 : *
51 : * ----------------------------------------------------------
52 : * | p = a + w_ab * ab + w_ac * ac is in the area of T iff: |
53 : * | w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1 |
54 : * ----------------------------------------------------------
55 : *
56 : * Proving backwards shouldn't be difficult.
57 : *
58 : * Vector p can also be written as:
59 : *
60 : * p = (1 - w_ab - w_ba) * a + w_ab * b + w_ba * c
61 : *
62 : *
63 : * Vector that crosses a triangle
64 : * ------------------------------
65 : * Let T be the same triangle discussed above and let v be a vector such that:
66 : *
67 : * v = x * a + y * b + z * c
68 : * where x >= 0, y >= 0, z >= 0, and x + y + z > 0.
69 : *
70 : * It's geometrically easy to see that v crosses the triangle T. But that can
71 : * also be verified analytically.
72 : *
73 : * The vector v crosses the triangle T iff there's a positive alpha such that
74 : * alpha * v is in the area formed by T, so we need to prove that such value
75 : * exists. To find alpha, we solve the equation alpha * v = p, which will lead
76 : * us to the system, for the variables alpha, w_ab and w_ac:
77 : *
78 : * alpha * x = 1 - w_ab - w_ac
79 : * alpha * y = w_ab
80 : * alpha * z = w_ac,
81 : * where w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1
82 : *
83 : * That will lead to alpha = 1 / (x + y + z), w_ab = y / (x + y + b) and
84 : * w_ac = z / (x + y + z) and the following holds:
85 : * - alpha does exist because x + y + z > 0.
86 : * - w_ab >= 0 and w_ac >= 0 because y >= 0 and z >= 0 and x + y + z > 0.
87 : * - 0 <= 1 - w_ab - w_ac <= 1 because 0 <= (y + z) / (x + y + z) <= 1.
88 : *
89 : * Thus:
90 : *
91 : * ----------------------------------------------------------
92 : * | v = x * a + y * b + z * c crosses T = (a, b, c), where |
93 : * | a, b and c are linearly independent, iff: |
94 : * | x >= 0, y >= 0, z >= 0 and x + y + z > 0 |
95 : * ----------------------------------------------------------
96 : *
97 : * Moreover:
98 : * - if one of the coefficients is zero, then v crosses the edge formed by the
99 : * vertices multiplied by the non-zero coefficients.
100 : * - if two of the coefficients are zero, then v crosses the vertex multiplied
101 : * by the non-zero coefficient.
102 : */
103 :
104 :
105 : #include "AP_GeodesicGrid.h"
106 :
107 : /* This was generated with
108 : * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
109 : const struct AP_GeodesicGrid::neighbor_umbrella
110 : AP_GeodesicGrid::_neighbor_umbrellas[3]{
111 : {{ 9, 8, 7, 12, 14}, 1, 2, 0, 0, 2},
112 : {{ 1, 2, 4, 5, 3}, 0, 0, 2, 2, 0},
113 : {{16, 15, 13, 18, 17}, 2, 2, 0, 2, 1},
114 : };
115 :
116 : /* This was generated with
117 : * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
118 : const Matrix3f AP_GeodesicGrid::_inverses[10]{
119 : {{-0.309017f, 0.500000f, 0.190983f},
120 : { 0.000000f, 0.000000f, -0.618034f},
121 : {-0.309017f, -0.500000f, 0.190983f}},
122 : {{-0.190983f, 0.309017f, -0.500000f},
123 : {-0.500000f, -0.190983f, 0.309017f},
124 : { 0.309017f, -0.500000f, -0.190983f}},
125 : {{-0.618034f, 0.000000f, 0.000000f},
126 : { 0.190983f, -0.309017f, -0.500000f},
127 : { 0.190983f, -0.309017f, 0.500000f}},
128 : {{-0.500000f, 0.190983f, -0.309017f},
129 : { 0.000000f, -0.618034f, 0.000000f},
130 : { 0.500000f, 0.190983f, -0.309017f}},
131 : {{-0.190983f, -0.309017f, -0.500000f},
132 : {-0.190983f, -0.309017f, 0.500000f},
133 : { 0.618034f, 0.000000f, 0.000000f}},
134 : {{-0.309017f, -0.500000f, -0.190983f},
135 : { 0.190983f, 0.309017f, -0.500000f},
136 : { 0.500000f, -0.190983f, 0.309017f}},
137 : {{ 0.309017f, -0.500000f, 0.190983f},
138 : { 0.000000f, 0.000000f, -0.618034f},
139 : { 0.309017f, 0.500000f, 0.190983f}},
140 : {{ 0.190983f, -0.309017f, -0.500000f},
141 : { 0.500000f, 0.190983f, 0.309017f},
142 : {-0.309017f, 0.500000f, -0.190983f}},
143 : {{ 0.500000f, -0.190983f, -0.309017f},
144 : { 0.000000f, 0.618034f, 0.000000f},
145 : {-0.500000f, -0.190983f, -0.309017f}},
146 : {{ 0.309017f, 0.500000f, -0.190983f},
147 : {-0.500000f, 0.190983f, 0.309017f},
148 : {-0.190983f, -0.309017f, -0.500000f}},
149 : };
150 :
151 : /* This was generated with
152 : * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
153 : const Matrix3f AP_GeodesicGrid::_mid_inverses[10]{
154 : {{-0.000000f, 1.000000f, -0.618034f},
155 : { 0.000000f, -1.000000f, -0.618034f},
156 : {-0.618034f, 0.000000f, 1.000000f}},
157 : {{-1.000000f, 0.618034f, -0.000000f},
158 : {-0.000000f, -1.000000f, 0.618034f},
159 : { 0.618034f, -0.000000f, -1.000000f}},
160 : {{-0.618034f, -0.000000f, -1.000000f},
161 : { 1.000000f, -0.618034f, -0.000000f},
162 : {-0.618034f, 0.000000f, 1.000000f}},
163 : {{-1.000000f, -0.618034f, -0.000000f},
164 : { 1.000000f, -0.618034f, 0.000000f},
165 : {-0.000000f, 1.000000f, -0.618034f}},
166 : {{-1.000000f, -0.618034f, 0.000000f},
167 : { 0.618034f, 0.000000f, 1.000000f},
168 : { 0.618034f, 0.000000f, -1.000000f}},
169 : {{-0.618034f, -0.000000f, -1.000000f},
170 : { 1.000000f, 0.618034f, -0.000000f},
171 : { 0.000000f, -1.000000f, 0.618034f}},
172 : {{ 0.000000f, -1.000000f, -0.618034f},
173 : { 0.000000f, 1.000000f, -0.618034f},
174 : { 0.618034f, -0.000000f, 1.000000f}},
175 : {{ 1.000000f, -0.618034f, -0.000000f},
176 : { 0.000000f, 1.000000f, 0.618034f},
177 : {-0.618034f, 0.000000f, -1.000000f}},
178 : {{ 1.000000f, 0.618034f, -0.000000f},
179 : {-1.000000f, 0.618034f, 0.000000f},
180 : { 0.000000f, -1.000000f, -0.618034f}},
181 : {{-0.000000f, 1.000000f, 0.618034f},
182 : {-1.000000f, -0.618034f, -0.000000f},
183 : { 0.618034f, 0.000000f, -1.000000f}},
184 : };
185 :
186 80252 : int AP_GeodesicGrid::section(const Vector3f &v, bool inclusive)
187 : {
188 80252 : int i = _triangle_index(v, inclusive);
189 80252 : if (i < 0) {
190 24 : return -1;
191 : }
192 :
193 80228 : int j = _subtriangle_index(i, v, inclusive);
194 80228 : if (j < 0) {
195 2 : return -1;
196 : }
197 :
198 80226 : return 4 * i + j;
199 : }
200 :
201 122426 : int AP_GeodesicGrid::_neighbor_umbrella_component(int idx, int comp_idx)
202 : {
203 122426 : if (idx < 3) {
204 54135 : return _neighbor_umbrellas[idx].components[comp_idx];
205 : }
206 68291 : return (_neighbor_umbrellas[idx % 3].components[comp_idx] + 10) % 20;
207 : }
208 :
209 71271 : int AP_GeodesicGrid::_from_neighbor_umbrella(int idx,
210 : const Vector3f &v,
211 : const Vector3f &u,
212 : bool inclusive)
213 : {
214 : /* The following comparisons between the umbrella's first and second
215 : * vertices' coefficients work for this algorithm because all vertices'
216 : * vectors are of the same length. */
217 :
218 71271 : if (is_equal(u.x, u.y)) {
219 : /* If the coefficients of the first and second vertices are equal, then
220 : * v crosses the first component or the edge formed by the umbrella's
221 : * pivot and forth vertex. */
222 133 : int comp = _neighbor_umbrella_component(idx, 0);
223 133 : auto w = _inverses[comp % 10] * v;
224 133 : if (comp > 9) {
225 66 : w = -w;
226 : }
227 133 : float x0 = w[_neighbor_umbrellas[idx % 3].v0_c0];
228 133 : if (is_zero(x0)) {
229 36 : if (!inclusive) {
230 24 : return -1;
231 : }
232 12 : return comp;
233 97 : } else if (x0 < 0) {
234 1 : if (!inclusive) {
235 0 : return -1;
236 : }
237 1 : return _neighbor_umbrella_component(idx, u.x < u.y ? 3 : 2);
238 : }
239 :
240 96 : return comp;
241 : }
242 :
243 71138 : if (u.y > u.x) {
244 : /* If the coefficient of the second vertex is greater than the first
245 : * one's, then v crosses the first, second or third component. */
246 36361 : int comp = _neighbor_umbrella_component(idx, 1);
247 36361 : auto w = _inverses[comp % 10] * v;
248 36361 : if (comp > 9) {
249 17008 : w = -w;
250 : }
251 36361 : float x1 = w[_neighbor_umbrellas[idx % 3].v1_c1];
252 36361 : float x2 = w[_neighbor_umbrellas[idx % 3].v2_c1];
253 :
254 36361 : if (is_zero(x1)) {
255 0 : if (!inclusive) {
256 0 : return -1;
257 : }
258 0 : return _neighbor_umbrella_component(idx, x1 < 0 ? 2 : 1);
259 36361 : } else if (x1 < 0) {
260 13963 : return _neighbor_umbrella_component(idx, 2);
261 : }
262 :
263 22398 : if (is_zero(x2)) {
264 0 : if (!inclusive) {
265 0 : return -1;
266 : }
267 0 : return _neighbor_umbrella_component(idx, x2 > 0 ? 1 : 0);
268 22398 : } else if (x2 < 0) {
269 12027 : return _neighbor_umbrella_component(idx, 0);
270 : }
271 :
272 10371 : return comp;
273 : } else {
274 : /* If the coefficient of the second vertex is lesser than the first
275 : * one's, then v crosses the first, fourth or fifth component. */
276 34777 : int comp = _neighbor_umbrella_component(idx, 4);
277 34777 : auto w = _inverses[comp % 10] * v;
278 34777 : if (comp > 9) {
279 14033 : w = -w;
280 : }
281 34777 : float x4 = w[_neighbor_umbrellas[idx % 3].v4_c4];
282 34777 : float x0 = w[_neighbor_umbrellas[idx % 3].v0_c4];
283 :
284 34777 : if (is_zero(x4)) {
285 0 : if (!inclusive) {
286 0 : return -1;
287 : }
288 0 : return _neighbor_umbrella_component(idx, x4 < 0 ? 0 : 4);
289 34777 : } else if (x4 < 0) {
290 11330 : return _neighbor_umbrella_component(idx, 0);
291 : }
292 :
293 23447 : if (is_zero(x0)) {
294 0 : if (!inclusive) {
295 0 : return -1;
296 : }
297 0 : return _neighbor_umbrella_component(idx, x0 > 0 ? 4 : 3);
298 23447 : } else if (x0 < 0) {
299 13834 : return _neighbor_umbrella_component(idx, 3);
300 : }
301 :
302 9613 : return comp;
303 : }
304 : }
305 :
306 81404 : int AP_GeodesicGrid::_triangle_index(const Vector3f &v, bool inclusive)
307 : {
308 : /* w holds the coordinates of v with respect to the basis comprised by the
309 : * vectors of T_i */
310 81404 : auto w = _inverses[0] * v;
311 81404 : int zero_count = 0;
312 81404 : int balance = 0;
313 81404 : int umbrella = -1;
314 :
315 81404 : if (is_zero(w.x)) {
316 120 : zero_count++;
317 81284 : } else if (w.x > 0) {
318 36469 : balance++;
319 : } else {
320 44815 : balance--;
321 : }
322 :
323 81404 : if (is_zero(w.y)) {
324 122 : zero_count++;
325 81282 : } else if (w.y > 0) {
326 44320 : balance++;
327 : } else {
328 36962 : balance--;
329 : }
330 :
331 81404 : if (is_zero(w.z)) {
332 120 : zero_count++;
333 81284 : } else if (w.z > 0) {
334 35465 : balance++;
335 : } else {
336 45819 : balance--;
337 : }
338 :
339 81404 : switch (balance) {
340 4993 : case 3:
341 : /* All coefficients are positive, thus return the first triangle. */
342 4993 : return 0;
343 5104 : case -3:
344 : /* All coefficients are negative, which means that the coefficients for
345 : * -w are positive, thus return the first triangle's opposite. */
346 5104 : return 10;
347 0 : case 2:
348 : /* Two coefficients are positive and one is zero, thus v crosses one of
349 : * the edges of the first triangle. */
350 0 : return inclusive ? 0 : -1;
351 0 : case -2:
352 : /* Analogous to the previous case, but for the opposite of the first
353 : * triangle. */
354 0 : return inclusive ? 10 : -1;
355 30004 : case 1:
356 : /* There are two possible cases when balance is 1:
357 : *
358 : * 1) Two coefficients are zero, which means v crosses one of the
359 : * vertices of the first triangle.
360 : *
361 : * 2) Two coefficients are positive and one is negative. Let a and b be
362 : * vertices with positive coefficients and c the one with the negative
363 : * coefficient. That means that v crosses the triangle formed by a, b
364 : * and -c. The vector -c happens to be the 3-th vertex, with respect to
365 : * (a, b), of the first triangle's neighbor umbrella with respect to a
366 : * and b. Thus, v crosses one of the components of that umbrella. */
367 30004 : if (zero_count == 2) {
368 18 : return inclusive ? 0 : -1;
369 : }
370 :
371 29986 : if (!is_zero(w.x) && w.x < 0) {
372 8553 : umbrella = 1;
373 21433 : } else if (!is_zero(w.y) && w.y < 0) {
374 12572 : umbrella = 2;
375 : } else {
376 8861 : umbrella = 0;
377 : }
378 :
379 29986 : break;
380 41013 : case -1:
381 : /* Analogous to the previous case, but for the opposite of the first
382 : * triangle. */
383 41013 : if (zero_count == 2) {
384 18 : return inclusive ? 10 : -1;
385 : }
386 :
387 40995 : if (!is_zero(w.x) && w.x > 0) {
388 9941 : umbrella = 4;
389 31054 : } else if (!is_zero(w.y) && w.y > 0) {
390 21811 : umbrella = 5;
391 : } else {
392 9243 : umbrella = 3;
393 : }
394 40995 : w = -w;
395 :
396 40995 : break;
397 290 : case 0:
398 : /* There are two possible cases when balance is 1:
399 : *
400 : * 1) The vector v is the null vector, which doesn't cross any section.
401 : *
402 : * 2) One coefficient is zero, another is positive and yet another is
403 : * negative. Let a, b and c be the respective vertices for those
404 : * coefficients, then the statements in case (2) for when balance is 1
405 : * are also valid here.
406 : */
407 290 : if (zero_count == 3) {
408 0 : return -1;
409 : }
410 :
411 290 : if (!is_zero(w.x) && w.x < 0) {
412 98 : umbrella = 1;
413 192 : } else if (!is_zero(w.y) && w.y < 0) {
414 96 : umbrella = 2;
415 : } else {
416 96 : umbrella = 0;
417 : }
418 :
419 290 : break;
420 : }
421 :
422 71271 : switch (umbrella % 3) {
423 18200 : case 0:
424 18200 : w.z = -w.z;
425 18200 : break;
426 18592 : case 1:
427 18592 : w = {w.y, w.z, -w.x};
428 18592 : break;
429 34479 : case 2:
430 34479 : w = {w.z, w.x, -w.y};
431 34479 : break;
432 : }
433 :
434 71271 : return _from_neighbor_umbrella(umbrella, v, w, inclusive);
435 : }
436 :
437 81356 : int AP_GeodesicGrid::_subtriangle_index(const unsigned int triangle_index,
438 : const Vector3f &v,
439 : bool inclusive)
440 : {
441 : /* w holds the coordinates of v with respect to the basis comprised by the
442 : * vectors of the middle triangle of T_i where i is triangle_index */
443 81356 : auto w = _mid_inverses[triangle_index % 10] * v;
444 81356 : if (triangle_index > 9) {
445 37315 : w = -w;
446 : }
447 :
448 81356 : if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) {
449 4 : return -1;
450 : }
451 :
452 : /* At this point, we know that v crosses the icosahedron triangle pointed
453 : * by triangle_index. Thus, we can geometrically see that if v doesn't
454 : * cross its middle triangle, then one of the coefficients will be negative
455 : * and the other ones positive. Let a and b be the non-negative
456 : * coefficients and c the negative one. In that case, v will cross the
457 : * triangle with vertices (a, b, -c). Since we know that v crosses the
458 : * icosahedron triangle and the only sub-triangle that contains the set of
459 : * points (seen as vectors) that cross the triangle (a, b, -c) is the
460 : * middle triangle's neighbor with respect to a and b, then that
461 : * sub-triangle is the one crossed by v. */
462 81352 : if (!is_zero(w.x) && w.x < 0) {
463 19551 : return 3;
464 : }
465 61801 : if (!is_zero(w.y) && w.y < 0) {
466 22645 : return 1;
467 : }
468 39156 : if (!is_zero(w.z) && w.z < 0) {
469 15083 : return 2;
470 : }
471 :
472 : /* If x >= 0 and y >= 0 and z >= 0, then v crosses the middle triangle. */
473 24073 : return 0;
474 : }
|
