1/**************************************************************************/
2/* geometry_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "geometry_3d.h"
32
33#include "thirdparty/misc/polypartition.h"
34
35void Geometry3D::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) {
36 // Based on David Eberly's Computation of Distance Between Line Segments algorithm.
37
38 Vector3 p = p_p1 - p_p0;
39 Vector3 q = p_q1 - p_q0;
40 Vector3 r = p_p0 - p_q0;
41
42 real_t a = p.dot(p);
43 real_t b = p.dot(q);
44 real_t c = q.dot(q);
45 real_t d = p.dot(r);
46 real_t e = q.dot(r);
47
48 real_t s = 0.0f;
49 real_t t = 0.0f;
50
51 real_t det = a * c - b * b;
52 if (det > CMP_EPSILON) {
53 // Non-parallel segments
54 real_t bte = b * e;
55 real_t ctd = c * d;
56
57 if (bte <= ctd) {
58 // s <= 0.0f
59 if (e <= 0.0f) {
60 // t <= 0.0f
61 s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f));
62 t = 0.0f;
63 } else if (e < c) {
64 // 0.0f < t < 1
65 s = 0.0f;
66 t = e / c;
67 } else {
68 // t >= 1
69 s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f));
70 t = 1;
71 }
72 } else {
73 // s > 0.0f
74 s = bte - ctd;
75 if (s >= det) {
76 // s >= 1
77 if (b + e <= 0.0f) {
78 // t <= 0.0f
79 s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1));
80 t = 0.0f;
81 } else if (b + e < c) {
82 // 0.0f < t < 1
83 s = 1;
84 t = (b + e) / c;
85 } else {
86 // t >= 1
87 s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1));
88 t = 1;
89 }
90 } else {
91 // 0.0f < s < 1
92 real_t ate = a * e;
93 real_t btd = b * d;
94
95 if (ate <= btd) {
96 // t <= 0.0f
97 s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
98 t = 0.0f;
99 } else {
100 // t > 0.0f
101 t = ate - btd;
102 if (t >= det) {
103 // t >= 1
104 s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
105 t = 1;
106 } else {
107 // 0.0f < t < 1
108 s /= det;
109 t /= det;
110 }
111 }
112 }
113 }
114 } else {
115 // Parallel segments
116 if (e <= 0.0f) {
117 s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
118 t = 0.0f;
119 } else if (e >= c) {
120 s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
121 t = 1;
122 } else {
123 s = 0.0f;
124 t = e / c;
125 }
126 }
127
128 r_ps = (1 - s) * p_p0 + s * p_p1;
129 r_qt = (1 - t) * p_q0 + t * p_q1;
130}
131
132real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) {
133 Vector3 ps;
134 Vector3 qt;
135 get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt);
136 Vector3 st = qt - ps;
137 return st.length();
138}
139
140void Geometry3D::MeshData::optimize_vertices() {
141 HashMap<int, int> vtx_remap;
142
143 for (MeshData::Face &face : faces) {
144 for (int &index : face.indices) {
145 if (!vtx_remap.has(index)) {
146 int ni = vtx_remap.size();
147 vtx_remap[index] = ni;
148 }
149 index = vtx_remap[index];
150 }
151 }
152
153 for (MeshData::Edge &edge : edges) {
154 int a = edge.vertex_a;
155 int b = edge.vertex_b;
156
157 if (!vtx_remap.has(a)) {
158 int ni = vtx_remap.size();
159 vtx_remap[a] = ni;
160 }
161 if (!vtx_remap.has(b)) {
162 int ni = vtx_remap.size();
163 vtx_remap[b] = ni;
164 }
165
166 edge.vertex_a = vtx_remap[a];
167 edge.vertex_b = vtx_remap[b];
168 }
169
170 LocalVector<Vector3> new_vertices;
171 new_vertices.resize(vtx_remap.size());
172
173 for (uint32_t i = 0; i < vertices.size(); i++) {
174 if (vtx_remap.has(i)) {
175 new_vertices[vtx_remap[i]] = vertices[i];
176 }
177 }
178 vertices = new_vertices;
179}
180
181struct _FaceClassify {
182 struct _Link {
183 int face = -1;
184 int edge = -1;
185 void clear() {
186 face = -1;
187 edge = -1;
188 }
189 _Link() {}
190 };
191 bool valid = false;
192 int group = -1;
193 _Link links[3];
194 Face3 face;
195 _FaceClassify() {}
196};
197
198/*** GEOMETRY WRAPPER ***/
199
200enum _CellFlags {
201 _CELL_SOLID = 1,
202 _CELL_EXTERIOR = 2,
203 _CELL_STEP_MASK = 0x1C,
204 _CELL_STEP_NONE = 0 << 2,
205 _CELL_STEP_Y_POS = 1 << 2,
206 _CELL_STEP_Y_NEG = 2 << 2,
207 _CELL_STEP_X_POS = 3 << 2,
208 _CELL_STEP_X_NEG = 4 << 2,
209 _CELL_STEP_Z_POS = 5 << 2,
210 _CELL_STEP_Z_NEG = 6 << 2,
211 _CELL_STEP_DONE = 7 << 2,
212 _CELL_PREV_MASK = 0xE0,
213 _CELL_PREV_NONE = 0 << 5,
214 _CELL_PREV_Y_POS = 1 << 5,
215 _CELL_PREV_Y_NEG = 2 << 5,
216 _CELL_PREV_X_POS = 3 << 5,
217 _CELL_PREV_X_NEG = 4 << 5,
218 _CELL_PREV_Z_POS = 5 << 5,
219 _CELL_PREV_Z_NEG = 6 << 5,
220 _CELL_PREV_FIRST = 7 << 5,
221};
222
223static inline void _plot_face(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, const Vector3 &voxelsize, const Face3 &p_face) {
224 AABB aabb(Vector3(x, y, z), Vector3(len_x, len_y, len_z));
225 aabb.position = aabb.position * voxelsize;
226 aabb.size = aabb.size * voxelsize;
227
228 if (!p_face.intersects_aabb(aabb)) {
229 return;
230 }
231
232 if (len_x == 1 && len_y == 1 && len_z == 1) {
233 p_cell_status[x][y][z] = _CELL_SOLID;
234 return;
235 }
236
237 int div_x = len_x > 1 ? 2 : 1;
238 int div_y = len_y > 1 ? 2 : 1;
239 int div_z = len_z > 1 ? 2 : 1;
240
241#define SPLIT_DIV(m_i, m_div, m_v, m_len_v, m_new_v, m_new_len_v) \
242 if (m_div == 1) { \
243 m_new_v = m_v; \
244 m_new_len_v = 1; \
245 } else if (m_i == 0) { \
246 m_new_v = m_v; \
247 m_new_len_v = m_len_v / 2; \
248 } else { \
249 m_new_v = m_v + m_len_v / 2; \
250 m_new_len_v = m_len_v - m_len_v / 2; \
251 }
252
253 int new_x;
254 int new_len_x;
255 int new_y;
256 int new_len_y;
257 int new_z;
258 int new_len_z;
259
260 for (int i = 0; i < div_x; i++) {
261 SPLIT_DIV(i, div_x, x, len_x, new_x, new_len_x);
262
263 for (int j = 0; j < div_y; j++) {
264 SPLIT_DIV(j, div_y, y, len_y, new_y, new_len_y);
265
266 for (int k = 0; k < div_z; k++) {
267 SPLIT_DIV(k, div_z, z, len_z, new_z, new_len_z);
268
269 _plot_face(p_cell_status, new_x, new_y, new_z, new_len_x, new_len_y, new_len_z, voxelsize, p_face);
270 }
271 }
272 }
273
274#undef SPLIT_DIV
275}
276
277static inline void _mark_outside(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z) {
278 if (p_cell_status[x][y][z] & 3) {
279 return; // Nothing to do, already used and/or visited.
280 }
281
282 p_cell_status[x][y][z] = _CELL_PREV_FIRST;
283
284 while (true) {
285 uint8_t &c = p_cell_status[x][y][z];
286
287 if ((c & _CELL_STEP_MASK) == _CELL_STEP_NONE) {
288 // Haven't been in here, mark as outside.
289 p_cell_status[x][y][z] |= _CELL_EXTERIOR;
290 }
291
292 if ((c & _CELL_STEP_MASK) != _CELL_STEP_DONE) {
293 // If not done, increase step.
294 c += 1 << 2;
295 }
296
297 if ((c & _CELL_STEP_MASK) == _CELL_STEP_DONE) {
298 // Go back.
299 switch (c & _CELL_PREV_MASK) {
300 case _CELL_PREV_FIRST: {
301 return;
302 } break;
303 case _CELL_PREV_Y_POS: {
304 y++;
305 ERR_FAIL_COND(y >= len_y);
306 } break;
307 case _CELL_PREV_Y_NEG: {
308 y--;
309 ERR_FAIL_COND(y < 0);
310 } break;
311 case _CELL_PREV_X_POS: {
312 x++;
313 ERR_FAIL_COND(x >= len_x);
314 } break;
315 case _CELL_PREV_X_NEG: {
316 x--;
317 ERR_FAIL_COND(x < 0);
318 } break;
319 case _CELL_PREV_Z_POS: {
320 z++;
321 ERR_FAIL_COND(z >= len_z);
322 } break;
323 case _CELL_PREV_Z_NEG: {
324 z--;
325 ERR_FAIL_COND(z < 0);
326 } break;
327 default: {
328 ERR_FAIL();
329 }
330 }
331 continue;
332 }
333
334 int next_x = x, next_y = y, next_z = z;
335 uint8_t prev = 0;
336
337 switch (c & _CELL_STEP_MASK) {
338 case _CELL_STEP_Y_POS: {
339 next_y++;
340 prev = _CELL_PREV_Y_NEG;
341 } break;
342 case _CELL_STEP_Y_NEG: {
343 next_y--;
344 prev = _CELL_PREV_Y_POS;
345 } break;
346 case _CELL_STEP_X_POS: {
347 next_x++;
348 prev = _CELL_PREV_X_NEG;
349 } break;
350 case _CELL_STEP_X_NEG: {
351 next_x--;
352 prev = _CELL_PREV_X_POS;
353 } break;
354 case _CELL_STEP_Z_POS: {
355 next_z++;
356 prev = _CELL_PREV_Z_NEG;
357 } break;
358 case _CELL_STEP_Z_NEG: {
359 next_z--;
360 prev = _CELL_PREV_Z_POS;
361 } break;
362 default:
363 ERR_FAIL();
364 }
365
366 if (next_x < 0 || next_x >= len_x) {
367 continue;
368 }
369 if (next_y < 0 || next_y >= len_y) {
370 continue;
371 }
372 if (next_z < 0 || next_z >= len_z) {
373 continue;
374 }
375
376 if (p_cell_status[next_x][next_y][next_z] & 3) {
377 continue;
378 }
379
380 x = next_x;
381 y = next_y;
382 z = next_z;
383 p_cell_status[x][y][z] |= prev;
384 }
385}
386
387static inline void _build_faces(uint8_t ***p_cell_status, int x, int y, int z, int len_x, int len_y, int len_z, Vector<Face3> &p_faces) {
388 ERR_FAIL_INDEX(x, len_x);
389 ERR_FAIL_INDEX(y, len_y);
390 ERR_FAIL_INDEX(z, len_z);
391
392 if (p_cell_status[x][y][z] & _CELL_EXTERIOR) {
393 return;
394 }
395
396#define vert(m_idx) Vector3(((m_idx)&4) >> 2, ((m_idx)&2) >> 1, (m_idx)&1)
397
398 static const uint8_t indices[6][4] = {
399 { 7, 6, 4, 5 },
400 { 7, 3, 2, 6 },
401 { 7, 5, 1, 3 },
402 { 0, 2, 3, 1 },
403 { 0, 1, 5, 4 },
404 { 0, 4, 6, 2 },
405
406 };
407
408 for (int i = 0; i < 6; i++) {
409 Vector3 face_points[4];
410 int disp_x = x + ((i % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
411 int disp_y = y + (((i - 1) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
412 int disp_z = z + (((i - 2) % 3) == 0 ? ((i < 3) ? 1 : -1) : 0);
413
414 bool plot = false;
415
416 if (disp_x < 0 || disp_x >= len_x) {
417 plot = true;
418 }
419 if (disp_y < 0 || disp_y >= len_y) {
420 plot = true;
421 }
422 if (disp_z < 0 || disp_z >= len_z) {
423 plot = true;
424 }
425
426 if (!plot && (p_cell_status[disp_x][disp_y][disp_z] & _CELL_EXTERIOR)) {
427 plot = true;
428 }
429
430 if (!plot) {
431 continue;
432 }
433
434 for (int j = 0; j < 4; j++) {
435 face_points[j] = vert(indices[i][j]) + Vector3(x, y, z);
436 }
437
438 p_faces.push_back(
439 Face3(
440 face_points[0],
441 face_points[1],
442 face_points[2]));
443
444 p_faces.push_back(
445 Face3(
446 face_points[2],
447 face_points[3],
448 face_points[0]));
449 }
450}
451
452Vector<Face3> Geometry3D::wrap_geometry(Vector<Face3> p_array, real_t *p_error) {
453 int face_count = p_array.size();
454 const Face3 *faces = p_array.ptr();
455 constexpr double min_size = 1.0;
456 constexpr int max_length = 20;
457
458 AABB global_aabb;
459
460 for (int i = 0; i < face_count; i++) {
461 if (i == 0) {
462 global_aabb = faces[i].get_aabb();
463 } else {
464 global_aabb.merge_with(faces[i].get_aabb());
465 }
466 }
467
468 global_aabb.grow_by(0.01f); // Avoid numerical error.
469
470 // Determine amount of cells in grid axis.
471 int div_x, div_y, div_z;
472
473 if (global_aabb.size.x / min_size < max_length) {
474 div_x = (int)(global_aabb.size.x / min_size) + 1;
475 } else {
476 div_x = max_length;
477 }
478
479 if (global_aabb.size.y / min_size < max_length) {
480 div_y = (int)(global_aabb.size.y / min_size) + 1;
481 } else {
482 div_y = max_length;
483 }
484
485 if (global_aabb.size.z / min_size < max_length) {
486 div_z = (int)(global_aabb.size.z / min_size) + 1;
487 } else {
488 div_z = max_length;
489 }
490
491 Vector3 voxelsize = global_aabb.size;
492 voxelsize.x /= div_x;
493 voxelsize.y /= div_y;
494 voxelsize.z /= div_z;
495
496 // Create and initialize cells to zero.
497
498 uint8_t ***cell_status = memnew_arr(uint8_t **, div_x);
499 for (int i = 0; i < div_x; i++) {
500 cell_status[i] = memnew_arr(uint8_t *, div_y);
501
502 for (int j = 0; j < div_y; j++) {
503 cell_status[i][j] = memnew_arr(uint8_t, div_z);
504
505 for (int k = 0; k < div_z; k++) {
506 cell_status[i][j][k] = 0;
507 }
508 }
509 }
510
511 // Plot faces into cells.
512
513 for (int i = 0; i < face_count; i++) {
514 Face3 f = faces[i];
515 for (int j = 0; j < 3; j++) {
516 f.vertex[j] -= global_aabb.position;
517 }
518 _plot_face(cell_status, 0, 0, 0, div_x, div_y, div_z, voxelsize, f);
519 }
520
521 // Determine which cells connect to the outside by traversing the outside and recursively flood-fill marking.
522
523 for (int i = 0; i < div_x; i++) {
524 for (int j = 0; j < div_y; j++) {
525 _mark_outside(cell_status, i, j, 0, div_x, div_y, div_z);
526 _mark_outside(cell_status, i, j, div_z - 1, div_x, div_y, div_z);
527 }
528 }
529
530 for (int i = 0; i < div_z; i++) {
531 for (int j = 0; j < div_y; j++) {
532 _mark_outside(cell_status, 0, j, i, div_x, div_y, div_z);
533 _mark_outside(cell_status, div_x - 1, j, i, div_x, div_y, div_z);
534 }
535 }
536
537 for (int i = 0; i < div_x; i++) {
538 for (int j = 0; j < div_z; j++) {
539 _mark_outside(cell_status, i, 0, j, div_x, div_y, div_z);
540 _mark_outside(cell_status, i, div_y - 1, j, div_x, div_y, div_z);
541 }
542 }
543
544 // Build faces for the inside-outside cell divisors.
545
546 Vector<Face3> wrapped_faces;
547
548 for (int i = 0; i < div_x; i++) {
549 for (int j = 0; j < div_y; j++) {
550 for (int k = 0; k < div_z; k++) {
551 _build_faces(cell_status, i, j, k, div_x, div_y, div_z, wrapped_faces);
552 }
553 }
554 }
555
556 // Transform face vertices to global coords.
557
558 int wrapped_faces_count = wrapped_faces.size();
559 Face3 *wrapped_faces_ptr = wrapped_faces.ptrw();
560
561 for (int i = 0; i < wrapped_faces_count; i++) {
562 for (int j = 0; j < 3; j++) {
563 Vector3 &v = wrapped_faces_ptr[i].vertex[j];
564 v = v * voxelsize;
565 v += global_aabb.position;
566 }
567 }
568
569 // clean up grid
570
571 for (int i = 0; i < div_x; i++) {
572 for (int j = 0; j < div_y; j++) {
573 memdelete_arr(cell_status[i][j]);
574 }
575
576 memdelete_arr(cell_status[i]);
577 }
578
579 memdelete_arr(cell_status);
580 if (p_error) {
581 *p_error = voxelsize.length();
582 }
583
584 return wrapped_faces;
585}
586
587Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes) {
588 MeshData mesh;
589
590#define SUBPLANE_SIZE 1024.0
591
592 real_t subplane_size = 1024.0; // Should compute this from the actual plane.
593 for (int i = 0; i < p_planes.size(); i++) {
594 Plane p = p_planes[i];
595
596 Vector3 ref = Vector3(0.0, 1.0, 0.0);
597
598 if (ABS(p.normal.dot(ref)) > 0.95f) {
599 ref = Vector3(0.0, 0.0, 1.0); // Change axis.
600 }
601
602 Vector3 right = p.normal.cross(ref).normalized();
603 Vector3 up = p.normal.cross(right).normalized();
604
605 Vector3 center = p.get_center();
606
607 // make a quad clockwise
608 LocalVector<Vector3> vertices = {
609 center - up * subplane_size + right * subplane_size,
610 center - up * subplane_size - right * subplane_size,
611 center + up * subplane_size - right * subplane_size,
612 center + up * subplane_size + right * subplane_size
613 };
614
615 for (int j = 0; j < p_planes.size(); j++) {
616 if (j == i) {
617 continue;
618 }
619
620 LocalVector<Vector3> new_vertices;
621 Plane clip = p_planes[j];
622
623 if (clip.normal.dot(p.normal) > 0.95f) {
624 continue;
625 }
626
627 if (vertices.size() < 3) {
628 break;
629 }
630
631 for (uint32_t k = 0; k < vertices.size(); k++) {
632 int k_n = (k + 1) % vertices.size();
633
634 Vector3 edge0_A = vertices[k];
635 Vector3 edge1_A = vertices[k_n];
636
637 real_t dist0 = clip.distance_to(edge0_A);
638 real_t dist1 = clip.distance_to(edge1_A);
639
640 if (dist0 <= 0) { // Behind plane.
641
642 new_vertices.push_back(vertices[k]);
643 }
644
645 // Check for different sides and non coplanar.
646 if ((dist0 * dist1) < 0) {
647 // Calculate intersection.
648 Vector3 rel = edge1_A - edge0_A;
649
650 real_t den = clip.normal.dot(rel);
651 if (Math::is_zero_approx(den)) {
652 continue; // Point too short.
653 }
654
655 real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den;
656 Vector3 inters = edge0_A + rel * dist;
657 new_vertices.push_back(inters);
658 }
659 }
660
661 vertices = new_vertices;
662 }
663
664 if (vertices.size() < 3) {
665 continue;
666 }
667
668 // Result is a clockwise face.
669
670 MeshData::Face face;
671
672 // Add face indices.
673 for (const Vector3 &vertex : vertices) {
674 int idx = -1;
675 for (uint32_t k = 0; k < mesh.vertices.size(); k++) {
676 if (mesh.vertices[k].distance_to(vertex) < 0.001f) {
677 idx = k;
678 break;
679 }
680 }
681
682 if (idx == -1) {
683 idx = mesh.vertices.size();
684 mesh.vertices.push_back(vertex);
685 }
686
687 face.indices.push_back(idx);
688 }
689 face.plane = p;
690 mesh.faces.push_back(face);
691
692 // Add edge.
693
694 for (uint32_t j = 0; j < face.indices.size(); j++) {
695 int a = face.indices[j];
696 int b = face.indices[(j + 1) % face.indices.size()];
697
698 bool found = false;
699 int found_idx = -1;
700 for (uint32_t k = 0; k < mesh.edges.size(); k++) {
701 if (mesh.edges[k].vertex_a == a && mesh.edges[k].vertex_b == b) {
702 found = true;
703 found_idx = k;
704 break;
705 }
706 if (mesh.edges[k].vertex_b == a && mesh.edges[k].vertex_a == b) {
707 found = true;
708 found_idx = k;
709 break;
710 }
711 }
712
713 if (found) {
714 mesh.edges[found_idx].face_b = j;
715 continue;
716 }
717 MeshData::Edge edge;
718 edge.vertex_a = a;
719 edge.vertex_b = b;
720 edge.face_a = j;
721 edge.face_b = -1;
722 mesh.edges.push_back(edge);
723 }
724 }
725
726 return mesh;
727}
728
729Vector<Plane> Geometry3D::build_box_planes(const Vector3 &p_extents) {
730 Vector<Plane> planes = {
731 Plane(Vector3(1, 0, 0), p_extents.x),
732 Plane(Vector3(-1, 0, 0), p_extents.x),
733 Plane(Vector3(0, 1, 0), p_extents.y),
734 Plane(Vector3(0, -1, 0), p_extents.y),
735 Plane(Vector3(0, 0, 1), p_extents.z),
736 Plane(Vector3(0, 0, -1), p_extents.z)
737 };
738
739 return planes;
740}
741
742Vector<Plane> Geometry3D::build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis) {
743 ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
744
745 Vector<Plane> planes;
746
747 const double sides_step = Math_TAU / p_sides;
748 for (int i = 0; i < p_sides; i++) {
749 Vector3 normal;
750 normal[(p_axis + 1) % 3] = Math::cos(i * sides_step);
751 normal[(p_axis + 2) % 3] = Math::sin(i * sides_step);
752
753 planes.push_back(Plane(normal, p_radius));
754 }
755
756 Vector3 axis;
757 axis[p_axis] = 1.0;
758
759 planes.push_back(Plane(axis, p_height * 0.5f));
760 planes.push_back(Plane(-axis, p_height * 0.5f));
761
762 return planes;
763}
764
765Vector<Plane> Geometry3D::build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis) {
766 ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
767
768 Vector<Plane> planes;
769
770 Vector3 axis;
771 axis[p_axis] = 1.0;
772
773 Vector3 axis_neg;
774 axis_neg[(p_axis + 1) % 3] = 1.0;
775 axis_neg[(p_axis + 2) % 3] = 1.0;
776 axis_neg[p_axis] = -1.0;
777
778 const double lon_step = Math_TAU / p_lons;
779 for (int i = 0; i < p_lons; i++) {
780 Vector3 normal;
781 normal[(p_axis + 1) % 3] = Math::cos(i * lon_step);
782 normal[(p_axis + 2) % 3] = Math::sin(i * lon_step);
783
784 planes.push_back(Plane(normal, p_radius));
785
786 for (int j = 1; j <= p_lats; j++) {
787 Vector3 plane_normal = normal.lerp(axis, j / (real_t)p_lats).normalized();
788 planes.push_back(Plane(plane_normal, p_radius));
789 planes.push_back(Plane(plane_normal * axis_neg, p_radius));
790 }
791 }
792
793 return planes;
794}
795
796Vector<Plane> Geometry3D::build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis) {
797 ERR_FAIL_INDEX_V(p_axis, 3, Vector<Plane>());
798
799 Vector<Plane> planes;
800
801 Vector3 axis;
802 axis[p_axis] = 1.0;
803
804 Vector3 axis_neg;
805 axis_neg[(p_axis + 1) % 3] = 1.0;
806 axis_neg[(p_axis + 2) % 3] = 1.0;
807 axis_neg[p_axis] = -1.0;
808
809 const double sides_step = Math_TAU / p_sides;
810 for (int i = 0; i < p_sides; i++) {
811 Vector3 normal;
812 normal[(p_axis + 1) % 3] = Math::cos(i * sides_step);
813 normal[(p_axis + 2) % 3] = Math::sin(i * sides_step);
814
815 planes.push_back(Plane(normal, p_radius));
816
817 for (int j = 1; j <= p_lats; j++) {
818 Vector3 plane_normal = normal.lerp(axis, j / (real_t)p_lats).normalized();
819 Vector3 position = axis * p_height * 0.5f + plane_normal * p_radius;
820 planes.push_back(Plane(plane_normal, position));
821 planes.push_back(Plane(plane_normal * axis_neg, position * axis_neg));
822 }
823 }
824
825 return planes;
826}
827
828Vector<Vector3> Geometry3D::compute_convex_mesh_points(const Plane *p_planes, int p_plane_count) {
829 Vector<Vector3> points;
830
831 // Iterate through every unique combination of any three planes.
832 for (int i = p_plane_count - 1; i >= 0; i--) {
833 for (int j = i - 1; j >= 0; j--) {
834 for (int k = j - 1; k >= 0; k--) {
835 // Find the point where these planes all cross over (if they
836 // do at all).
837 Vector3 convex_shape_point;
838 if (p_planes[i].intersect_3(p_planes[j], p_planes[k], &convex_shape_point)) {
839 // See if any *other* plane excludes this point because it's
840 // on the wrong side.
841 bool excluded = false;
842 for (int n = 0; n < p_plane_count; n++) {
843 if (n != i && n != j && n != k) {
844 real_t dp = p_planes[n].normal.dot(convex_shape_point);
845 if (dp - p_planes[n].d > (real_t)CMP_EPSILON) {
846 excluded = true;
847 break;
848 }
849 }
850 }
851
852 // Only add the point if it passed all tests.
853 if (!excluded) {
854 points.push_back(convex_shape_point);
855 }
856 }
857 }
858 }
859 }
860
861 return points;
862}
863
864#define square(m_s) ((m_s) * (m_s))
865#define INF 1e20
866
867/* dt of 1d function using squared distance */
868static void edt(float *f, int stride, int n) {
869 float *d = (float *)alloca(sizeof(float) * n + sizeof(int) * n + sizeof(float) * (n + 1));
870 int *v = reinterpret_cast<int *>(&(d[n]));
871 float *z = reinterpret_cast<float *>(&v[n]);
872
873 int k = 0;
874 v[0] = 0;
875 z[0] = -INF;
876 z[1] = +INF;
877 for (int q = 1; q <= n - 1; q++) {
878 float s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]);
879 while (s <= z[k]) {
880 k--;
881 s = ((f[q * stride] + square(q)) - (f[v[k] * stride] + square(v[k]))) / (2 * q - 2 * v[k]);
882 }
883 k++;
884 v[k] = q;
885
886 z[k] = s;
887 z[k + 1] = +INF;
888 }
889
890 k = 0;
891 for (int q = 0; q <= n - 1; q++) {
892 while (z[k + 1] < q) {
893 k++;
894 }
895 d[q] = square(q - v[k]) + f[v[k] * stride];
896 }
897
898 for (int i = 0; i < n; i++) {
899 f[i * stride] = d[i];
900 }
901}
902
903#undef square
904
905Vector<uint32_t> Geometry3D::generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative) {
906 uint32_t float_count = p_size.x * p_size.y * p_size.z;
907
908 ERR_FAIL_COND_V((uint32_t)p_voxels.size() != float_count, Vector<uint32_t>());
909
910 float *work_memory = memnew_arr(float, float_count);
911 for (uint32_t i = 0; i < float_count; i++) {
912 work_memory[i] = INF;
913 }
914
915 uint32_t y_mult = p_size.x;
916 uint32_t z_mult = y_mult * p_size.y;
917
918 //plot solid cells
919 {
920 const bool *voxr = p_voxels.ptr();
921 for (uint32_t i = 0; i < float_count; i++) {
922 bool plot = voxr[i];
923 if (p_negative) {
924 plot = !plot;
925 }
926 if (plot) {
927 work_memory[i] = 0;
928 }
929 }
930 }
931
932 //process in each direction
933
934 //xy->z
935
936 for (int i = 0; i < p_size.x; i++) {
937 for (int j = 0; j < p_size.y; j++) {
938 edt(&work_memory[i + j * y_mult], z_mult, p_size.z);
939 }
940 }
941
942 //xz->y
943
944 for (int i = 0; i < p_size.x; i++) {
945 for (int j = 0; j < p_size.z; j++) {
946 edt(&work_memory[i + j * z_mult], y_mult, p_size.y);
947 }
948 }
949
950 //yz->x
951 for (int i = 0; i < p_size.y; i++) {
952 for (int j = 0; j < p_size.z; j++) {
953 edt(&work_memory[i * y_mult + j * z_mult], 1, p_size.x);
954 }
955 }
956
957 Vector<uint32_t> ret;
958 ret.resize(float_count);
959 {
960 uint32_t *w = ret.ptrw();
961 for (uint32_t i = 0; i < float_count; i++) {
962 w[i] = uint32_t(Math::sqrt(work_memory[i]));
963 }
964 }
965
966 memdelete_arr(work_memory);
967
968 return ret;
969}
970
971Vector<int8_t> Geometry3D::generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative) {
972 ERR_FAIL_COND_V(p_positive.size() != p_negative.size(), Vector<int8_t>());
973 Vector<int8_t> sdf8;
974 int s = p_positive.size();
975 sdf8.resize(s);
976
977 const uint32_t *rpos = p_positive.ptr();
978 const uint32_t *rneg = p_negative.ptr();
979 int8_t *wsdf = sdf8.ptrw();
980 for (int i = 0; i < s; i++) {
981 int32_t diff = int32_t(rpos[i]) - int32_t(rneg[i]);
982 wsdf[i] = CLAMP(diff, -128, 127);
983 }
984 return sdf8;
985}
986