1/**************************************************************************/
2/* delaunay_3d.h */
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#ifndef DELAUNAY_3D_H
32#define DELAUNAY_3D_H
33
34#include "core/io/file_access.h"
35#include "core/math/aabb.h"
36#include "core/math/projection.h"
37#include "core/math/vector3.h"
38#include "core/templates/local_vector.h"
39#include "core/templates/oa_hash_map.h"
40#include "core/templates/vector.h"
41#include "core/variant/variant.h"
42
43#include "thirdparty/misc/r128.h"
44
45class Delaunay3D {
46 struct Simplex;
47
48 enum {
49 ACCEL_GRID_SIZE = 16
50 };
51 struct GridPos {
52 Vector3i pos;
53 List<Simplex *>::Element *E = nullptr;
54 };
55
56 struct Simplex {
57 uint32_t points[4];
58 R128 circum_center_x;
59 R128 circum_center_y;
60 R128 circum_center_z;
61 R128 circum_r2;
62 LocalVector<GridPos> grid_positions;
63 List<Simplex *>::Element *SE = nullptr;
64
65 _FORCE_INLINE_ Simplex() {}
66 _FORCE_INLINE_ Simplex(uint32_t p_a, uint32_t p_b, uint32_t p_c, uint32_t p_d) {
67 points[0] = p_a;
68 points[1] = p_b;
69 points[2] = p_c;
70 points[3] = p_d;
71 }
72 };
73
74 struct Triangle {
75 uint32_t triangle[3];
76 bool bad = false;
77 _FORCE_INLINE_ bool operator==(const Triangle &p_triangle) const {
78 return triangle[0] == p_triangle.triangle[0] && triangle[1] == p_triangle.triangle[1] && triangle[2] == p_triangle.triangle[2];
79 }
80
81 _FORCE_INLINE_ Triangle() {}
82 _FORCE_INLINE_ Triangle(uint32_t p_a, uint32_t p_b, uint32_t p_c) {
83 if (p_a > p_b) {
84 SWAP(p_a, p_b);
85 }
86 if (p_b > p_c) {
87 SWAP(p_b, p_c);
88 }
89 if (p_a > p_b) {
90 SWAP(p_a, p_b);
91 }
92
93 triangle[0] = p_a;
94 triangle[1] = p_b;
95 triangle[2] = p_c;
96 }
97 };
98
99 struct TriangleHasher {
100 _FORCE_INLINE_ static uint32_t hash(const Triangle &p_triangle) {
101 uint32_t h = hash_djb2_one_32(p_triangle.triangle[0]);
102 h = hash_djb2_one_32(p_triangle.triangle[1], h);
103 return hash_fmix32(hash_djb2_one_32(p_triangle.triangle[2], h));
104 }
105 };
106
107 _FORCE_INLINE_ static void circum_sphere_compute(const Vector3 *p_points, Simplex *p_simplex) {
108 // The only part in the algorithm where there may be precision errors is this one,
109 // so ensure that we do it with the maximum precision possible.
110
111 R128 v0_x = p_points[p_simplex->points[0]].x;
112 R128 v0_y = p_points[p_simplex->points[0]].y;
113 R128 v0_z = p_points[p_simplex->points[0]].z;
114 R128 v1_x = p_points[p_simplex->points[1]].x;
115 R128 v1_y = p_points[p_simplex->points[1]].y;
116 R128 v1_z = p_points[p_simplex->points[1]].z;
117 R128 v2_x = p_points[p_simplex->points[2]].x;
118 R128 v2_y = p_points[p_simplex->points[2]].y;
119 R128 v2_z = p_points[p_simplex->points[2]].z;
120 R128 v3_x = p_points[p_simplex->points[3]].x;
121 R128 v3_y = p_points[p_simplex->points[3]].y;
122 R128 v3_z = p_points[p_simplex->points[3]].z;
123
124 // Create the rows of our "unrolled" 3x3 matrix.
125 R128 row1_x = v1_x - v0_x;
126 R128 row1_y = v1_y - v0_y;
127 R128 row1_z = v1_z - v0_z;
128
129 R128 row2_x = v2_x - v0_x;
130 R128 row2_y = v2_y - v0_y;
131 R128 row2_z = v2_z - v0_z;
132
133 R128 row3_x = v3_x - v0_x;
134 R128 row3_y = v3_y - v0_y;
135 R128 row3_z = v3_z - v0_z;
136
137 R128 sq_lenght1 = row1_x * row1_x + row1_y * row1_y + row1_z * row1_z;
138 R128 sq_lenght2 = row2_x * row2_x + row2_y * row2_y + row2_z * row2_z;
139 R128 sq_lenght3 = row3_x * row3_x + row3_y * row3_y + row3_z * row3_z;
140
141 // Compute the determinant of said matrix.
142 R128 determinant = row1_x * (row2_y * row3_z - row3_y * row2_z) - row2_x * (row1_y * row3_z - row3_y * row1_z) + row3_x * (row1_y * row2_z - row2_y * row1_z);
143
144 // Compute the volume of the tetrahedron, and precompute a scalar quantity for reuse in the formula.
145 R128 volume = determinant / R128(6.f);
146 R128 i12volume = R128(1.f) / (volume * R128(12.f));
147
148 R128 center_x = v0_x + i12volume * ((row2_y * row3_z - row3_y * row2_z) * sq_lenght1 - (row1_y * row3_z - row3_y * row1_z) * sq_lenght2 + (row1_y * row2_z - row2_y * row1_z) * sq_lenght3);
149 R128 center_y = v0_y + i12volume * (-(row2_x * row3_z - row3_x * row2_z) * sq_lenght1 + (row1_x * row3_z - row3_x * row1_z) * sq_lenght2 - (row1_x * row2_z - row2_x * row1_z) * sq_lenght3);
150 R128 center_z = v0_z + i12volume * ((row2_x * row3_y - row3_x * row2_y) * sq_lenght1 - (row1_x * row3_y - row3_x * row1_y) * sq_lenght2 + (row1_x * row2_y - row2_x * row1_y) * sq_lenght3);
151
152 // Once we know the center, the radius is clearly the distance to any vertex.
153 R128 rel1_x = center_x - v0_x;
154 R128 rel1_y = center_y - v0_y;
155 R128 rel1_z = center_z - v0_z;
156
157 R128 radius1 = rel1_x * rel1_x + rel1_y * rel1_y + rel1_z * rel1_z;
158
159 p_simplex->circum_center_x = center_x;
160 p_simplex->circum_center_y = center_y;
161 p_simplex->circum_center_z = center_z;
162 p_simplex->circum_r2 = radius1;
163 }
164
165 _FORCE_INLINE_ static bool simplex_contains(const Vector3 *p_points, const Simplex &p_simplex, uint32_t p_vertex) {
166 R128 v_x = p_points[p_vertex].x;
167 R128 v_y = p_points[p_vertex].y;
168 R128 v_z = p_points[p_vertex].z;
169
170 R128 rel2_x = p_simplex.circum_center_x - v_x;
171 R128 rel2_y = p_simplex.circum_center_y - v_y;
172 R128 rel2_z = p_simplex.circum_center_z - v_z;
173
174 R128 radius2 = rel2_x * rel2_x + rel2_y * rel2_y + rel2_z * rel2_z;
175
176 return radius2 < (p_simplex.circum_r2 - R128(0.00001));
177 }
178
179 static bool simplex_is_coplanar(const Vector3 *p_points, const Simplex &p_simplex) {
180 Plane p(p_points[p_simplex.points[0]], p_points[p_simplex.points[1]], p_points[p_simplex.points[2]]);
181 if (ABS(p.distance_to(p_points[p_simplex.points[3]])) < CMP_EPSILON) {
182 return true;
183 }
184
185 Projection cm;
186
187 cm.columns[0][0] = p_points[p_simplex.points[0]].x;
188 cm.columns[0][1] = p_points[p_simplex.points[1]].x;
189 cm.columns[0][2] = p_points[p_simplex.points[2]].x;
190 cm.columns[0][3] = p_points[p_simplex.points[3]].x;
191
192 cm.columns[1][0] = p_points[p_simplex.points[0]].y;
193 cm.columns[1][1] = p_points[p_simplex.points[1]].y;
194 cm.columns[1][2] = p_points[p_simplex.points[2]].y;
195 cm.columns[1][3] = p_points[p_simplex.points[3]].y;
196
197 cm.columns[2][0] = p_points[p_simplex.points[0]].z;
198 cm.columns[2][1] = p_points[p_simplex.points[1]].z;
199 cm.columns[2][2] = p_points[p_simplex.points[2]].z;
200 cm.columns[2][3] = p_points[p_simplex.points[3]].z;
201
202 cm.columns[3][0] = 1.0;
203 cm.columns[3][1] = 1.0;
204 cm.columns[3][2] = 1.0;
205 cm.columns[3][3] = 1.0;
206
207 return ABS(cm.determinant()) <= CMP_EPSILON;
208 }
209
210public:
211 struct OutputSimplex {
212 uint32_t points[4];
213 };
214
215 static Vector<OutputSimplex> tetrahedralize(const Vector<Vector3> &p_points) {
216 uint32_t point_count = p_points.size();
217 Vector3 *points = (Vector3 *)memalloc(sizeof(Vector3) * (point_count + 4));
218
219 {
220 const Vector3 *src_points = p_points.ptr();
221 AABB rect;
222 for (uint32_t i = 0; i < point_count; i++) {
223 Vector3 point = src_points[i];
224 if (i == 0) {
225 rect.position = point;
226 } else {
227 rect.expand_to(point);
228 }
229 points[i] = point;
230 }
231
232 for (uint32_t i = 0; i < point_count; i++) {
233 points[i] = (points[i] - rect.position) / rect.size;
234 }
235
236 float delta_max = Math::sqrt(2.0) * 20.0;
237 Vector3 center = Vector3(0.5, 0.5, 0.5);
238
239 // any simplex that contains everything is good
240 points[point_count + 0] = center + Vector3(0, 1, 0) * delta_max;
241 points[point_count + 1] = center + Vector3(0, -1, 1) * delta_max;
242 points[point_count + 2] = center + Vector3(1, -1, -1) * delta_max;
243 points[point_count + 3] = center + Vector3(-1, -1, -1) * delta_max;
244 }
245
246 List<Simplex *> acceleration_grid[ACCEL_GRID_SIZE][ACCEL_GRID_SIZE][ACCEL_GRID_SIZE];
247
248 List<Simplex *> simplex_list;
249 {
250 //create root simplex
251 Simplex *root = memnew(Simplex(point_count + 0, point_count + 1, point_count + 2, point_count + 3));
252 root->SE = simplex_list.push_back(root);
253
254 for (uint32_t i = 0; i < ACCEL_GRID_SIZE; i++) {
255 for (uint32_t j = 0; j < ACCEL_GRID_SIZE; j++) {
256 for (uint32_t k = 0; k < ACCEL_GRID_SIZE; k++) {
257 GridPos gp;
258 gp.E = acceleration_grid[i][j][k].push_back(root);
259 gp.pos = Vector3i(i, j, k);
260 root->grid_positions.push_back(gp);
261 }
262 }
263 }
264
265 circum_sphere_compute(points, root);
266 }
267
268 OAHashMap<Triangle, uint32_t, TriangleHasher> triangles_inserted;
269 LocalVector<Triangle> triangles;
270
271 for (uint32_t i = 0; i < point_count; i++) {
272 bool unique = true;
273 for (uint32_t j = i + 1; j < point_count; j++) {
274 if (points[i].is_equal_approx(points[j])) {
275 unique = false;
276 break;
277 }
278 }
279 if (!unique) {
280 continue;
281 }
282
283 Vector3i grid_pos = Vector3i(points[i] * ACCEL_GRID_SIZE);
284 grid_pos.x = CLAMP(grid_pos.x, 0, ACCEL_GRID_SIZE - 1);
285 grid_pos.y = CLAMP(grid_pos.y, 0, ACCEL_GRID_SIZE - 1);
286 grid_pos.z = CLAMP(grid_pos.z, 0, ACCEL_GRID_SIZE - 1);
287
288 for (List<Simplex *>::Element *E = acceleration_grid[grid_pos.x][grid_pos.y][grid_pos.z].front(); E;) {
289 List<Simplex *>::Element *N = E->next(); //may be deleted
290
291 Simplex *simplex = E->get();
292
293 if (simplex_contains(points, *simplex, i)) {
294 static const uint32_t triangle_order[4][3] = {
295 { 0, 1, 2 },
296 { 0, 1, 3 },
297 { 0, 2, 3 },
298 { 1, 2, 3 },
299 };
300
301 for (uint32_t k = 0; k < 4; k++) {
302 Triangle t = Triangle(simplex->points[triangle_order[k][0]], simplex->points[triangle_order[k][1]], simplex->points[triangle_order[k][2]]);
303 uint32_t *p = triangles_inserted.lookup_ptr(t);
304 if (p) {
305 triangles[*p].bad = true;
306 } else {
307 triangles_inserted.insert(t, triangles.size());
308 triangles.push_back(t);
309 }
310 }
311
312 //remove simplex and continue
313 simplex_list.erase(simplex->SE);
314
315 for (const GridPos &gp : simplex->grid_positions) {
316 Vector3i p = gp.pos;
317 acceleration_grid[p.x][p.y][p.z].erase(gp.E);
318 }
319 memdelete(simplex);
320 }
321 E = N;
322 }
323
324 for (const Triangle &triangle : triangles) {
325 if (triangle.bad) {
326 continue;
327 }
328 Simplex *new_simplex = memnew(Simplex(triangle.triangle[0], triangle.triangle[1], triangle.triangle[2], i));
329 circum_sphere_compute(points, new_simplex);
330 new_simplex->SE = simplex_list.push_back(new_simplex);
331 {
332 Vector3 center;
333 center.x = double(new_simplex->circum_center_x);
334 center.y = double(new_simplex->circum_center_y);
335 center.z = double(new_simplex->circum_center_z);
336
337 float radius2 = Math::sqrt(double(new_simplex->circum_r2));
338 radius2 += 0.0001; //
339 Vector3 extents = Vector3(radius2, radius2, radius2);
340 Vector3i from = Vector3i((center - extents) * ACCEL_GRID_SIZE);
341 Vector3i to = Vector3i((center + extents) * ACCEL_GRID_SIZE);
342 from.x = CLAMP(from.x, 0, ACCEL_GRID_SIZE - 1);
343 from.y = CLAMP(from.y, 0, ACCEL_GRID_SIZE - 1);
344 from.z = CLAMP(from.z, 0, ACCEL_GRID_SIZE - 1);
345 to.x = CLAMP(to.x, 0, ACCEL_GRID_SIZE - 1);
346 to.y = CLAMP(to.y, 0, ACCEL_GRID_SIZE - 1);
347 to.z = CLAMP(to.z, 0, ACCEL_GRID_SIZE - 1);
348
349 for (int32_t x = from.x; x <= to.x; x++) {
350 for (int32_t y = from.y; y <= to.y; y++) {
351 for (int32_t z = from.z; z <= to.z; z++) {
352 GridPos gp;
353 gp.pos = Vector3(x, y, z);
354 gp.E = acceleration_grid[x][y][z].push_back(new_simplex);
355 new_simplex->grid_positions.push_back(gp);
356 }
357 }
358 }
359 }
360 }
361
362 triangles.clear();
363 triangles_inserted.clear();
364 }
365
366 //print_line("end with simplices: " + itos(simplex_list.size()));
367 Vector<OutputSimplex> ret_simplices;
368 ret_simplices.resize(simplex_list.size());
369 OutputSimplex *ret_simplicesw = ret_simplices.ptrw();
370 uint32_t simplices_written = 0;
371
372 for (Simplex *simplex : simplex_list) {
373 bool invalid = false;
374 for (int j = 0; j < 4; j++) {
375 if (simplex->points[j] >= point_count) {
376 invalid = true;
377 break;
378 }
379 }
380 if (invalid || simplex_is_coplanar(points, *simplex)) {
381 memdelete(simplex);
382 continue;
383 }
384
385 ret_simplicesw[simplices_written].points[0] = simplex->points[0];
386 ret_simplicesw[simplices_written].points[1] = simplex->points[1];
387 ret_simplicesw[simplices_written].points[2] = simplex->points[2];
388 ret_simplicesw[simplices_written].points[3] = simplex->points[3];
389 simplices_written++;
390 memdelete(simplex);
391 }
392
393 ret_simplices.resize(simplices_written);
394
395 memfree(points);
396
397 return ret_simplices;
398 }
399};
400
401#endif // DELAUNAY_3D_H
402