1/**************************************************************************/
2/* triangle_mesh.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 "triangle_mesh.h"
32
33#include "core/templates/sort_array.h"
34
35int TriangleMesh::_create_bvh(BVH *p_bvh, BVH **p_bb, int p_from, int p_size, int p_depth, int &r_max_depth, int &r_max_alloc) {
36 if (p_depth > r_max_depth) {
37 r_max_depth = p_depth;
38 }
39
40 if (p_size == 1) {
41 return p_bb[p_from] - p_bvh;
42 } else if (p_size == 0) {
43 return -1;
44 }
45
46 AABB aabb;
47 aabb = p_bb[p_from]->aabb;
48 for (int i = 1; i < p_size; i++) {
49 aabb.merge_with(p_bb[p_from + i]->aabb);
50 }
51
52 int li = aabb.get_longest_axis_index();
53
54 switch (li) {
55 case Vector3::AXIS_X: {
56 SortArray<BVH *, BVHCmpX> sort_x;
57 sort_x.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
58 //sort_x.sort(&p_bb[p_from],p_size);
59 } break;
60 case Vector3::AXIS_Y: {
61 SortArray<BVH *, BVHCmpY> sort_y;
62 sort_y.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
63 //sort_y.sort(&p_bb[p_from],p_size);
64 } break;
65 case Vector3::AXIS_Z: {
66 SortArray<BVH *, BVHCmpZ> sort_z;
67 sort_z.nth_element(0, p_size, p_size / 2, &p_bb[p_from]);
68 //sort_z.sort(&p_bb[p_from],p_size);
69
70 } break;
71 }
72
73 int left = _create_bvh(p_bvh, p_bb, p_from, p_size / 2, p_depth + 1, r_max_depth, r_max_alloc);
74 int right = _create_bvh(p_bvh, p_bb, p_from + p_size / 2, p_size - p_size / 2, p_depth + 1, r_max_depth, r_max_alloc);
75
76 int index = r_max_alloc++;
77 BVH *_new = &p_bvh[index];
78 _new->aabb = aabb;
79 _new->center = aabb.get_center();
80 _new->face_index = -1;
81 _new->left = left;
82 _new->right = right;
83
84 return index;
85}
86
87void TriangleMesh::get_indices(Vector<int> *r_triangles_indices) const {
88 if (!valid) {
89 return;
90 }
91
92 const int triangles_num = triangles.size();
93
94 // Parse vertices indices
95 const Triangle *triangles_read = triangles.ptr();
96
97 r_triangles_indices->resize(triangles_num * 3);
98 int *r_indices_write = r_triangles_indices->ptrw();
99
100 for (int i = 0; i < triangles_num; ++i) {
101 r_indices_write[3 * i + 0] = triangles_read[i].indices[0];
102 r_indices_write[3 * i + 1] = triangles_read[i].indices[1];
103 r_indices_write[3 * i + 2] = triangles_read[i].indices[2];
104 }
105}
106
107void TriangleMesh::create(const Vector<Vector3> &p_faces, const Vector<int32_t> &p_surface_indices) {
108 valid = false;
109
110 ERR_FAIL_COND(p_surface_indices.size() && p_surface_indices.size() != p_faces.size());
111
112 int fc = p_faces.size();
113 ERR_FAIL_COND(!fc || ((fc % 3) != 0));
114 fc /= 3;
115 triangles.resize(fc);
116
117 bvh.resize(fc * 3); //will never be larger than this (todo make better)
118 BVH *bw = bvh.ptrw();
119
120 {
121 //create faces and indices and base bvh
122 //except for the Set for repeated triangles, everything
123 //goes in-place.
124
125 const Vector3 *r = p_faces.ptr();
126 const int32_t *si = p_surface_indices.ptr();
127 Triangle *w = triangles.ptrw();
128 HashMap<Vector3, int> db;
129
130 for (int i = 0; i < fc; i++) {
131 Triangle &f = w[i];
132 const Vector3 *v = &r[i * 3];
133
134 for (int j = 0; j < 3; j++) {
135 int vidx = -1;
136 Vector3 vs = v[j].snapped(Vector3(0.0001, 0.0001, 0.0001));
137 HashMap<Vector3, int>::Iterator E = db.find(vs);
138 if (E) {
139 vidx = E->value;
140 } else {
141 vidx = db.size();
142 db[vs] = vidx;
143 }
144
145 f.indices[j] = vidx;
146 if (j == 0) {
147 bw[i].aabb.position = vs;
148 } else {
149 bw[i].aabb.expand_to(vs);
150 }
151 }
152
153 f.normal = Face3(r[i * 3 + 0], r[i * 3 + 1], r[i * 3 + 2]).get_plane().get_normal();
154 f.surface_index = si ? si[i] : 0;
155
156 bw[i].left = -1;
157 bw[i].right = -1;
158 bw[i].face_index = i;
159 bw[i].center = bw[i].aabb.get_center();
160 }
161
162 vertices.resize(db.size());
163 Vector3 *vw = vertices.ptrw();
164 for (const KeyValue<Vector3, int> &E : db) {
165 vw[E.value] = E.key;
166 }
167 }
168
169 Vector<BVH *> bwptrs;
170 bwptrs.resize(fc);
171 BVH **bwp = bwptrs.ptrw();
172 for (int i = 0; i < fc; i++) {
173 bwp[i] = &bw[i];
174 }
175
176 max_depth = 0;
177 int max_alloc = fc;
178 _create_bvh(bw, bwp, 0, fc, 1, max_depth, max_alloc);
179
180 bvh.resize(max_alloc); //resize back
181
182 valid = true;
183}
184
185bool TriangleMesh::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index) const {
186 uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
187
188 enum {
189 TEST_AABB_BIT = 0,
190 VISIT_LEFT_BIT = 1,
191 VISIT_RIGHT_BIT = 2,
192 VISIT_DONE_BIT = 3,
193 VISITED_BIT_SHIFT = 29,
194 NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
195 VISITED_BIT_MASK = ~NODE_IDX_MASK,
196
197 };
198
199 Vector3 n = (p_end - p_begin).normalized();
200 real_t d = 1e10;
201 bool inters = false;
202
203 int level = 0;
204
205 const Triangle *triangleptr = triangles.ptr();
206 const Vector3 *vertexptr = vertices.ptr();
207 const BVH *bvhptr = bvh.ptr();
208
209 int pos = bvh.size() - 1;
210
211 stack[0] = pos;
212 while (true) {
213 uint32_t node = stack[level] & NODE_IDX_MASK;
214 const BVH &b = bvhptr[node];
215 bool done = false;
216
217 switch (stack[level] >> VISITED_BIT_SHIFT) {
218 case TEST_AABB_BIT: {
219 if (!b.aabb.intersects_segment(p_begin, p_end)) {
220 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
221 } else {
222 if (b.face_index >= 0) {
223 const Triangle &s = triangleptr[b.face_index];
224 Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]);
225
226 Vector3 res;
227
228 if (f3.intersects_segment(p_begin, p_end, &res)) {
229 real_t nd = n.dot(res);
230 if (nd < d) {
231 d = nd;
232 r_point = res;
233 r_normal = f3.get_plane().get_normal();
234 if (r_surf_index) {
235 *r_surf_index = s.surface_index;
236 }
237 inters = true;
238 }
239 }
240
241 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
242
243 } else {
244 stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
245 }
246 }
247 continue;
248 }
249 case VISIT_LEFT_BIT: {
250 stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
251 level++;
252 stack[level] = b.left | TEST_AABB_BIT;
253 continue;
254 }
255 case VISIT_RIGHT_BIT: {
256 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
257 level++;
258 stack[level] = b.right | TEST_AABB_BIT;
259 continue;
260 }
261 case VISIT_DONE_BIT: {
262 if (level == 0) {
263 done = true;
264 break;
265 } else {
266 level--;
267 }
268 continue;
269 }
270 }
271
272 if (done) {
273 break;
274 }
275 }
276
277 if (inters) {
278 if (n.dot(r_normal) > 0) {
279 r_normal = -r_normal;
280 }
281 }
282
283 return inters;
284}
285
286bool TriangleMesh::intersect_ray(const Vector3 &p_begin, const Vector3 &p_dir, Vector3 &r_point, Vector3 &r_normal, int32_t *r_surf_index) const {
287 uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
288
289 enum {
290 TEST_AABB_BIT = 0,
291 VISIT_LEFT_BIT = 1,
292 VISIT_RIGHT_BIT = 2,
293 VISIT_DONE_BIT = 3,
294 VISITED_BIT_SHIFT = 29,
295 NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
296 VISITED_BIT_MASK = ~NODE_IDX_MASK,
297
298 };
299
300 Vector3 n = p_dir;
301 real_t d = 1e20;
302 bool inters = false;
303
304 int level = 0;
305
306 const Triangle *triangleptr = triangles.ptr();
307 const Vector3 *vertexptr = vertices.ptr();
308 const BVH *bvhptr = bvh.ptr();
309
310 int pos = bvh.size() - 1;
311
312 stack[0] = pos;
313 while (true) {
314 uint32_t node = stack[level] & NODE_IDX_MASK;
315 const BVH &b = bvhptr[node];
316 bool done = false;
317
318 switch (stack[level] >> VISITED_BIT_SHIFT) {
319 case TEST_AABB_BIT: {
320 if (!b.aabb.intersects_ray(p_begin, p_dir)) {
321 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
322 } else {
323 if (b.face_index >= 0) {
324 const Triangle &s = triangleptr[b.face_index];
325 Face3 f3(vertexptr[s.indices[0]], vertexptr[s.indices[1]], vertexptr[s.indices[2]]);
326
327 Vector3 res;
328
329 if (f3.intersects_ray(p_begin, p_dir, &res)) {
330 real_t nd = n.dot(res);
331 if (nd < d) {
332 d = nd;
333 r_point = res;
334 r_normal = f3.get_plane().get_normal();
335 if (r_surf_index) {
336 *r_surf_index = s.surface_index;
337 }
338 inters = true;
339 }
340 }
341
342 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
343
344 } else {
345 stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
346 }
347 }
348 continue;
349 }
350 case VISIT_LEFT_BIT: {
351 stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
352 level++;
353 stack[level] = b.left | TEST_AABB_BIT;
354 continue;
355 }
356 case VISIT_RIGHT_BIT: {
357 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
358 level++;
359 stack[level] = b.right | TEST_AABB_BIT;
360 continue;
361 }
362 case VISIT_DONE_BIT: {
363 if (level == 0) {
364 done = true;
365 break;
366 } else {
367 level--;
368 }
369 continue;
370 }
371 }
372
373 if (done) {
374 break;
375 }
376 }
377
378 if (inters) {
379 if (n.dot(r_normal) > 0) {
380 r_normal = -r_normal;
381 }
382 }
383
384 return inters;
385}
386
387bool TriangleMesh::inside_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count, Vector3 p_scale) const {
388 uint32_t *stack = (uint32_t *)alloca(sizeof(int) * max_depth);
389
390 enum {
391 TEST_AABB_BIT = 0,
392 VISIT_LEFT_BIT = 1,
393 VISIT_RIGHT_BIT = 2,
394 VISIT_DONE_BIT = 3,
395 VISITED_BIT_SHIFT = 29,
396 NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
397 VISITED_BIT_MASK = ~NODE_IDX_MASK,
398
399 };
400
401 int level = 0;
402
403 const Triangle *triangleptr = triangles.ptr();
404 const Vector3 *vertexptr = vertices.ptr();
405 const BVH *bvhptr = bvh.ptr();
406
407 Transform3D scale(Basis().scaled(p_scale));
408
409 int pos = bvh.size() - 1;
410
411 stack[0] = pos;
412 while (true) {
413 uint32_t node = stack[level] & NODE_IDX_MASK;
414 const BVH &b = bvhptr[node];
415 bool done = false;
416
417 switch (stack[level] >> VISITED_BIT_SHIFT) {
418 case TEST_AABB_BIT: {
419 bool intersects = scale.xform(b.aabb).intersects_convex_shape(p_planes, p_plane_count, p_points, p_point_count);
420 if (!intersects) {
421 return false;
422 }
423
424 bool inside = scale.xform(b.aabb).inside_convex_shape(p_planes, p_plane_count);
425 if (inside) {
426 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
427
428 } else {
429 if (b.face_index >= 0) {
430 const Triangle &s = triangleptr[b.face_index];
431 for (int j = 0; j < 3; ++j) {
432 Vector3 point = scale.xform(vertexptr[s.indices[j]]);
433 for (int i = 0; i < p_plane_count; i++) {
434 const Plane &p = p_planes[i];
435 if (p.is_point_over(point)) {
436 return false;
437 }
438 }
439 }
440
441 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
442
443 } else {
444 stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
445 }
446 }
447 continue;
448 }
449 case VISIT_LEFT_BIT: {
450 stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
451 level++;
452 stack[level] = b.left | TEST_AABB_BIT;
453 continue;
454 }
455 case VISIT_RIGHT_BIT: {
456 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
457 level++;
458 stack[level] = b.right | TEST_AABB_BIT;
459 continue;
460 }
461 case VISIT_DONE_BIT: {
462 if (level == 0) {
463 done = true;
464 break;
465 } else {
466 level--;
467 }
468 continue;
469 }
470 }
471
472 if (done) {
473 break;
474 }
475 }
476
477 return true;
478}
479
480bool TriangleMesh::is_valid() const {
481 return valid;
482}
483
484Vector<Face3> TriangleMesh::get_faces() const {
485 if (!valid) {
486 return Vector<Face3>();
487 }
488
489 Vector<Face3> faces;
490 int ts = triangles.size();
491 faces.resize(triangles.size());
492
493 Face3 *w = faces.ptrw();
494 const Triangle *r = triangles.ptr();
495 const Vector3 *rv = vertices.ptr();
496
497 for (int i = 0; i < ts; i++) {
498 for (int j = 0; j < 3; j++) {
499 w[i].vertex[j] = rv[r[i].indices[j]];
500 }
501 }
502
503 return faces;
504}
505
506TriangleMesh::TriangleMesh() {
507 valid = false;
508 max_depth = 0;
509}
510