1/**************************************************************************/
2/* face3.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 "face3.h"
32
33#include "core/math/geometry_3d.h"
34
35int Face3::split_by_plane(const Plane &p_plane, Face3 p_res[3], bool p_is_point_over[3]) const {
36 ERR_FAIL_COND_V(is_degenerate(), 0);
37
38 Vector3 above[4];
39 int above_count = 0;
40
41 Vector3 below[4];
42 int below_count = 0;
43
44 for (int i = 0; i < 3; i++) {
45 if (p_plane.has_point(vertex[i], (real_t)CMP_EPSILON)) { // point is in plane
46
47 ERR_FAIL_COND_V(above_count >= 4, 0);
48 above[above_count++] = vertex[i];
49 ERR_FAIL_COND_V(below_count >= 4, 0);
50 below[below_count++] = vertex[i];
51
52 } else {
53 if (p_plane.is_point_over(vertex[i])) {
54 //Point is over
55 ERR_FAIL_COND_V(above_count >= 4, 0);
56 above[above_count++] = vertex[i];
57
58 } else {
59 //Point is under
60 ERR_FAIL_COND_V(below_count >= 4, 0);
61 below[below_count++] = vertex[i];
62 }
63
64 /* Check for Intersection between this and the next vertex*/
65
66 Vector3 inters;
67 if (!p_plane.intersects_segment(vertex[i], vertex[(i + 1) % 3], &inters)) {
68 continue;
69 }
70
71 /* Intersection goes to both */
72 ERR_FAIL_COND_V(above_count >= 4, 0);
73 above[above_count++] = inters;
74 ERR_FAIL_COND_V(below_count >= 4, 0);
75 below[below_count++] = inters;
76 }
77 }
78
79 int polygons_created = 0;
80
81 ERR_FAIL_COND_V(above_count >= 4 && below_count >= 4, 0); //bug in the algo
82
83 if (above_count >= 3) {
84 p_res[polygons_created] = Face3(above[0], above[1], above[2]);
85 p_is_point_over[polygons_created] = true;
86 polygons_created++;
87
88 if (above_count == 4) {
89 p_res[polygons_created] = Face3(above[2], above[3], above[0]);
90 p_is_point_over[polygons_created] = true;
91 polygons_created++;
92 }
93 }
94
95 if (below_count >= 3) {
96 p_res[polygons_created] = Face3(below[0], below[1], below[2]);
97 p_is_point_over[polygons_created] = false;
98 polygons_created++;
99
100 if (below_count == 4) {
101 p_res[polygons_created] = Face3(below[2], below[3], below[0]);
102 p_is_point_over[polygons_created] = false;
103 polygons_created++;
104 }
105 }
106
107 return polygons_created;
108}
109
110bool Face3::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
111 return Geometry3D::ray_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection);
112}
113
114bool Face3::intersects_segment(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
115 return Geometry3D::segment_intersects_triangle(p_from, p_dir, vertex[0], vertex[1], vertex[2], p_intersection);
116}
117
118bool Face3::is_degenerate() const {
119 Vector3 normal = vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]);
120 return (normal.length_squared() < (real_t)CMP_EPSILON2);
121}
122
123Vector3 Face3::get_random_point_inside() const {
124 real_t a = Math::random(0.0, 1.0);
125 real_t b = Math::random(0.0, 1.0);
126 if (a > b) {
127 SWAP(a, b);
128 }
129
130 return vertex[0] * a + vertex[1] * (b - a) + vertex[2] * (1.0f - b);
131}
132
133Plane Face3::get_plane(ClockDirection p_dir) const {
134 return Plane(vertex[0], vertex[1], vertex[2], p_dir);
135}
136
137real_t Face3::get_area() const {
138 return vec3_cross(vertex[0] - vertex[1], vertex[0] - vertex[2]).length() * 0.5f;
139}
140
141bool Face3::intersects_aabb(const AABB &p_aabb) const {
142 /** TEST PLANE **/
143 if (!p_aabb.intersects_plane(get_plane())) {
144 return false;
145 }
146
147#define TEST_AXIS(m_ax) \
148 /** TEST FACE AXIS */ \
149 { \
150 real_t aabb_min = p_aabb.position.m_ax; \
151 real_t aabb_max = p_aabb.position.m_ax + p_aabb.size.m_ax; \
152 real_t tri_min = vertex[0].m_ax; \
153 real_t tri_max = vertex[0].m_ax; \
154 for (int i = 1; i < 3; i++) { \
155 if (vertex[i].m_ax > tri_max) \
156 tri_max = vertex[i].m_ax; \
157 if (vertex[i].m_ax < tri_min) \
158 tri_min = vertex[i].m_ax; \
159 } \
160 \
161 if (tri_max < aabb_min || aabb_max < tri_min) \
162 return false; \
163 }
164
165 TEST_AXIS(x);
166 TEST_AXIS(y);
167 TEST_AXIS(z);
168
169 /** TEST ALL EDGES **/
170
171 const Vector3 edge_norms[3] = {
172 vertex[0] - vertex[1],
173 vertex[1] - vertex[2],
174 vertex[2] - vertex[0],
175 };
176
177 for (int i = 0; i < 12; i++) {
178 Vector3 from, to;
179 p_aabb.get_edge(i, from, to);
180 Vector3 e1 = from - to;
181 for (int j = 0; j < 3; j++) {
182 Vector3 e2 = edge_norms[j];
183
184 Vector3 axis = vec3_cross(e1, e2);
185
186 if (axis.length_squared() < 0.0001f) {
187 continue; // coplanar
188 }
189 axis.normalize();
190
191 real_t minA, maxA, minB, maxB;
192 p_aabb.project_range_in_plane(Plane(axis), minA, maxA);
193 project_range(axis, Transform3D(), minB, maxB);
194
195 if (maxA < minB || maxB < minA) {
196 return false;
197 }
198 }
199 }
200 return true;
201}
202
203Face3::operator String() const {
204 return String() + vertex[0] + ", " + vertex[1] + ", " + vertex[2];
205}
206
207void Face3::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
208 for (int i = 0; i < 3; i++) {
209 Vector3 v = p_transform.xform(vertex[i]);
210 real_t d = p_normal.dot(v);
211
212 if (i == 0 || d > r_max) {
213 r_max = d;
214 }
215
216 if (i == 0 || d < r_min) {
217 r_min = d;
218 }
219 }
220}
221
222void Face3::get_support(const Vector3 &p_normal, const Transform3D &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const {
223 constexpr double face_support_threshold = 0.98;
224 constexpr double edge_support_threshold = 0.05;
225
226 if (p_max <= 0) {
227 return;
228 }
229
230 Vector3 n = p_transform.basis.xform_inv(p_normal);
231
232 /** TEST FACE AS SUPPORT **/
233 if (get_plane().normal.dot(n) > face_support_threshold) {
234 *p_count = MIN(3, p_max);
235
236 for (int i = 0; i < *p_count; i++) {
237 p_vertices[i] = p_transform.xform(vertex[i]);
238 }
239
240 return;
241 }
242
243 /** FIND SUPPORT VERTEX **/
244
245 int vert_support_idx = -1;
246 real_t support_max = 0;
247
248 for (int i = 0; i < 3; i++) {
249 real_t d = n.dot(vertex[i]);
250
251 if (i == 0 || d > support_max) {
252 support_max = d;
253 vert_support_idx = i;
254 }
255 }
256
257 /** TEST EDGES AS SUPPORT **/
258
259 for (int i = 0; i < 3; i++) {
260 if (i != vert_support_idx && i + 1 != vert_support_idx) {
261 continue;
262 }
263
264 // check if edge is valid as a support
265 real_t dot = (vertex[i] - vertex[(i + 1) % 3]).normalized().dot(n);
266 dot = ABS(dot);
267 if (dot < edge_support_threshold) {
268 *p_count = MIN(2, p_max);
269
270 for (int j = 0; j < *p_count; j++) {
271 p_vertices[j] = p_transform.xform(vertex[(j + i) % 3]);
272 }
273
274 return;
275 }
276 }
277
278 *p_count = 1;
279 p_vertices[0] = p_transform.xform(vertex[vert_support_idx]);
280}
281
282Vector3 Face3::get_closest_point_to(const Vector3 &p_point) const {
283 Vector3 edge0 = vertex[1] - vertex[0];
284 Vector3 edge1 = vertex[2] - vertex[0];
285 Vector3 v0 = vertex[0] - p_point;
286
287 real_t a = edge0.dot(edge0);
288 real_t b = edge0.dot(edge1);
289 real_t c = edge1.dot(edge1);
290 real_t d = edge0.dot(v0);
291 real_t e = edge1.dot(v0);
292
293 real_t det = a * c - b * b;
294 real_t s = b * e - c * d;
295 real_t t = b * d - a * e;
296
297 if (s + t < det) {
298 if (s < 0.f) {
299 if (t < 0.f) {
300 if (d < 0.f) {
301 s = CLAMP(-d / a, 0.f, 1.f);
302 t = 0.f;
303 } else {
304 s = 0.f;
305 t = CLAMP(-e / c, 0.f, 1.f);
306 }
307 } else {
308 s = 0.f;
309 t = CLAMP(-e / c, 0.f, 1.f);
310 }
311 } else if (t < 0.f) {
312 s = CLAMP(-d / a, 0.f, 1.f);
313 t = 0.f;
314 } else {
315 real_t invDet = 1.f / det;
316 s *= invDet;
317 t *= invDet;
318 }
319 } else {
320 if (s < 0.f) {
321 real_t tmp0 = b + d;
322 real_t tmp1 = c + e;
323 if (tmp1 > tmp0) {
324 real_t numer = tmp1 - tmp0;
325 real_t denom = a - 2 * b + c;
326 s = CLAMP(numer / denom, 0.f, 1.f);
327 t = 1 - s;
328 } else {
329 t = CLAMP(-e / c, 0.f, 1.f);
330 s = 0.f;
331 }
332 } else if (t < 0.f) {
333 if (a + d > b + e) {
334 real_t numer = c + e - b - d;
335 real_t denom = a - 2 * b + c;
336 s = CLAMP(numer / denom, 0.f, 1.f);
337 t = 1 - s;
338 } else {
339 s = CLAMP(-d / a, 0.f, 1.f);
340 t = 0.f;
341 }
342 } else {
343 real_t numer = c + e - b - d;
344 real_t denom = a - 2 * b + c;
345 s = CLAMP(numer / denom, 0.f, 1.f);
346 t = 1.f - s;
347 }
348 }
349
350 return vertex[0] + s * edge0 + t * edge1;
351}
352