1/**************************************************************************/
2/* geometry_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 GEOMETRY_3D_H
32#define GEOMETRY_3D_H
33
34#include "core/math/face3.h"
35#include "core/object/object.h"
36#include "core/templates/local_vector.h"
37#include "core/templates/vector.h"
38
39class Geometry3D {
40public:
41 static void 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);
42 static real_t get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1);
43
44 static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) {
45 Vector3 e1 = p_v1 - p_v0;
46 Vector3 e2 = p_v2 - p_v0;
47 Vector3 h = p_dir.cross(e2);
48 real_t a = e1.dot(h);
49 if (Math::is_zero_approx(a)) { // Parallel test.
50 return false;
51 }
52
53 real_t f = 1.0f / a;
54
55 Vector3 s = p_from - p_v0;
56 real_t u = f * s.dot(h);
57
58 if ((u < 0.0f) || (u > 1.0f)) {
59 return false;
60 }
61
62 Vector3 q = s.cross(e1);
63
64 real_t v = f * p_dir.dot(q);
65
66 if ((v < 0.0f) || (u + v > 1.0f)) {
67 return false;
68 }
69
70 // At this stage we can compute t to find out where
71 // the intersection point is on the line.
72 real_t t = f * e2.dot(q);
73
74 if (t > 0.00001f) { // ray intersection
75 if (r_res) {
76 *r_res = p_from + p_dir * t;
77 }
78 return true;
79 } else { // This means that there is a line intersection but not a ray intersection.
80 return false;
81 }
82 }
83
84 static inline bool segment_intersects_triangle(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) {
85 Vector3 rel = p_to - p_from;
86 Vector3 e1 = p_v1 - p_v0;
87 Vector3 e2 = p_v2 - p_v0;
88 Vector3 h = rel.cross(e2);
89 real_t a = e1.dot(h);
90 if (Math::is_zero_approx(a)) { // Parallel test.
91 return false;
92 }
93
94 real_t f = 1.0f / a;
95
96 Vector3 s = p_from - p_v0;
97 real_t u = f * s.dot(h);
98
99 if ((u < 0.0f) || (u > 1.0f)) {
100 return false;
101 }
102
103 Vector3 q = s.cross(e1);
104
105 real_t v = f * rel.dot(q);
106
107 if ((v < 0.0f) || (u + v > 1.0f)) {
108 return false;
109 }
110
111 // At this stage we can compute t to find out where
112 // the intersection point is on the line.
113 real_t t = f * e2.dot(q);
114
115 if (t > (real_t)CMP_EPSILON && t <= 1.0f) { // Ray intersection.
116 if (r_res) {
117 *r_res = p_from + rel * t;
118 }
119 return true;
120 } else { // This means that there is a line intersection but not a ray intersection.
121 return false;
122 }
123 }
124
125 static inline bool segment_intersects_sphere(const Vector3 &p_from, const Vector3 &p_to, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) {
126 Vector3 sphere_pos = p_sphere_pos - p_from;
127 Vector3 rel = (p_to - p_from);
128 real_t rel_l = rel.length();
129 if (rel_l < (real_t)CMP_EPSILON) {
130 return false; // Both points are the same.
131 }
132 Vector3 normal = rel / rel_l;
133
134 real_t sphere_d = normal.dot(sphere_pos);
135
136 real_t ray_distance = sphere_pos.distance_to(normal * sphere_d);
137
138 if (ray_distance >= p_sphere_radius) {
139 return false;
140 }
141
142 real_t inters_d2 = p_sphere_radius * p_sphere_radius - ray_distance * ray_distance;
143 real_t inters_d = sphere_d;
144
145 if (inters_d2 >= (real_t)CMP_EPSILON) {
146 inters_d -= Math::sqrt(inters_d2);
147 }
148
149 // Check in segment.
150 if (inters_d < 0 || inters_d > rel_l) {
151 return false;
152 }
153
154 Vector3 result = p_from + normal * inters_d;
155
156 if (r_res) {
157 *r_res = result;
158 }
159 if (r_norm) {
160 *r_norm = (result - p_sphere_pos).normalized();
161 }
162
163 return true;
164 }
165
166 static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr, int p_cylinder_axis = 2) {
167 Vector3 rel = (p_to - p_from);
168 real_t rel_l = rel.length();
169 if (rel_l < (real_t)CMP_EPSILON) {
170 return false; // Both points are the same.
171 }
172
173 ERR_FAIL_COND_V(p_cylinder_axis < 0, false);
174 ERR_FAIL_COND_V(p_cylinder_axis > 2, false);
175 Vector3 cylinder_axis;
176 cylinder_axis[p_cylinder_axis] = 1.0f;
177
178 // First check if they are parallel.
179 Vector3 normal = (rel / rel_l);
180 Vector3 crs = normal.cross(cylinder_axis);
181 real_t crs_l = crs.length();
182
183 Vector3 axis_dir;
184
185 if (crs_l < (real_t)CMP_EPSILON) {
186 Vector3 side_axis;
187 side_axis[(p_cylinder_axis + 1) % 3] = 1.0f; // Any side axis OK.
188 axis_dir = side_axis;
189 } else {
190 axis_dir = crs / crs_l;
191 }
192
193 real_t dist = axis_dir.dot(p_from);
194
195 if (dist >= p_radius) {
196 return false; // Too far away.
197 }
198
199 // Convert to 2D.
200 real_t w2 = p_radius * p_radius - dist * dist;
201 if (w2 < (real_t)CMP_EPSILON) {
202 return false; // Avoid numerical error.
203 }
204 Size2 size(Math::sqrt(w2), p_height * 0.5f);
205
206 Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized();
207
208 Vector2 from2D(side_dir.dot(p_from), p_from[p_cylinder_axis]);
209 Vector2 to2D(side_dir.dot(p_to), p_to[p_cylinder_axis]);
210
211 real_t min = 0, max = 1;
212
213 int axis = -1;
214
215 for (int i = 0; i < 2; i++) {
216 real_t seg_from = from2D[i];
217 real_t seg_to = to2D[i];
218 real_t box_begin = -size[i];
219 real_t box_end = size[i];
220 real_t cmin, cmax;
221
222 if (seg_from < seg_to) {
223 if (seg_from > box_end || seg_to < box_begin) {
224 return false;
225 }
226 real_t length = seg_to - seg_from;
227 cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
228 cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
229
230 } else {
231 if (seg_to > box_end || seg_from < box_begin) {
232 return false;
233 }
234 real_t length = seg_to - seg_from;
235 cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
236 cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
237 }
238
239 if (cmin > min) {
240 min = cmin;
241 axis = i;
242 }
243 if (cmax < max) {
244 max = cmax;
245 }
246 if (max < min) {
247 return false;
248 }
249 }
250
251 // Convert to 3D again.
252 Vector3 result = p_from + (rel * min);
253 Vector3 res_normal = result;
254
255 if (axis == 0) {
256 res_normal[p_cylinder_axis] = 0;
257 } else {
258 int axis_side = (p_cylinder_axis + 1) % 3;
259 res_normal[axis_side] = 0;
260 axis_side = (axis_side + 1) % 3;
261 res_normal[axis_side] = 0;
262 }
263
264 res_normal.normalize();
265
266 if (r_res) {
267 *r_res = result;
268 }
269 if (r_norm) {
270 *r_norm = res_normal;
271 }
272
273 return true;
274 }
275
276 static bool segment_intersects_convex(const Vector3 &p_from, const Vector3 &p_to, const Plane *p_planes, int p_plane_count, Vector3 *p_res, Vector3 *p_norm) {
277 real_t min = -1e20, max = 1e20;
278
279 Vector3 rel = p_to - p_from;
280 real_t rel_l = rel.length();
281
282 if (rel_l < (real_t)CMP_EPSILON) {
283 return false;
284 }
285
286 Vector3 dir = rel / rel_l;
287
288 int min_index = -1;
289
290 for (int i = 0; i < p_plane_count; i++) {
291 const Plane &p = p_planes[i];
292
293 real_t den = p.normal.dot(dir);
294
295 if (Math::abs(den) <= (real_t)CMP_EPSILON) {
296 continue; // Ignore parallel plane.
297 }
298
299 real_t dist = -p.distance_to(p_from) / den;
300
301 if (den > 0) {
302 // Backwards facing plane.
303 if (dist < max) {
304 max = dist;
305 }
306 } else {
307 // Front facing plane.
308 if (dist > min) {
309 min = dist;
310 min_index = i;
311 }
312 }
313 }
314
315 if (max <= min || min < 0 || min > rel_l || min_index == -1) { // Exit conditions.
316 return false; // No intersection.
317 }
318
319 if (p_res) {
320 *p_res = p_from + dir * min;
321 }
322 if (p_norm) {
323 *p_norm = p_planes[min_index].normal;
324 }
325
326 return true;
327 }
328
329 static Vector3 get_closest_point_to_segment(const Vector3 &p_point, const Vector3 *p_segment) {
330 Vector3 p = p_point - p_segment[0];
331 Vector3 n = p_segment[1] - p_segment[0];
332 real_t l2 = n.length_squared();
333 if (l2 < 1e-20f) {
334 return p_segment[0]; // Both points are the same, just give any.
335 }
336
337 real_t d = n.dot(p) / l2;
338
339 if (d <= 0.0f) {
340 return p_segment[0]; // Before first point.
341 } else if (d >= 1.0f) {
342 return p_segment[1]; // After first point.
343 } else {
344 return p_segment[0] + n * d; // Inside.
345 }
346 }
347
348 static Vector3 get_closest_point_to_segment_uncapped(const Vector3 &p_point, const Vector3 *p_segment) {
349 Vector3 p = p_point - p_segment[0];
350 Vector3 n = p_segment[1] - p_segment[0];
351 real_t l2 = n.length_squared();
352 if (l2 < 1e-20f) {
353 return p_segment[0]; // Both points are the same, just give any.
354 }
355
356 real_t d = n.dot(p) / l2;
357
358 return p_segment[0] + n * d; // Inside.
359 }
360
361 static inline bool point_in_projected_triangle(const Vector3 &p_point, const Vector3 &p_v1, const Vector3 &p_v2, const Vector3 &p_v3) {
362 Vector3 face_n = (p_v1 - p_v3).cross(p_v1 - p_v2);
363
364 Vector3 n1 = (p_point - p_v3).cross(p_point - p_v2);
365
366 if (face_n.dot(n1) < 0) {
367 return false;
368 }
369
370 Vector3 n2 = (p_v1 - p_v3).cross(p_v1 - p_point);
371
372 if (face_n.dot(n2) < 0) {
373 return false;
374 }
375
376 Vector3 n3 = (p_v1 - p_point).cross(p_v1 - p_v2);
377
378 if (face_n.dot(n3) < 0) {
379 return false;
380 }
381
382 return true;
383 }
384
385 static inline bool triangle_sphere_intersection_test(const Vector3 *p_triangle, const Vector3 &p_normal, const Vector3 &p_sphere_pos, real_t p_sphere_radius, Vector3 &r_triangle_contact, Vector3 &r_sphere_contact) {
386 real_t d = p_normal.dot(p_sphere_pos) - p_normal.dot(p_triangle[0]);
387
388 if (d > p_sphere_radius || d < -p_sphere_radius) {
389 // Not touching the plane of the face, return.
390 return false;
391 }
392
393 Vector3 contact = p_sphere_pos - (p_normal * d);
394
395 /** 2nd) TEST INSIDE TRIANGLE **/
396
397 if (Geometry3D::point_in_projected_triangle(contact, p_triangle[0], p_triangle[1], p_triangle[2])) {
398 r_triangle_contact = contact;
399 r_sphere_contact = p_sphere_pos - p_normal * p_sphere_radius;
400 //printf("solved inside triangle\n");
401 return true;
402 }
403
404 /** 3rd TEST INSIDE EDGE CYLINDERS **/
405
406 const Vector3 verts[4] = { p_triangle[0], p_triangle[1], p_triangle[2], p_triangle[0] }; // for() friendly
407
408 for (int i = 0; i < 3; i++) {
409 // Check edge cylinder.
410
411 Vector3 n1 = verts[i] - verts[i + 1];
412 Vector3 n2 = p_sphere_pos - verts[i + 1];
413
414 ///@TODO Maybe discard by range here to make the algorithm quicker.
415
416 // Check point within cylinder radius.
417 Vector3 axis = n1.cross(n2).cross(n1);
418 axis.normalize();
419
420 real_t ad = axis.dot(n2);
421
422 if (ABS(ad) > p_sphere_radius) {
423 // No chance with this edge, too far away.
424 continue;
425 }
426
427 // Check point within edge capsule cylinder.
428 /** 4th TEST INSIDE EDGE POINTS **/
429
430 real_t sphere_at = n1.dot(n2);
431
432 if (sphere_at >= 0 && sphere_at < n1.dot(n1)) {
433 r_triangle_contact = p_sphere_pos - axis * (axis.dot(n2));
434 r_sphere_contact = p_sphere_pos - axis * p_sphere_radius;
435 // Point inside here.
436 return true;
437 }
438
439 real_t r2 = p_sphere_radius * p_sphere_radius;
440
441 if (n2.length_squared() < r2) {
442 Vector3 n = (p_sphere_pos - verts[i + 1]).normalized();
443
444 r_triangle_contact = verts[i + 1];
445 r_sphere_contact = p_sphere_pos - n * p_sphere_radius;
446 return true;
447 }
448
449 if (n2.distance_squared_to(n1) < r2) {
450 Vector3 n = (p_sphere_pos - verts[i]).normalized();
451
452 r_triangle_contact = verts[i];
453 r_sphere_contact = p_sphere_pos - n * p_sphere_radius;
454 return true;
455 }
456
457 break; // It's pointless to continue at this point, so save some CPU cycles.
458 }
459
460 return false;
461 }
462
463 static inline Vector<Vector3> clip_polygon(const Vector<Vector3> &polygon, const Plane &p_plane) {
464 enum LocationCache {
465 LOC_INSIDE = 1,
466 LOC_BOUNDARY = 0,
467 LOC_OUTSIDE = -1
468 };
469
470 if (polygon.size() == 0) {
471 return polygon;
472 }
473
474 int *location_cache = (int *)alloca(sizeof(int) * polygon.size());
475 int inside_count = 0;
476 int outside_count = 0;
477
478 for (int a = 0; a < polygon.size(); a++) {
479 real_t dist = p_plane.distance_to(polygon[a]);
480 if (dist < (real_t)-CMP_POINT_IN_PLANE_EPSILON) {
481 location_cache[a] = LOC_INSIDE;
482 inside_count++;
483 } else {
484 if (dist > (real_t)CMP_POINT_IN_PLANE_EPSILON) {
485 location_cache[a] = LOC_OUTSIDE;
486 outside_count++;
487 } else {
488 location_cache[a] = LOC_BOUNDARY;
489 }
490 }
491 }
492
493 if (outside_count == 0) {
494 return polygon; // No changes.
495 } else if (inside_count == 0) {
496 return Vector<Vector3>(); // Empty.
497 }
498
499 long previous = polygon.size() - 1;
500 Vector<Vector3> clipped;
501
502 for (int index = 0; index < polygon.size(); index++) {
503 int loc = location_cache[index];
504 if (loc == LOC_OUTSIDE) {
505 if (location_cache[previous] == LOC_INSIDE) {
506 const Vector3 &v1 = polygon[previous];
507 const Vector3 &v2 = polygon[index];
508
509 Vector3 segment = v1 - v2;
510 real_t den = p_plane.normal.dot(segment);
511 real_t dist = p_plane.distance_to(v1) / den;
512 dist = -dist;
513 clipped.push_back(v1 + segment * dist);
514 }
515 } else {
516 const Vector3 &v1 = polygon[index];
517 if ((loc == LOC_INSIDE) && (location_cache[previous] == LOC_OUTSIDE)) {
518 const Vector3 &v2 = polygon[previous];
519 Vector3 segment = v1 - v2;
520 real_t den = p_plane.normal.dot(segment);
521 real_t dist = p_plane.distance_to(v1) / den;
522 dist = -dist;
523 clipped.push_back(v1 + segment * dist);
524 }
525
526 clipped.push_back(v1);
527 }
528
529 previous = index;
530 }
531
532 return clipped;
533 }
534
535 // Create a "wrap" that encloses the given geometry.
536 static Vector<Face3> wrap_geometry(Vector<Face3> p_array, real_t *p_error = nullptr);
537
538 struct MeshData {
539 struct Face {
540 Plane plane;
541 LocalVector<int> indices;
542 };
543
544 LocalVector<Face> faces;
545
546 struct Edge {
547 int vertex_a, vertex_b;
548 int face_a, face_b;
549 };
550
551 LocalVector<Edge> edges;
552
553 LocalVector<Vector3> vertices;
554
555 void optimize_vertices();
556 };
557
558 static MeshData build_convex_mesh(const Vector<Plane> &p_planes);
559 static Vector<Plane> build_sphere_planes(real_t p_radius, int p_lats, int p_lons, Vector3::Axis p_axis = Vector3::AXIS_Z);
560 static Vector<Plane> build_box_planes(const Vector3 &p_extents);
561 static Vector<Plane> build_cylinder_planes(real_t p_radius, real_t p_height, int p_sides, Vector3::Axis p_axis = Vector3::AXIS_Z);
562 static Vector<Plane> build_capsule_planes(real_t p_radius, real_t p_height, int p_sides, int p_lats, Vector3::Axis p_axis = Vector3::AXIS_Z);
563
564 static Vector<Vector3> compute_convex_mesh_points(const Plane *p_planes, int p_plane_count);
565
566#define FINDMINMAX(x0, x1, x2, min, max) \
567 min = max = x0; \
568 if (x1 < min) { \
569 min = x1; \
570 } \
571 if (x1 > max) { \
572 max = x1; \
573 } \
574 if (x2 < min) { \
575 min = x2; \
576 } \
577 if (x2 > max) { \
578 max = x2; \
579 }
580
581 _FORCE_INLINE_ static bool planeBoxOverlap(Vector3 normal, float d, Vector3 maxbox) {
582 int q;
583 Vector3 vmin, vmax;
584 for (q = 0; q <= 2; q++) {
585 if (normal[q] > 0.0f) {
586 vmin[q] = -maxbox[q];
587 vmax[q] = maxbox[q];
588 } else {
589 vmin[q] = maxbox[q];
590 vmax[q] = -maxbox[q];
591 }
592 }
593 if (normal.dot(vmin) + d > 0.0f) {
594 return false;
595 }
596 if (normal.dot(vmax) + d >= 0.0f) {
597 return true;
598 }
599
600 return false;
601 }
602
603/*======================== X-tests ========================*/
604#define AXISTEST_X01(a, b, fa, fb) \
605 p0 = a * v0.y - b * v0.z; \
606 p2 = a * v2.y - b * v2.z; \
607 if (p0 < p2) { \
608 min = p0; \
609 max = p2; \
610 } else { \
611 min = p2; \
612 max = p0; \
613 } \
614 rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \
615 if (min > rad || max < -rad) { \
616 return false; \
617 }
618
619#define AXISTEST_X2(a, b, fa, fb) \
620 p0 = a * v0.y - b * v0.z; \
621 p1 = a * v1.y - b * v1.z; \
622 if (p0 < p1) { \
623 min = p0; \
624 max = p1; \
625 } else { \
626 min = p1; \
627 max = p0; \
628 } \
629 rad = fa * boxhalfsize.y + fb * boxhalfsize.z; \
630 if (min > rad || max < -rad) { \
631 return false; \
632 }
633
634/*======================== Y-tests ========================*/
635#define AXISTEST_Y02(a, b, fa, fb) \
636 p0 = -a * v0.x + b * v0.z; \
637 p2 = -a * v2.x + b * v2.z; \
638 if (p0 < p2) { \
639 min = p0; \
640 max = p2; \
641 } else { \
642 min = p2; \
643 max = p0; \
644 } \
645 rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \
646 if (min > rad || max < -rad) { \
647 return false; \
648 }
649
650#define AXISTEST_Y1(a, b, fa, fb) \
651 p0 = -a * v0.x + b * v0.z; \
652 p1 = -a * v1.x + b * v1.z; \
653 if (p0 < p1) { \
654 min = p0; \
655 max = p1; \
656 } else { \
657 min = p1; \
658 max = p0; \
659 } \
660 rad = fa * boxhalfsize.x + fb * boxhalfsize.z; \
661 if (min > rad || max < -rad) { \
662 return false; \
663 }
664
665 /*======================== Z-tests ========================*/
666
667#define AXISTEST_Z12(a, b, fa, fb) \
668 p1 = a * v1.x - b * v1.y; \
669 p2 = a * v2.x - b * v2.y; \
670 if (p2 < p1) { \
671 min = p2; \
672 max = p1; \
673 } else { \
674 min = p1; \
675 max = p2; \
676 } \
677 rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \
678 if (min > rad || max < -rad) { \
679 return false; \
680 }
681
682#define AXISTEST_Z0(a, b, fa, fb) \
683 p0 = a * v0.x - b * v0.y; \
684 p1 = a * v1.x - b * v1.y; \
685 if (p0 < p1) { \
686 min = p0; \
687 max = p1; \
688 } else { \
689 min = p1; \
690 max = p0; \
691 } \
692 rad = fa * boxhalfsize.x + fb * boxhalfsize.y; \
693 if (min > rad || max < -rad) { \
694 return false; \
695 }
696
697 _FORCE_INLINE_ static bool triangle_box_overlap(const Vector3 &boxcenter, const Vector3 boxhalfsize, const Vector3 *triverts) {
698 /* use separating axis theorem to test overlap between triangle and box */
699 /* need to test for overlap in these directions: */
700 /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
701 /* we do not even need to test these) */
702 /* 2) normal of the triangle */
703 /* 3) crossproduct(edge from tri, {x,y,z}-directin) */
704 /* this gives 3x3=9 more tests */
705 Vector3 v0, v1, v2;
706 float min, max, d, p0, p1, p2, rad, fex, fey, fez;
707 Vector3 normal, e0, e1, e2;
708
709 /* This is the fastest branch on Sun */
710 /* move everything so that the boxcenter is in (0,0,0) */
711
712 v0 = triverts[0] - boxcenter;
713 v1 = triverts[1] - boxcenter;
714 v2 = triverts[2] - boxcenter;
715
716 /* compute triangle edges */
717 e0 = v1 - v0; /* tri edge 0 */
718 e1 = v2 - v1; /* tri edge 1 */
719 e2 = v0 - v2; /* tri edge 2 */
720
721 /* Bullet 3: */
722 /* test the 9 tests first (this was faster) */
723 fex = Math::abs(e0.x);
724 fey = Math::abs(e0.y);
725 fez = Math::abs(e0.z);
726 AXISTEST_X01(e0.z, e0.y, fez, fey);
727 AXISTEST_Y02(e0.z, e0.x, fez, fex);
728 AXISTEST_Z12(e0.y, e0.x, fey, fex);
729
730 fex = Math::abs(e1.x);
731 fey = Math::abs(e1.y);
732 fez = Math::abs(e1.z);
733 AXISTEST_X01(e1.z, e1.y, fez, fey);
734 AXISTEST_Y02(e1.z, e1.x, fez, fex);
735 AXISTEST_Z0(e1.y, e1.x, fey, fex);
736
737 fex = Math::abs(e2.x);
738 fey = Math::abs(e2.y);
739 fez = Math::abs(e2.z);
740 AXISTEST_X2(e2.z, e2.y, fez, fey);
741 AXISTEST_Y1(e2.z, e2.x, fez, fex);
742 AXISTEST_Z12(e2.y, e2.x, fey, fex);
743
744 /* Bullet 1: */
745 /* first test overlap in the {x,y,z}-directions */
746 /* find min, max of the triangle each direction, and test for overlap in */
747 /* that direction -- this is equivalent to testing a minimal AABB around */
748 /* the triangle against the AABB */
749
750 /* test in X-direction */
751 FINDMINMAX(v0.x, v1.x, v2.x, min, max);
752 if (min > boxhalfsize.x || max < -boxhalfsize.x) {
753 return false;
754 }
755
756 /* test in Y-direction */
757 FINDMINMAX(v0.y, v1.y, v2.y, min, max);
758 if (min > boxhalfsize.y || max < -boxhalfsize.y) {
759 return false;
760 }
761
762 /* test in Z-direction */
763 FINDMINMAX(v0.z, v1.z, v2.z, min, max);
764 if (min > boxhalfsize.z || max < -boxhalfsize.z) {
765 return false;
766 }
767
768 /* Bullet 2: */
769 /* test if the box intersects the plane of the triangle */
770 /* compute plane equation of triangle: normal*x+d=0 */
771 normal = e0.cross(e1);
772 d = -normal.dot(v0); /* plane eq: normal.x+d=0 */
773 return planeBoxOverlap(normal, d, boxhalfsize); /* if true, box and triangle overlaps */
774 }
775
776 static Vector<uint32_t> generate_edf(const Vector<bool> &p_voxels, const Vector3i &p_size, bool p_negative);
777 static Vector<int8_t> generate_sdf8(const Vector<uint32_t> &p_positive, const Vector<uint32_t> &p_negative);
778
779 static Vector3 triangle_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_pos) {
780 Vector3 v0 = p_b - p_a;
781 Vector3 v1 = p_c - p_a;
782 Vector3 v2 = p_pos - p_a;
783
784 float d00 = v0.dot(v0);
785 float d01 = v0.dot(v1);
786 float d11 = v1.dot(v1);
787 float d20 = v2.dot(v0);
788 float d21 = v2.dot(v1);
789 float denom = (d00 * d11 - d01 * d01);
790 if (denom == 0) {
791 return Vector3(); //invalid triangle, return empty
792 }
793 float v = (d11 * d20 - d01 * d21) / denom;
794 float w = (d00 * d21 - d01 * d20) / denom;
795 float u = 1.0f - v - w;
796 return Vector3(u, v, w);
797 }
798
799 static Color tetrahedron_get_barycentric_coords(const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_c, const Vector3 &p_d, const Vector3 &p_pos) {
800 Vector3 vap = p_pos - p_a;
801 Vector3 vbp = p_pos - p_b;
802
803 Vector3 vab = p_b - p_a;
804 Vector3 vac = p_c - p_a;
805 Vector3 vad = p_d - p_a;
806
807 Vector3 vbc = p_c - p_b;
808 Vector3 vbd = p_d - p_b;
809 // ScTP computes the scalar triple product
810#define STP(m_a, m_b, m_c) ((m_a).dot((m_b).cross((m_c))))
811 float va6 = STP(vbp, vbd, vbc);
812 float vb6 = STP(vap, vac, vad);
813 float vc6 = STP(vap, vad, vab);
814 float vd6 = STP(vap, vab, vac);
815 float v6 = 1 / STP(vab, vac, vad);
816 return Color(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6);
817#undef STP
818 }
819
820 _FORCE_INLINE_ static Vector3 octahedron_map_decode(const Vector2 &p_uv) {
821 // https://twitter.com/Stubbesaurus/status/937994790553227264
822 Vector2 f = p_uv * 2.0f - Vector2(1.0f, 1.0f);
823 Vector3 n = Vector3(f.x, f.y, 1.0f - Math::abs(f.x) - Math::abs(f.y));
824 float t = CLAMP(-n.z, 0.0f, 1.0f);
825 n.x += n.x >= 0 ? -t : t;
826 n.y += n.y >= 0 ? -t : t;
827 return n.normalized();
828 }
829};
830
831#endif // GEOMETRY_3D_H
832