1/**************************************************************************/
2/* quick_hull.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 "quick_hull.h"
32
33#include "core/templates/rb_map.h"
34
35uint32_t QuickHull::debug_stop_after = 0xFFFFFFFF;
36
37Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_mesh) {
38 /* CREATE AABB VOLUME */
39
40 AABB aabb;
41 for (int i = 0; i < p_points.size(); i++) {
42 if (i == 0) {
43 aabb.position = p_points[i];
44 } else {
45 aabb.expand_to(p_points[i]);
46 }
47 }
48
49 if (aabb.size == Vector3()) {
50 return ERR_CANT_CREATE;
51 }
52
53 Vector<bool> valid_points;
54 valid_points.resize(p_points.size());
55 HashSet<Vector3> valid_cache;
56
57 for (int i = 0; i < p_points.size(); i++) {
58 Vector3 sp = p_points[i].snapped(Vector3(0.0001, 0.0001, 0.0001));
59 if (valid_cache.has(sp)) {
60 valid_points.write[i] = false;
61 } else {
62 valid_points.write[i] = true;
63 valid_cache.insert(sp);
64 }
65 }
66
67 /* CREATE INITIAL SIMPLEX */
68
69 int longest_axis = aabb.get_longest_axis_index();
70
71 //first two vertices are the most distant
72 int simplex[4] = { 0 };
73
74 {
75 real_t max = 0, min = 0;
76
77 for (int i = 0; i < p_points.size(); i++) {
78 if (!valid_points[i]) {
79 continue;
80 }
81 real_t d = p_points[i][longest_axis];
82 if (i == 0 || d < min) {
83 simplex[0] = i;
84 min = d;
85 }
86
87 if (i == 0 || d > max) {
88 simplex[1] = i;
89 max = d;
90 }
91 }
92 }
93
94 //third vertex is one most further away from the line
95
96 {
97 real_t maxd = 0;
98 Vector3 rel12 = p_points[simplex[0]] - p_points[simplex[1]];
99
100 for (int i = 0; i < p_points.size(); i++) {
101 if (!valid_points[i]) {
102 continue;
103 }
104
105 Vector3 n = rel12.cross(p_points[simplex[0]] - p_points[i]).cross(rel12).normalized();
106 real_t d = Math::abs(n.dot(p_points[simplex[0]]) - n.dot(p_points[i]));
107
108 if (i == 0 || d > maxd) {
109 maxd = d;
110 simplex[2] = i;
111 }
112 }
113 }
114
115 //fourth vertex is the one most further away from the plane
116
117 {
118 real_t maxd = 0;
119 Plane p(p_points[simplex[0]], p_points[simplex[1]], p_points[simplex[2]]);
120
121 for (int i = 0; i < p_points.size(); i++) {
122 if (!valid_points[i]) {
123 continue;
124 }
125
126 real_t d = Math::abs(p.distance_to(p_points[i]));
127
128 if (i == 0 || d > maxd) {
129 maxd = d;
130 simplex[3] = i;
131 }
132 }
133 }
134
135 //compute center of simplex, this is a point always warranted to be inside
136 Vector3 center;
137
138 for (int i = 0; i < 4; i++) {
139 center += p_points[simplex[i]];
140 }
141
142 center /= 4.0;
143
144 //add faces
145
146 List<Face> faces;
147
148 for (int i = 0; i < 4; i++) {
149 static const int face_order[4][3] = {
150 { 0, 1, 2 },
151 { 0, 1, 3 },
152 { 0, 2, 3 },
153 { 1, 2, 3 }
154 };
155
156 Face f;
157 for (int j = 0; j < 3; j++) {
158 f.vertices[j] = simplex[face_order[i][j]];
159 }
160
161 Plane p(p_points[f.vertices[0]], p_points[f.vertices[1]], p_points[f.vertices[2]]);
162
163 if (p.is_point_over(center)) {
164 //flip face to clockwise if facing inwards
165 SWAP(f.vertices[0], f.vertices[1]);
166 p = -p;
167 }
168
169 f.plane = p;
170
171 faces.push_back(f);
172 }
173
174 real_t over_tolerance = 3 * UNIT_EPSILON * (aabb.size.x + aabb.size.y + aabb.size.z);
175
176 /* COMPUTE AVAILABLE VERTICES */
177
178 for (int i = 0; i < p_points.size(); i++) {
179 if (i == simplex[0]) {
180 continue;
181 }
182 if (i == simplex[1]) {
183 continue;
184 }
185 if (i == simplex[2]) {
186 continue;
187 }
188 if (i == simplex[3]) {
189 continue;
190 }
191 if (!valid_points[i]) {
192 continue;
193 }
194
195 for (Face &E : faces) {
196 if (E.plane.distance_to(p_points[i]) > over_tolerance) {
197 E.points_over.push_back(i);
198 break;
199 }
200 }
201 }
202
203 faces.sort(); // sort them, so the ones with points are in the back
204
205 /* BUILD HULL */
206
207 //poop face (while still remain)
208 //find further away point
209 //find lit faces
210 //determine horizon edges
211 //build new faces with horizon edges, them assign points side from all lit faces
212 //remove lit faces
213
214 uint32_t debug_stop = debug_stop_after;
215
216 while (debug_stop > 0 && faces.back()->get().points_over.size()) {
217 debug_stop--;
218 Face &f = faces.back()->get();
219
220 //find vertex most outside
221 int next = -1;
222 real_t next_d = 0;
223
224 for (int i = 0; i < f.points_over.size(); i++) {
225 real_t d = f.plane.distance_to(p_points[f.points_over[i]]);
226
227 if (d > next_d) {
228 next_d = d;
229 next = i;
230 }
231 }
232
233 ERR_FAIL_COND_V(next == -1, ERR_BUG);
234
235 Vector3 v = p_points[f.points_over[next]];
236
237 //find lit faces and lit edges
238 List<List<Face>::Element *> lit_faces; //lit face is a death sentence
239
240 HashMap<Edge, FaceConnect, Edge> lit_edges; //create this on the flight, should not be that bad for performance and simplifies code a lot
241
242 for (List<Face>::Element *E = faces.front(); E; E = E->next()) {
243 if (E->get().plane.distance_to(v) > 0) {
244 lit_faces.push_back(E);
245
246 for (int i = 0; i < 3; i++) {
247 uint32_t a = E->get().vertices[i];
248 uint32_t b = E->get().vertices[(i + 1) % 3];
249 Edge e(a, b);
250
251 HashMap<Edge, FaceConnect, Edge>::Iterator F = lit_edges.find(e);
252 if (!F) {
253 F = lit_edges.insert(e, FaceConnect());
254 }
255 if (e.vertices[0] == a) {
256 //left
257 F->value.left = E;
258 } else {
259 F->value.right = E;
260 }
261 }
262 }
263 }
264
265 //create new faces from horizon edges
266 List<List<Face>::Element *> new_faces; //new faces
267
268 for (KeyValue<Edge, FaceConnect> &E : lit_edges) {
269 FaceConnect &fc = E.value;
270 if (fc.left && fc.right) {
271 continue; //edge is uninteresting, not on horizon
272 }
273
274 //create new face!
275
276 Face face;
277 face.vertices[0] = f.points_over[next];
278 face.vertices[1] = E.key.vertices[0];
279 face.vertices[2] = E.key.vertices[1];
280
281 Plane p(p_points[face.vertices[0]], p_points[face.vertices[1]], p_points[face.vertices[2]]);
282
283 if (p.is_point_over(center)) {
284 //flip face to clockwise if facing inwards
285 SWAP(face.vertices[0], face.vertices[1]);
286 p = -p;
287 }
288
289 face.plane = p;
290 new_faces.push_back(faces.push_back(face));
291 }
292
293 //distribute points into new faces
294
295 for (List<Face>::Element *&F : lit_faces) {
296 Face &lf = F->get();
297
298 for (int i = 0; i < lf.points_over.size(); i++) {
299 if (lf.points_over[i] == f.points_over[next]) { //do not add current one
300 continue;
301 }
302
303 Vector3 p = p_points[lf.points_over[i]];
304 for (List<Face>::Element *&E : new_faces) {
305 Face &f2 = E->get();
306 if (f2.plane.distance_to(p) > over_tolerance) {
307 f2.points_over.push_back(lf.points_over[i]);
308 break;
309 }
310 }
311 }
312 }
313
314 //erase lit faces
315
316 while (lit_faces.size()) {
317 faces.erase(lit_faces.front()->get());
318 lit_faces.pop_front();
319 }
320
321 //put faces that contain no points on the front
322
323 for (List<Face>::Element *&E : new_faces) {
324 Face &f2 = E->get();
325 if (f2.points_over.size() == 0) {
326 faces.move_to_front(E);
327 }
328 }
329
330 //whew, done with iteration, go next
331 }
332
333 /* CREATE MESHDATA */
334
335 //make a map of edges again
336 HashMap<Edge, RetFaceConnect, Edge> ret_edges;
337 List<Geometry3D::MeshData::Face> ret_faces;
338
339 for (const Face &E : faces) {
340 Geometry3D::MeshData::Face f;
341 f.plane = E.plane;
342
343 for (int i = 0; i < 3; i++) {
344 f.indices.push_back(E.vertices[i]);
345 }
346
347 List<Geometry3D::MeshData::Face>::Element *F = ret_faces.push_back(f);
348
349 for (int i = 0; i < 3; i++) {
350 uint32_t a = E.vertices[i];
351 uint32_t b = E.vertices[(i + 1) % 3];
352 Edge e(a, b);
353
354 HashMap<Edge, RetFaceConnect, Edge>::Iterator G = ret_edges.find(e);
355 if (!G) {
356 G = ret_edges.insert(e, RetFaceConnect());
357 }
358 if (e.vertices[0] == a) {
359 //left
360 G->value.left = F;
361 } else {
362 G->value.right = F;
363 }
364 }
365 }
366
367 //fill faces
368
369 for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
370 Geometry3D::MeshData::Face &f = E->get();
371
372 for (uint32_t i = 0; i < f.indices.size(); i++) {
373 int a = E->get().indices[i];
374 int b = E->get().indices[(i + 1) % f.indices.size()];
375 Edge e(a, b);
376
377 HashMap<Edge, RetFaceConnect, Edge>::Iterator F = ret_edges.find(e);
378
379 ERR_CONTINUE(!F);
380 List<Geometry3D::MeshData::Face>::Element *O = F->value.left == E ? F->value.right : F->value.left;
381 ERR_CONTINUE(O == E);
382 ERR_CONTINUE(O == nullptr);
383
384 if (O->get().plane.is_equal_approx(f.plane)) {
385 //merge and delete edge and contiguous face, while repointing edges (uuugh!)
386 int o_index_size = O->get().indices.size();
387
388 for (int j = 0; j < o_index_size; j++) {
389 //search a
390 if (O->get().indices[j] == a) {
391 //append the rest
392 for (int k = 0; k < o_index_size; k++) {
393 int idx = O->get().indices[(k + j) % o_index_size];
394 int idxn = O->get().indices[(k + j + 1) % o_index_size];
395 if (idx == b && idxn == a) { //already have b!
396 break;
397 }
398 if (idx != a) {
399 f.indices.insert(i + 1, idx);
400 i++;
401 }
402 Edge e2(idx, idxn);
403
404 HashMap<Edge, RetFaceConnect, Edge>::Iterator F2 = ret_edges.find(e2);
405 ERR_CONTINUE(!F2);
406 //change faceconnect, point to this face instead
407 if (F2->value.left == O) {
408 F2->value.left = E;
409 } else if (F2->value.right == O) {
410 F2->value.right = E;
411 }
412 }
413
414 break;
415 }
416 }
417
418 // remove all edge connections to this face
419 for (KeyValue<Edge, RetFaceConnect> &G : ret_edges) {
420 if (G.value.left == O) {
421 G.value.left = nullptr;
422 }
423
424 if (G.value.right == O) {
425 G.value.right = nullptr;
426 }
427 }
428
429 ret_edges.remove(F); //remove the edge
430 ret_faces.erase(O); //remove the face
431 }
432 }
433 }
434
435 //fill mesh
436 r_mesh.faces.clear();
437 r_mesh.faces.resize(ret_faces.size());
438
439 HashMap<List<Geometry3D::MeshData::Face>::Element *, int> face_indices;
440
441 int idx = 0;
442 for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
443 face_indices[E] = idx;
444 r_mesh.faces[idx++] = E->get();
445 }
446 r_mesh.edges.resize(ret_edges.size());
447 idx = 0;
448 for (const KeyValue<Edge, RetFaceConnect> &E : ret_edges) {
449 Geometry3D::MeshData::Edge e;
450 e.vertex_a = E.key.vertices[0];
451 e.vertex_b = E.key.vertices[1];
452 ERR_CONTINUE(!face_indices.has(E.value.left));
453 ERR_CONTINUE(!face_indices.has(E.value.right));
454 e.face_a = face_indices[E.value.left];
455 e.face_b = face_indices[E.value.right];
456 r_mesh.edges[idx++] = e;
457 }
458
459 r_mesh.vertices = p_points;
460
461 return OK;
462}
463