1/**************************************************************************/
2/* godot_shape_3d.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 "godot_shape_3d.h"
32
33#include "core/io/image.h"
34#include "core/math/convex_hull.h"
35#include "core/math/geometry_3d.h"
36#include "core/templates/sort_array.h"
37
38// GodotHeightMapShape3D is based on Bullet btHeightfieldTerrainShape.
39
40/*
41Bullet Continuous Collision Detection and Physics Library
42Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
43
44This software is provided 'as-is', without any express or implied warranty.
45In no event will the authors be held liable for any damages arising from the use of this software.
46Permission is granted to anyone to use this software for any purpose,
47including commercial applications, and to alter it and redistribute it freely,
48subject to the following restrictions:
49
501. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
512. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
523. This notice may not be removed or altered from any source distribution.
53*/
54
55const double edge_support_threshold = 0.99999998;
56const double edge_support_threshold_lower = Math::sqrt(1.0 - edge_support_threshold * edge_support_threshold);
57// For a unit normal vector n, the horizontality condition
58// sqrt(n.x * n.x + n.z * n.z) > edge_support_threshold
59// is equivalent to the condition
60// abs(n.y) < edge_support_threshold_lower,
61// which is cheaper to test.
62const double face_support_threshold = 0.9998;
63
64const double cylinder_edge_support_threshold = 0.999998;
65const double cylinder_edge_support_threshold_lower = Math::sqrt(1.0 - cylinder_edge_support_threshold * cylinder_edge_support_threshold);
66const double cylinder_face_support_threshold = 0.999;
67
68void GodotShape3D::configure(const AABB &p_aabb) {
69 aabb = p_aabb;
70 configured = true;
71 for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) {
72 GodotShapeOwner3D *co = const_cast<GodotShapeOwner3D *>(E.key);
73 co->_shape_changed();
74 }
75}
76
77Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const {
78 Vector3 res;
79 int amnt;
80 FeatureType type;
81 get_supports(p_normal, 1, &res, amnt, type);
82 return res;
83}
84
85void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) {
86 HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner);
87 if (E) {
88 E->value++;
89 } else {
90 owners[p_owner] = 1;
91 }
92}
93
94void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) {
95 HashMap<GodotShapeOwner3D *, int>::Iterator E = owners.find(p_owner);
96 ERR_FAIL_COND(!E);
97 E->value--;
98 if (E->value == 0) {
99 owners.remove(E);
100 }
101}
102
103bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const {
104 return owners.has(p_owner);
105}
106
107const HashMap<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const {
108 return owners;
109}
110
111GodotShape3D::~GodotShape3D() {
112 ERR_FAIL_COND(owners.size());
113}
114
115Plane GodotWorldBoundaryShape3D::get_plane() const {
116 return plane;
117}
118
119void GodotWorldBoundaryShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
120 // gibberish, a plane is infinity
121 r_min = -1e7;
122 r_max = 1e7;
123}
124
125Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const {
126 return p_normal * 1e15;
127}
128
129bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
130 bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
131 if (inters) {
132 r_normal = plane.normal;
133 }
134 return inters;
135}
136
137bool GodotWorldBoundaryShape3D::intersect_point(const Vector3 &p_point) const {
138 return plane.distance_to(p_point) < 0;
139}
140
141Vector3 GodotWorldBoundaryShape3D::get_closest_point_to(const Vector3 &p_point) const {
142 if (plane.is_point_over(p_point)) {
143 return plane.project(p_point);
144 } else {
145 return p_point;
146 }
147}
148
149Vector3 GodotWorldBoundaryShape3D::get_moment_of_inertia(real_t p_mass) const {
150 return Vector3(); // not applicable.
151}
152
153void GodotWorldBoundaryShape3D::_setup(const Plane &p_plane) {
154 plane = p_plane;
155 configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
156}
157
158void GodotWorldBoundaryShape3D::set_data(const Variant &p_data) {
159 _setup(p_data);
160}
161
162Variant GodotWorldBoundaryShape3D::get_data() const {
163 return plane;
164}
165
166GodotWorldBoundaryShape3D::GodotWorldBoundaryShape3D() {
167}
168
169//
170
171real_t GodotSeparationRayShape3D::get_length() const {
172 return length;
173}
174
175bool GodotSeparationRayShape3D::get_slide_on_slope() const {
176 return slide_on_slope;
177}
178
179void GodotSeparationRayShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
180 // don't think this will be even used
181 r_min = 0;
182 r_max = 1;
183}
184
185Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const {
186 if (p_normal.z > 0) {
187 return Vector3(0, 0, length);
188 } else {
189 return Vector3(0, 0, 0);
190 }
191}
192
193void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
194 if (Math::abs(p_normal.z) < edge_support_threshold_lower) {
195 r_amount = 2;
196 r_type = FEATURE_EDGE;
197 r_supports[0] = Vector3(0, 0, 0);
198 r_supports[1] = Vector3(0, 0, length);
199 } else if (p_normal.z > 0) {
200 r_amount = 1;
201 r_type = FEATURE_POINT;
202 *r_supports = Vector3(0, 0, length);
203 } else {
204 r_amount = 1;
205 r_type = FEATURE_POINT;
206 *r_supports = Vector3(0, 0, 0);
207 }
208}
209
210bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
211 return false; //simply not possible
212}
213
214bool GodotSeparationRayShape3D::intersect_point(const Vector3 &p_point) const {
215 return false; //simply not possible
216}
217
218Vector3 GodotSeparationRayShape3D::get_closest_point_to(const Vector3 &p_point) const {
219 Vector3 s[2] = {
220 Vector3(0, 0, 0),
221 Vector3(0, 0, length)
222 };
223
224 return Geometry3D::get_closest_point_to_segment(p_point, s);
225}
226
227Vector3 GodotSeparationRayShape3D::get_moment_of_inertia(real_t p_mass) const {
228 return Vector3();
229}
230
231void GodotSeparationRayShape3D::_setup(real_t p_length, bool p_slide_on_slope) {
232 length = p_length;
233 slide_on_slope = p_slide_on_slope;
234 configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
235}
236
237void GodotSeparationRayShape3D::set_data(const Variant &p_data) {
238 Dictionary d = p_data;
239 _setup(d["length"], d["slide_on_slope"]);
240}
241
242Variant GodotSeparationRayShape3D::get_data() const {
243 Dictionary d;
244 d["length"] = length;
245 d["slide_on_slope"] = slide_on_slope;
246 return d;
247}
248
249GodotSeparationRayShape3D::GodotSeparationRayShape3D() {}
250
251/********** SPHERE *************/
252
253real_t GodotSphereShape3D::get_radius() const {
254 return radius;
255}
256
257void GodotSphereShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
258 real_t d = p_normal.dot(p_transform.origin);
259
260 // figure out scale at point
261 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
262 real_t scale = local_normal.length();
263
264 r_min = d - (radius)*scale;
265 r_max = d + (radius)*scale;
266}
267
268Vector3 GodotSphereShape3D::get_support(const Vector3 &p_normal) const {
269 return p_normal * radius;
270}
271
272void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
273 *r_supports = p_normal * radius;
274 r_amount = 1;
275 r_type = FEATURE_POINT;
276}
277
278bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
279 return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
280}
281
282bool GodotSphereShape3D::intersect_point(const Vector3 &p_point) const {
283 return p_point.length() < radius;
284}
285
286Vector3 GodotSphereShape3D::get_closest_point_to(const Vector3 &p_point) const {
287 Vector3 p = p_point;
288 real_t l = p.length();
289 if (l < radius) {
290 return p_point;
291 }
292 return (p / l) * radius;
293}
294
295Vector3 GodotSphereShape3D::get_moment_of_inertia(real_t p_mass) const {
296 real_t s = 0.4 * p_mass * radius * radius;
297 return Vector3(s, s, s);
298}
299
300void GodotSphereShape3D::_setup(real_t p_radius) {
301 radius = p_radius;
302 configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
303}
304
305void GodotSphereShape3D::set_data(const Variant &p_data) {
306 _setup(p_data);
307}
308
309Variant GodotSphereShape3D::get_data() const {
310 return radius;
311}
312
313GodotSphereShape3D::GodotSphereShape3D() {}
314
315/********** BOX *************/
316
317void GodotBoxShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
318 // no matter the angle, the box is mirrored anyway
319 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
320
321 real_t length = local_normal.abs().dot(half_extents);
322 real_t distance = p_normal.dot(p_transform.origin);
323
324 r_min = distance - length;
325 r_max = distance + length;
326}
327
328Vector3 GodotBoxShape3D::get_support(const Vector3 &p_normal) const {
329 Vector3 point(
330 (p_normal.x < 0) ? -half_extents.x : half_extents.x,
331 (p_normal.y < 0) ? -half_extents.y : half_extents.y,
332 (p_normal.z < 0) ? -half_extents.z : half_extents.z);
333
334 return point;
335}
336
337void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
338 static const int next[3] = { 1, 2, 0 };
339 static const int next2[3] = { 2, 0, 1 };
340
341 for (int i = 0; i < 3; i++) {
342 Vector3 axis;
343 axis[i] = 1.0;
344 real_t dot = p_normal.dot(axis);
345 if (Math::abs(dot) > face_support_threshold) {
346 //Vector3 axis_b;
347
348 bool neg = dot < 0;
349 r_amount = 4;
350 r_type = FEATURE_FACE;
351
352 Vector3 point;
353 point[i] = half_extents[i];
354
355 int i_n = next[i];
356 int i_n2 = next2[i];
357
358 static const real_t sign[4][2] = {
359 { -1.0, 1.0 },
360 { 1.0, 1.0 },
361 { 1.0, -1.0 },
362 { -1.0, -1.0 },
363 };
364
365 for (int j = 0; j < 4; j++) {
366 point[i_n] = sign[j][0] * half_extents[i_n];
367 point[i_n2] = sign[j][1] * half_extents[i_n2];
368 r_supports[j] = neg ? -point : point;
369 }
370
371 if (neg) {
372 SWAP(r_supports[1], r_supports[2]);
373 SWAP(r_supports[0], r_supports[3]);
374 }
375
376 return;
377 }
378
379 r_amount = 0;
380 }
381
382 for (int i = 0; i < 3; i++) {
383 Vector3 axis;
384 axis[i] = 1.0;
385
386 if (Math::abs(p_normal.dot(axis)) < edge_support_threshold_lower) {
387 r_amount = 2;
388 r_type = FEATURE_EDGE;
389
390 int i_n = next[i];
391 int i_n2 = next2[i];
392
393 Vector3 point = half_extents;
394
395 if (p_normal[i_n] < 0) {
396 point[i_n] = -point[i_n];
397 }
398 if (p_normal[i_n2] < 0) {
399 point[i_n2] = -point[i_n2];
400 }
401
402 r_supports[0] = point;
403 point[i] = -point[i];
404 r_supports[1] = point;
405 return;
406 }
407 }
408 /* USE POINT */
409
410 Vector3 point(
411 (p_normal.x < 0) ? -half_extents.x : half_extents.x,
412 (p_normal.y < 0) ? -half_extents.y : half_extents.y,
413 (p_normal.z < 0) ? -half_extents.z : half_extents.z);
414
415 r_amount = 1;
416 r_type = FEATURE_POINT;
417 r_supports[0] = point;
418}
419
420bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
421 AABB aabb_ext(-half_extents, half_extents * 2.0);
422
423 return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal);
424}
425
426bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const {
427 return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z);
428}
429
430Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const {
431 int outside = 0;
432 Vector3 min_point;
433
434 for (int i = 0; i < 3; i++) {
435 if (Math::abs(p_point[i]) > half_extents[i]) {
436 outside++;
437 if (outside == 1) {
438 //use plane if only one side matches
439 Vector3 n;
440 n[i] = SIGN(p_point[i]);
441
442 Plane p(n, half_extents[i]);
443 min_point = p.project(p_point);
444 }
445 }
446 }
447
448 if (!outside) {
449 return p_point; //it's inside, don't do anything else
450 }
451
452 if (outside == 1) { //if only above one plane, this plane clearly wins
453 return min_point;
454 }
455
456 //check segments
457 real_t min_distance = 1e20;
458 Vector3 closest_vertex = half_extents * p_point.sign();
459 Vector3 s[2] = {
460 closest_vertex,
461 closest_vertex
462 };
463
464 for (int i = 0; i < 3; i++) {
465 s[1] = closest_vertex;
466 s[1][i] = -s[1][i]; //edge
467
468 Vector3 closest_edge = Geometry3D::get_closest_point_to_segment(p_point, s);
469
470 real_t d = p_point.distance_to(closest_edge);
471 if (d < min_distance) {
472 min_point = closest_edge;
473 min_distance = d;
474 }
475 }
476
477 return min_point;
478}
479
480Vector3 GodotBoxShape3D::get_moment_of_inertia(real_t p_mass) const {
481 real_t lx = half_extents.x;
482 real_t ly = half_extents.y;
483 real_t lz = half_extents.z;
484
485 return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly));
486}
487
488void GodotBoxShape3D::_setup(const Vector3 &p_half_extents) {
489 half_extents = p_half_extents.abs();
490
491 configure(AABB(-half_extents, half_extents * 2));
492}
493
494void GodotBoxShape3D::set_data(const Variant &p_data) {
495 _setup(p_data);
496}
497
498Variant GodotBoxShape3D::get_data() const {
499 return half_extents;
500}
501
502GodotBoxShape3D::GodotBoxShape3D() {}
503
504/********** CAPSULE *************/
505
506void GodotCapsuleShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
507 Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
508 real_t h = height * 0.5 - radius;
509
510 n *= radius;
511 n.y += (n.y > 0) ? h : -h;
512
513 r_max = p_normal.dot(p_transform.xform(n));
514 r_min = p_normal.dot(p_transform.xform(-n));
515}
516
517Vector3 GodotCapsuleShape3D::get_support(const Vector3 &p_normal) const {
518 Vector3 n = p_normal;
519
520 real_t h = height * 0.5 - radius;
521
522 n *= radius;
523 n.y += (n.y > 0) ? h : -h;
524 return n;
525}
526
527void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
528 Vector3 n = p_normal;
529
530 real_t d = n.y;
531 real_t h = height * 0.5 - radius; // half-height of the cylinder part
532
533 if (h > 0 && Math::abs(d) < edge_support_threshold_lower) {
534 // make it flat
535 n.y = 0.0;
536 n.normalize();
537 n *= radius;
538
539 r_amount = 2;
540 r_type = FEATURE_EDGE;
541 r_supports[0] = n;
542 r_supports[0].y += h;
543 r_supports[1] = n;
544 r_supports[1].y -= h;
545 } else {
546 n *= radius;
547 n.y += (d > 0) ? h : -h;
548 r_amount = 1;
549 r_type = FEATURE_POINT;
550 *r_supports = n;
551 }
552}
553
554bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
555 Vector3 norm = (p_end - p_begin).normalized();
556 real_t min_d = 1e20;
557
558 Vector3 res, n;
559 bool collision = false;
560
561 Vector3 auxres, auxn;
562 bool collided;
563
564 // test against cylinder and spheres :-|
565
566 collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1);
567
568 if (collided) {
569 real_t d = norm.dot(auxres);
570 if (d < min_d) {
571 min_d = d;
572 res = auxres;
573 n = auxn;
574 collision = true;
575 }
576 }
577
578 collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn);
579
580 if (collided) {
581 real_t d = norm.dot(auxres);
582 if (d < min_d) {
583 min_d = d;
584 res = auxres;
585 n = auxn;
586 collision = true;
587 }
588 }
589
590 collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn);
591
592 if (collided) {
593 real_t d = norm.dot(auxres);
594
595 if (d < min_d) {
596 min_d = d;
597 res = auxres;
598 n = auxn;
599 collision = true;
600 }
601 }
602
603 if (collision) {
604 r_result = res;
605 r_normal = n;
606 }
607 return collision;
608}
609
610bool GodotCapsuleShape3D::intersect_point(const Vector3 &p_point) const {
611 if (Math::abs(p_point.y) < height * 0.5 - radius) {
612 return Vector3(p_point.x, 0, p_point.z).length() < radius;
613 } else {
614 Vector3 p = p_point;
615 p.y = Math::abs(p.y) - height * 0.5 + radius;
616 return p.length() < radius;
617 }
618}
619
620Vector3 GodotCapsuleShape3D::get_closest_point_to(const Vector3 &p_point) const {
621 Vector3 s[2] = {
622 Vector3(0, -height * 0.5 + radius, 0),
623 Vector3(0, height * 0.5 - radius, 0),
624 };
625
626 Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
627
628 if (p.distance_to(p_point) < radius) {
629 return p_point;
630 }
631
632 return p + (p_point - p).normalized() * radius;
633}
634
635Vector3 GodotCapsuleShape3D::get_moment_of_inertia(real_t p_mass) const {
636 // use bad AABB approximation
637 Vector3 extents = get_aabb().size * 0.5;
638
639 return Vector3(
640 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
641 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
642 (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
643}
644
645void GodotCapsuleShape3D::_setup(real_t p_height, real_t p_radius) {
646 height = p_height;
647 radius = p_radius;
648 configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2)));
649}
650
651void GodotCapsuleShape3D::set_data(const Variant &p_data) {
652 Dictionary d = p_data;
653 ERR_FAIL_COND(!d.has("radius"));
654 ERR_FAIL_COND(!d.has("height"));
655 _setup(d["height"], d["radius"]);
656}
657
658Variant GodotCapsuleShape3D::get_data() const {
659 Dictionary d;
660 d["radius"] = radius;
661 d["height"] = height;
662 return d;
663}
664
665GodotCapsuleShape3D::GodotCapsuleShape3D() {}
666
667/********** CYLINDER *************/
668
669void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
670 Vector3 cylinder_axis = p_transform.basis.get_column(1).normalized();
671 real_t axis_dot = cylinder_axis.dot(p_normal);
672
673 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
674 real_t scale = local_normal.length();
675 real_t scaled_radius = radius * scale;
676 real_t scaled_height = height * scale;
677
678 real_t length;
679 if (Math::abs(axis_dot) > 1.0) {
680 length = scaled_height * 0.5;
681 } else {
682 length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot);
683 }
684
685 real_t distance = p_normal.dot(p_transform.origin);
686
687 r_min = distance - length;
688 r_max = distance + length;
689}
690
691Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const {
692 Vector3 n = p_normal;
693 real_t h = (n.y > 0) ? height : -height;
694 real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
695 if (Math::is_zero_approx(s)) {
696 n.x = radius;
697 n.y = h * 0.5;
698 n.z = 0.0;
699 } else {
700 real_t d = radius / s;
701 n.x = n.x * d;
702 n.y = h * 0.5;
703 n.z = n.z * d;
704 }
705
706 return n;
707}
708
709void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
710 real_t d = p_normal.y;
711 if (Math::abs(d) > cylinder_face_support_threshold) {
712 real_t h = (d > 0) ? height : -height;
713
714 Vector3 n = p_normal;
715 n.x = 0.0;
716 n.z = 0.0;
717 n.y = h * 0.5;
718
719 r_amount = 3;
720 r_type = FEATURE_CIRCLE;
721 r_supports[0] = n;
722 r_supports[1] = n;
723 r_supports[1].x += radius;
724 r_supports[2] = n;
725 r_supports[2].z += radius;
726 } else if (Math::abs(d) < cylinder_edge_support_threshold_lower) {
727 // make it flat
728 Vector3 n = p_normal;
729 n.y = 0.0;
730 n.normalize();
731 n *= radius;
732
733 r_amount = 2;
734 r_type = FEATURE_EDGE;
735 r_supports[0] = n;
736 r_supports[0].y += height * 0.5;
737 r_supports[1] = n;
738 r_supports[1].y -= height * 0.5;
739 } else {
740 r_amount = 1;
741 r_type = FEATURE_POINT;
742 r_supports[0] = get_support(p_normal);
743 }
744}
745
746bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
747 return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1);
748}
749
750bool GodotCylinderShape3D::intersect_point(const Vector3 &p_point) const {
751 if (Math::abs(p_point.y) < height * 0.5) {
752 return Vector3(p_point.x, 0, p_point.z).length() < radius;
753 }
754 return false;
755}
756
757Vector3 GodotCylinderShape3D::get_closest_point_to(const Vector3 &p_point) const {
758 if (Math::absf(p_point.y) > height * 0.5) {
759 // Project point to top disk.
760 real_t dir = p_point.y > 0.0 ? 1.0 : -1.0;
761 Vector3 circle_pos(0.0, dir * height * 0.5, 0.0);
762 Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos);
763 Vector3 proj_point = circle_plane.project(p_point);
764
765 // Clip position.
766 Vector3 delta_point_1 = proj_point - circle_pos;
767 real_t dist_point_1 = delta_point_1.length_squared();
768 if (!Math::is_zero_approx(dist_point_1)) {
769 dist_point_1 = Math::sqrt(dist_point_1);
770 proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1;
771 }
772
773 return proj_point;
774 } else {
775 Vector3 s[2] = {
776 Vector3(0, -height * 0.5, 0),
777 Vector3(0, height * 0.5, 0),
778 };
779
780 Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
781
782 if (p.distance_to(p_point) < radius) {
783 return p_point;
784 }
785
786 return p + (p_point - p).normalized() * radius;
787 }
788}
789
790Vector3 GodotCylinderShape3D::get_moment_of_inertia(real_t p_mass) const {
791 // use bad AABB approximation
792 Vector3 extents = get_aabb().size * 0.5;
793
794 return Vector3(
795 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
796 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
797 (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
798}
799
800void GodotCylinderShape3D::_setup(real_t p_height, real_t p_radius) {
801 height = p_height;
802 radius = p_radius;
803 configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0)));
804}
805
806void GodotCylinderShape3D::set_data(const Variant &p_data) {
807 Dictionary d = p_data;
808 ERR_FAIL_COND(!d.has("radius"));
809 ERR_FAIL_COND(!d.has("height"));
810 _setup(d["height"], d["radius"]);
811}
812
813Variant GodotCylinderShape3D::get_data() const {
814 Dictionary d;
815 d["radius"] = radius;
816 d["height"] = height;
817 return d;
818}
819
820GodotCylinderShape3D::GodotCylinderShape3D() {}
821
822/********** CONVEX POLYGON *************/
823
824void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
825 uint32_t vertex_count = mesh.vertices.size();
826 if (vertex_count == 0) {
827 return;
828 }
829
830 const Vector3 *vrts = &mesh.vertices[0];
831
832 if (vertex_count > 3 * extreme_vertices.size()) {
833 // For a large mesh, two calls to get_support() is faster than a full
834 // scan over all vertices.
835
836 Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
837 r_min = p_normal.dot(p_transform.xform(get_support(-n)));
838 r_max = p_normal.dot(p_transform.xform(get_support(n)));
839 } else {
840 for (uint32_t i = 0; i < vertex_count; i++) {
841 real_t d = p_normal.dot(p_transform.xform(vrts[i]));
842
843 if (i == 0 || d > r_max) {
844 r_max = d;
845 }
846 if (i == 0 || d < r_min) {
847 r_min = d;
848 }
849 }
850 }
851}
852
853Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
854 // Skip if there are no vertices in the mesh
855 if (mesh.vertices.size() == 0) {
856 return Vector3();
857 }
858
859 // Get the array of vertices
860 const Vector3 *const vertices_array = mesh.vertices.ptr();
861
862 // Start with an initial assumption of the first extreme vertex.
863 int best_vertex = extreme_vertices[0];
864 real_t max_support = p_normal.dot(vertices_array[best_vertex]);
865
866 // Check the remaining extreme vertices for a better vertex.
867 for (const int &vert : extreme_vertices) {
868 real_t s = p_normal.dot(vertices_array[vert]);
869 if (s > max_support) {
870 best_vertex = vert;
871 max_support = s;
872 }
873 }
874
875 // If we checked all vertices in the mesh then we're done.
876 if (extreme_vertices.size() == mesh.vertices.size()) {
877 return vertices_array[best_vertex];
878 }
879
880 // Move along the surface until we reach the true support vertex.
881 int last_vertex = -1;
882 while (true) {
883 int next_vertex = -1;
884
885 // Iterate over all the neighbors checking for a better vertex.
886 for (const int &vert : vertex_neighbors[best_vertex]) {
887 if (vert != last_vertex) {
888 real_t s = p_normal.dot(vertices_array[vert]);
889 if (s > max_support) {
890 next_vertex = vert;
891 max_support = s;
892 break;
893 }
894 }
895 }
896
897 // No better vertex found, we have the best
898 if (next_vertex == -1) {
899 return vertices_array[best_vertex];
900 }
901
902 // Move to the better vertex and try again
903 last_vertex = best_vertex;
904 best_vertex = next_vertex;
905 }
906}
907
908void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
909 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
910 int fc = mesh.faces.size();
911
912 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
913 int ec = mesh.edges.size();
914
915 const Vector3 *vertices = mesh.vertices.ptr();
916 int vc = mesh.vertices.size();
917
918 r_amount = 0;
919 ERR_FAIL_COND_MSG(vc == 0, "Convex polygon shape has no vertices.");
920
921 //find vertex first
922 real_t max = 0;
923 int vtx = 0;
924
925 for (int i = 0; i < vc; i++) {
926 real_t d = p_normal.dot(vertices[i]);
927
928 if (i == 0 || d > max) {
929 max = d;
930 vtx = i;
931 }
932 }
933
934 for (int i = 0; i < fc; i++) {
935 if (faces[i].plane.normal.dot(p_normal) > face_support_threshold) {
936 int ic = faces[i].indices.size();
937 const int *ind = faces[i].indices.ptr();
938
939 bool valid = false;
940 for (int j = 0; j < ic; j++) {
941 if (ind[j] == vtx) {
942 valid = true;
943 break;
944 }
945 }
946
947 if (!valid) {
948 continue;
949 }
950
951 int m = MIN(p_max, ic);
952 for (int j = 0; j < m; j++) {
953 r_supports[j] = vertices[ind[j]];
954 }
955 r_amount = m;
956 r_type = FEATURE_FACE;
957 return;
958 }
959 }
960
961 for (int i = 0; i < ec; i++) {
962 real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal);
963 dot = ABS(dot);
964 if (dot < edge_support_threshold_lower && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) {
965 r_amount = 2;
966 r_type = FEATURE_EDGE;
967 r_supports[0] = vertices[edges[i].vertex_a];
968 r_supports[1] = vertices[edges[i].vertex_b];
969 return;
970 }
971 }
972
973 r_supports[0] = vertices[vtx];
974 r_amount = 1;
975 r_type = FEATURE_POINT;
976}
977
978bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
979 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
980 int fc = mesh.faces.size();
981
982 const Vector3 *vertices = mesh.vertices.ptr();
983
984 Vector3 n = p_end - p_begin;
985 real_t min = 1e20;
986 bool col = false;
987
988 for (int i = 0; i < fc; i++) {
989 if (faces[i].plane.normal.dot(n) > 0) {
990 continue; //opposing face
991 }
992
993 int ic = faces[i].indices.size();
994 const int *ind = faces[i].indices.ptr();
995
996 for (int j = 1; j < ic - 1; j++) {
997 Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]);
998 Vector3 result;
999 if (f.intersects_segment(p_begin, p_end, &result)) {
1000 real_t d = n.dot(result);
1001 if (d < min) {
1002 min = d;
1003 r_result = result;
1004 r_normal = faces[i].plane.normal;
1005 col = true;
1006 }
1007
1008 break;
1009 }
1010 }
1011 }
1012
1013 return col;
1014}
1015
1016bool GodotConvexPolygonShape3D::intersect_point(const Vector3 &p_point) const {
1017 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
1018 int fc = mesh.faces.size();
1019
1020 for (int i = 0; i < fc; i++) {
1021 if (faces[i].plane.distance_to(p_point) >= 0) {
1022 return false;
1023 }
1024 }
1025
1026 return true;
1027}
1028
1029Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) const {
1030 const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
1031 int fc = mesh.faces.size();
1032 const Vector3 *vertices = mesh.vertices.ptr();
1033
1034 bool all_inside = true;
1035 for (int i = 0; i < fc; i++) {
1036 if (!faces[i].plane.is_point_over(p_point)) {
1037 continue;
1038 }
1039
1040 all_inside = false;
1041 bool is_inside = true;
1042 int ic = faces[i].indices.size();
1043 const int *indices = faces[i].indices.ptr();
1044
1045 for (int j = 0; j < ic; j++) {
1046 Vector3 a = vertices[indices[j]];
1047 Vector3 b = vertices[indices[(j + 1) % ic]];
1048 Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
1049 if (Plane(n, a).is_point_over(p_point)) {
1050 is_inside = false;
1051 break;
1052 }
1053 }
1054
1055 if (is_inside) {
1056 return faces[i].plane.project(p_point);
1057 }
1058 }
1059
1060 if (all_inside) {
1061 return p_point;
1062 }
1063
1064 real_t min_distance = 1e20;
1065 Vector3 min_point;
1066
1067 //check edges
1068 const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
1069 int ec = mesh.edges.size();
1070 for (int i = 0; i < ec; i++) {
1071 Vector3 s[2] = {
1072 vertices[edges[i].vertex_a],
1073 vertices[edges[i].vertex_b]
1074 };
1075
1076 Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s);
1077 real_t d = closest.distance_to(p_point);
1078 if (d < min_distance) {
1079 min_distance = d;
1080 min_point = closest;
1081 }
1082 }
1083
1084 return min_point;
1085}
1086
1087Vector3 GodotConvexPolygonShape3D::get_moment_of_inertia(real_t p_mass) const {
1088 // use bad AABB approximation
1089 Vector3 extents = get_aabb().size * 0.5;
1090
1091 return Vector3(
1092 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
1093 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
1094 (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
1095}
1096
1097void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
1098 Error err = ConvexHullComputer::convex_hull(p_vertices, mesh);
1099 if (err != OK) {
1100 ERR_PRINT("Failed to build convex hull");
1101 }
1102 extreme_vertices.resize(0);
1103 vertex_neighbors.resize(0);
1104
1105 AABB _aabb;
1106
1107 for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
1108 if (i == 0) {
1109 _aabb.position = mesh.vertices[i];
1110 } else {
1111 _aabb.expand_to(mesh.vertices[i]);
1112 }
1113 }
1114
1115 configure(_aabb);
1116
1117 // Pre-compute the extreme vertices in 26 directions. This will be used
1118 // to speed up get_support() by letting us quickly get a good guess for
1119 // the support vertex.
1120
1121 for (int x = -1; x < 2; x++) {
1122 for (int y = -1; y < 2; y++) {
1123 for (int z = -1; z < 2; z++) {
1124 if (x != 0 || y != 0 || z != 0) {
1125 Vector3 dir(x, y, z);
1126 dir.normalize();
1127 real_t max_support = 0.0;
1128 int best_vertex = -1;
1129 for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
1130 real_t s = dir.dot(mesh.vertices[i]);
1131 if (best_vertex == -1 || s > max_support) {
1132 best_vertex = i;
1133 max_support = s;
1134 }
1135 }
1136 if (extreme_vertices.find(best_vertex) == -1)
1137 extreme_vertices.push_back(best_vertex);
1138 }
1139 }
1140 }
1141 }
1142
1143 // Record all the neighbors of each vertex. This is used in get_support().
1144
1145 if (extreme_vertices.size() < mesh.vertices.size()) {
1146 vertex_neighbors.resize(mesh.vertices.size());
1147 for (Geometry3D::MeshData::Edge &edge : mesh.edges) {
1148 vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b);
1149 vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a);
1150 }
1151 }
1152}
1153
1154void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
1155 _setup(p_data);
1156}
1157
1158Variant GodotConvexPolygonShape3D::get_data() const {
1159 Vector<Vector3> vertices;
1160 vertices.resize(mesh.vertices.size());
1161 for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
1162 vertices.write[i] = mesh.vertices[i];
1163 }
1164 return vertices;
1165}
1166
1167GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() {
1168}
1169
1170/********** FACE POLYGON *************/
1171
1172void GodotFaceShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
1173 for (int i = 0; i < 3; i++) {
1174 Vector3 v = p_transform.xform(vertex[i]);
1175 real_t d = p_normal.dot(v);
1176
1177 if (i == 0 || d > r_max) {
1178 r_max = d;
1179 }
1180
1181 if (i == 0 || d < r_min) {
1182 r_min = d;
1183 }
1184 }
1185}
1186
1187Vector3 GodotFaceShape3D::get_support(const Vector3 &p_normal) const {
1188 int vert_support_idx = -1;
1189 real_t support_max = 0;
1190
1191 for (int i = 0; i < 3; i++) {
1192 real_t d = p_normal.dot(vertex[i]);
1193
1194 if (i == 0 || d > support_max) {
1195 support_max = d;
1196 vert_support_idx = i;
1197 }
1198 }
1199
1200 return vertex[vert_support_idx];
1201}
1202
1203void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
1204 Vector3 n = p_normal;
1205
1206 /** TEST FACE AS SUPPORT **/
1207 if (Math::abs(normal.dot(n)) > face_support_threshold) {
1208 r_amount = 3;
1209 r_type = FEATURE_FACE;
1210 for (int i = 0; i < 3; i++) {
1211 r_supports[i] = vertex[i];
1212 }
1213 return;
1214 }
1215
1216 /** FIND SUPPORT VERTEX **/
1217
1218 int vert_support_idx = -1;
1219 real_t support_max = 0;
1220
1221 for (int i = 0; i < 3; i++) {
1222 real_t d = n.dot(vertex[i]);
1223
1224 if (i == 0 || d > support_max) {
1225 support_max = d;
1226 vert_support_idx = i;
1227 }
1228 }
1229
1230 /** TEST EDGES AS SUPPORT **/
1231
1232 for (int i = 0; i < 3; i++) {
1233 int nx = (i + 1) % 3;
1234 if (i != vert_support_idx && nx != vert_support_idx) {
1235 continue;
1236 }
1237
1238 // check if edge is valid as a support
1239 real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n);
1240 dot = ABS(dot);
1241 if (dot < edge_support_threshold_lower) {
1242 r_amount = 2;
1243 r_type = FEATURE_EDGE;
1244 r_supports[0] = vertex[i];
1245 r_supports[1] = vertex[nx];
1246 return;
1247 }
1248 }
1249
1250 r_amount = 1;
1251 r_type = FEATURE_POINT;
1252 r_supports[0] = vertex[vert_support_idx];
1253}
1254
1255bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
1256 bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
1257 if (c) {
1258 r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
1259 if (r_normal.dot(p_end - p_begin) > 0) {
1260 if (backface_collision && p_hit_back_faces) {
1261 r_normal = -r_normal;
1262 } else {
1263 c = false;
1264 }
1265 }
1266 }
1267
1268 return c;
1269}
1270
1271bool GodotFaceShape3D::intersect_point(const Vector3 &p_point) const {
1272 return false; //face is flat
1273}
1274
1275Vector3 GodotFaceShape3D::get_closest_point_to(const Vector3 &p_point) const {
1276 return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
1277}
1278
1279Vector3 GodotFaceShape3D::get_moment_of_inertia(real_t p_mass) const {
1280 return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
1281}
1282
1283GodotFaceShape3D::GodotFaceShape3D() {
1284 configure(AABB());
1285}
1286
1287Vector<Vector3> GodotConcavePolygonShape3D::get_faces() const {
1288 Vector<Vector3> rfaces;
1289 rfaces.resize(faces.size() * 3);
1290
1291 for (int i = 0; i < faces.size(); i++) {
1292 Face f = faces.get(i);
1293
1294 for (int j = 0; j < 3; j++) {
1295 rfaces.set(i * 3 + j, vertices.get(f.indices[j]));
1296 }
1297 }
1298
1299 return rfaces;
1300}
1301
1302void GodotConcavePolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
1303 int count = vertices.size();
1304 if (count == 0) {
1305 r_min = 0;
1306 r_max = 0;
1307 return;
1308 }
1309 const Vector3 *vptr = vertices.ptr();
1310
1311 for (int i = 0; i < count; i++) {
1312 real_t d = p_normal.dot(p_transform.xform(vptr[i]));
1313
1314 if (i == 0 || d > r_max) {
1315 r_max = d;
1316 }
1317 if (i == 0 || d < r_min) {
1318 r_min = d;
1319 }
1320 }
1321}
1322
1323Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const {
1324 int count = vertices.size();
1325 if (count == 0) {
1326 return Vector3();
1327 }
1328
1329 const Vector3 *vptr = vertices.ptr();
1330
1331 Vector3 n = p_normal;
1332
1333 int vert_support_idx = -1;
1334 real_t support_max = 0;
1335
1336 for (int i = 0; i < count; i++) {
1337 real_t d = n.dot(vptr[i]);
1338
1339 if (i == 0 || d > support_max) {
1340 support_max = d;
1341 vert_support_idx = i;
1342 }
1343 }
1344
1345 return vptr[vert_support_idx];
1346}
1347
1348void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
1349 const BVH *params_bvh = &p_params->bvh[p_idx];
1350
1351 if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
1352 return;
1353 }
1354
1355 if (params_bvh->face_index >= 0) {
1356 const Face *f = &p_params->faces[params_bvh->face_index];
1357 GodotFaceShape3D *face = p_params->face;
1358 face->normal = f->normal;
1359 face->vertex[0] = p_params->vertices[f->indices[0]];
1360 face->vertex[1] = p_params->vertices[f->indices[1]];
1361 face->vertex[2] = p_params->vertices[f->indices[2]];
1362
1363 Vector3 res;
1364 Vector3 normal;
1365 int face_index = params_bvh->face_index;
1366 if (face->intersect_segment(p_params->from, p_params->to, res, normal, face_index, true)) {
1367 real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
1368 if ((d > 0) && (d < p_params->min_d)) {
1369 p_params->min_d = d;
1370 p_params->result = res;
1371 p_params->normal = normal;
1372 p_params->face_index = face_index;
1373 p_params->collisions++;
1374 }
1375 }
1376 } else {
1377 if (params_bvh->left >= 0) {
1378 _cull_segment(params_bvh->left, p_params);
1379 }
1380 if (params_bvh->right >= 0) {
1381 _cull_segment(params_bvh->right, p_params);
1382 }
1383 }
1384}
1385
1386bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
1387 if (faces.size() == 0) {
1388 return false;
1389 }
1390
1391 // unlock data
1392 const Face *fr = faces.ptr();
1393 const Vector3 *vr = vertices.ptr();
1394 const BVH *br = bvh.ptr();
1395
1396 GodotFaceShape3D face;
1397 face.backface_collision = backface_collision && p_hit_back_faces;
1398
1399 _SegmentCullParams params;
1400 params.from = p_begin;
1401 params.to = p_end;
1402 params.dir = (p_end - p_begin).normalized();
1403
1404 params.faces = fr;
1405 params.vertices = vr;
1406 params.bvh = br;
1407
1408 params.face = &face;
1409
1410 // cull
1411 _cull_segment(0, &params);
1412
1413 if (params.collisions > 0) {
1414 r_result = params.result;
1415 r_normal = params.normal;
1416 r_face_index = params.face_index;
1417 return true;
1418 } else {
1419 return false;
1420 }
1421}
1422
1423bool GodotConcavePolygonShape3D::intersect_point(const Vector3 &p_point) const {
1424 return false; //face is flat
1425}
1426
1427Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) const {
1428 return Vector3();
1429}
1430
1431bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
1432 const BVH *params_bvh = &p_params->bvh[p_idx];
1433
1434 if (!p_params->aabb.intersects(params_bvh->aabb)) {
1435 return false;
1436 }
1437
1438 if (params_bvh->face_index >= 0) {
1439 const Face *f = &p_params->faces[params_bvh->face_index];
1440 GodotFaceShape3D *face = p_params->face;
1441 face->normal = f->normal;
1442 face->vertex[0] = p_params->vertices[f->indices[0]];
1443 face->vertex[1] = p_params->vertices[f->indices[1]];
1444 face->vertex[2] = p_params->vertices[f->indices[2]];
1445 if (p_params->callback(p_params->userdata, face)) {
1446 return true;
1447 }
1448 } else {
1449 if (params_bvh->left >= 0) {
1450 if (_cull(params_bvh->left, p_params)) {
1451 return true;
1452 }
1453 }
1454
1455 if (params_bvh->right >= 0) {
1456 if (_cull(params_bvh->right, p_params)) {
1457 return true;
1458 }
1459 }
1460 }
1461
1462 return false;
1463}
1464
1465void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
1466 // make matrix local to concave
1467 if (faces.size() == 0) {
1468 return;
1469 }
1470
1471 AABB local_aabb = p_local_aabb;
1472
1473 // unlock data
1474 const Face *fr = faces.ptr();
1475 const Vector3 *vr = vertices.ptr();
1476 const BVH *br = bvh.ptr();
1477
1478 GodotFaceShape3D face; // use this to send in the callback
1479 face.backface_collision = backface_collision;
1480 face.invert_backface_collision = p_invert_backface_collision;
1481
1482 _CullParams params;
1483 params.aabb = local_aabb;
1484 params.face = &face;
1485 params.faces = fr;
1486 params.vertices = vr;
1487 params.bvh = br;
1488 params.callback = p_callback;
1489 params.userdata = p_userdata;
1490
1491 // cull
1492 _cull(0, &params);
1493}
1494
1495Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const {
1496 // use bad AABB approximation
1497 Vector3 extents = get_aabb().size * 0.5;
1498
1499 return Vector3(
1500 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
1501 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
1502 (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
1503}
1504
1505struct _Volume_BVH_Element {
1506 AABB aabb;
1507 Vector3 center;
1508 int face_index = 0;
1509};
1510
1511struct _Volume_BVH_CompareX {
1512 _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
1513 return a.center.x < b.center.x;
1514 }
1515};
1516
1517struct _Volume_BVH_CompareY {
1518 _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
1519 return a.center.y < b.center.y;
1520 }
1521};
1522
1523struct _Volume_BVH_CompareZ {
1524 _FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
1525 return a.center.z < b.center.z;
1526 }
1527};
1528
1529struct _Volume_BVH {
1530 AABB aabb;
1531 _Volume_BVH *left = nullptr;
1532 _Volume_BVH *right = nullptr;
1533
1534 int face_index = 0;
1535};
1536
1537_Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) {
1538 _Volume_BVH *bvh = memnew(_Volume_BVH);
1539
1540 if (p_size == 1) {
1541 //leaf
1542 bvh->aabb = p_elements[0].aabb;
1543 bvh->left = nullptr;
1544 bvh->right = nullptr;
1545 bvh->face_index = p_elements->face_index;
1546 count++;
1547 return bvh;
1548 } else {
1549 bvh->face_index = -1;
1550 }
1551
1552 AABB aabb;
1553 for (int i = 0; i < p_size; i++) {
1554 if (i == 0) {
1555 aabb = p_elements[i].aabb;
1556 } else {
1557 aabb.merge_with(p_elements[i].aabb);
1558 }
1559 }
1560 bvh->aabb = aabb;
1561 switch (aabb.get_longest_axis_index()) {
1562 case 0: {
1563 SortArray<_Volume_BVH_Element, _Volume_BVH_CompareX> sort_x;
1564 sort_x.sort(p_elements, p_size);
1565
1566 } break;
1567 case 1: {
1568 SortArray<_Volume_BVH_Element, _Volume_BVH_CompareY> sort_y;
1569 sort_y.sort(p_elements, p_size);
1570 } break;
1571 case 2: {
1572 SortArray<_Volume_BVH_Element, _Volume_BVH_CompareZ> sort_z;
1573 sort_z.sort(p_elements, p_size);
1574 } break;
1575 }
1576
1577 int split = p_size / 2;
1578 bvh->left = _volume_build_bvh(p_elements, split, count);
1579 bvh->right = _volume_build_bvh(&p_elements[split], p_size - split, count);
1580
1581 //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
1582 count++;
1583 return bvh;
1584}
1585
1586void GodotConcavePolygonShape3D::_fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
1587 int idx = p_idx;
1588
1589 p_bvh_array[idx].aabb = p_bvh_tree->aabb;
1590 p_bvh_array[idx].face_index = p_bvh_tree->face_index;
1591 //printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
1592
1593 if (p_bvh_tree->left) {
1594 p_bvh_array[idx].left = ++p_idx;
1595 _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx);
1596
1597 } else {
1598 p_bvh_array[p_idx].left = -1;
1599 }
1600
1601 if (p_bvh_tree->right) {
1602 p_bvh_array[idx].right = ++p_idx;
1603 _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx);
1604
1605 } else {
1606 p_bvh_array[p_idx].right = -1;
1607 }
1608
1609 memdelete(p_bvh_tree);
1610}
1611
1612void GodotConcavePolygonShape3D::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) {
1613 int src_face_count = p_faces.size();
1614 if (src_face_count == 0) {
1615 configure(AABB());
1616 return;
1617 }
1618 ERR_FAIL_COND(src_face_count % 3);
1619 src_face_count /= 3;
1620
1621 const Vector3 *facesr = p_faces.ptr();
1622
1623 Vector<_Volume_BVH_Element> bvh_array;
1624 bvh_array.resize(src_face_count);
1625
1626 _Volume_BVH_Element *bvh_arrayw = bvh_array.ptrw();
1627
1628 faces.resize(src_face_count);
1629 Face *facesw = faces.ptrw();
1630
1631 vertices.resize(src_face_count * 3);
1632
1633 Vector3 *verticesw = vertices.ptrw();
1634
1635 AABB _aabb;
1636
1637 for (int i = 0; i < src_face_count; i++) {
1638 Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
1639
1640 bvh_arrayw[i].aabb = face.get_aabb();
1641 bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center();
1642 bvh_arrayw[i].face_index = i;
1643 facesw[i].indices[0] = i * 3 + 0;
1644 facesw[i].indices[1] = i * 3 + 1;
1645 facesw[i].indices[2] = i * 3 + 2;
1646 facesw[i].normal = face.get_plane().normal;
1647 verticesw[i * 3 + 0] = face.vertex[0];
1648 verticesw[i * 3 + 1] = face.vertex[1];
1649 verticesw[i * 3 + 2] = face.vertex[2];
1650 if (i == 0) {
1651 _aabb = bvh_arrayw[i].aabb;
1652 } else {
1653 _aabb.merge_with(bvh_arrayw[i].aabb);
1654 }
1655 }
1656
1657 int count = 0;
1658 _Volume_BVH *bvh_tree = _volume_build_bvh(bvh_arrayw, src_face_count, count);
1659
1660 bvh.resize(count + 1);
1661
1662 BVH *bvh_arrayw2 = bvh.ptrw();
1663
1664 int idx = 0;
1665 _fill_bvh(bvh_tree, bvh_arrayw2, idx);
1666
1667 backface_collision = p_backface_collision;
1668
1669 configure(_aabb); // this type of shape has no margin
1670}
1671
1672void GodotConcavePolygonShape3D::set_data(const Variant &p_data) {
1673 Dictionary d = p_data;
1674 ERR_FAIL_COND(!d.has("faces"));
1675
1676 _setup(d["faces"], d["backface_collision"]);
1677}
1678
1679Variant GodotConcavePolygonShape3D::get_data() const {
1680 Dictionary d;
1681 d["faces"] = get_faces();
1682 d["backface_collision"] = backface_collision;
1683
1684 return d;
1685}
1686
1687GodotConcavePolygonShape3D::GodotConcavePolygonShape3D() {
1688}
1689
1690/* HEIGHT MAP SHAPE */
1691
1692Vector<real_t> GodotHeightMapShape3D::get_heights() const {
1693 return heights;
1694}
1695
1696int GodotHeightMapShape3D::get_width() const {
1697 return width;
1698}
1699
1700int GodotHeightMapShape3D::get_depth() const {
1701 return depth;
1702}
1703
1704void GodotHeightMapShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
1705 //not very useful, but not very used either
1706 p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal), r_min, r_max);
1707}
1708
1709Vector3 GodotHeightMapShape3D::get_support(const Vector3 &p_normal) const {
1710 //not very useful, but not very used either
1711 return get_aabb().get_support(p_normal);
1712}
1713
1714struct _HeightmapSegmentCullParams {
1715 Vector3 from;
1716 Vector3 to;
1717 Vector3 dir;
1718
1719 Vector3 result;
1720 Vector3 normal;
1721
1722 const GodotHeightMapShape3D *heightmap = nullptr;
1723 GodotFaceShape3D *face = nullptr;
1724};
1725
1726struct _HeightmapGridCullState {
1727 real_t length = 0.0;
1728 real_t length_flat = 0.0;
1729
1730 real_t dist = 0.0;
1731 real_t prev_dist = 0.0;
1732
1733 int x = 0;
1734 int z = 0;
1735};
1736
1737_FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) {
1738 Vector3 res;
1739 Vector3 normal;
1740 int fi = -1;
1741 if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal, fi, true)) {
1742 p_params.result = res;
1743 p_params.normal = normal;
1744
1745 return true;
1746 }
1747
1748 return false;
1749}
1750
1751_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
1752 // First triangle.
1753 p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]);
1754 p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]);
1755 p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]);
1756 p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
1757 if (_heightmap_face_cull_segment(p_params)) {
1758 return true;
1759 }
1760
1761 // Second triangle.
1762 p_params.face->vertex[0] = p_params.face->vertex[1];
1763 p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]);
1764 p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
1765 if (_heightmap_face_cull_segment(p_params)) {
1766 return true;
1767 }
1768
1769 return false;
1770}
1771
1772_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
1773 const GodotHeightMapShape3D::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z);
1774
1775 Vector3 enter_pos;
1776 Vector3 exit_pos;
1777
1778 if (p_state.length_flat > CMP_EPSILON) {
1779 real_t flat_to_3d = p_state.length / p_state.length_flat;
1780 real_t enter_param = p_state.prev_dist * flat_to_3d;
1781 real_t exit_param = p_state.dist * flat_to_3d;
1782 enter_pos = p_params.from + p_params.dir * enter_param;
1783 exit_pos = p_params.from + p_params.dir * exit_param;
1784 } else {
1785 // Consider the ray vertical.
1786 // (though we shouldn't reach this often because there is an early check up-front)
1787 enter_pos = p_params.from;
1788 exit_pos = p_params.to;
1789 }
1790
1791 // Transform positions to heightmap space.
1792 enter_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE;
1793 exit_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE;
1794
1795 // We did enter the flat projection of the AABB,
1796 // but we have to check if we intersect it on the vertical axis.
1797 if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) {
1798 return false;
1799 }
1800 if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) {
1801 return false;
1802 }
1803
1804 return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal);
1805}
1806
1807template <typename ProcessFunction>
1808bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const {
1809 Vector3 delta = (p_end - p_begin);
1810 real_t length = delta.length();
1811
1812 if (length < CMP_EPSILON) {
1813 return false;
1814 }
1815
1816 Vector3 local_begin = p_begin + offset;
1817
1818 GodotFaceShape3D face;
1819 face.backface_collision = false;
1820
1821 _HeightmapSegmentCullParams params;
1822 params.from = p_begin;
1823 params.to = p_end;
1824 params.dir = delta / length;
1825 params.heightmap = this;
1826 params.face = &face;
1827
1828 _HeightmapGridCullState state;
1829
1830 // Perform grid query from projected ray.
1831 Vector2 ray_dir_flat(delta.x, delta.z);
1832 state.length = length;
1833 state.length_flat = ray_dir_flat.length();
1834
1835 if (state.length_flat < CMP_EPSILON) {
1836 ray_dir_flat = Vector2();
1837 } else {
1838 ray_dir_flat /= state.length_flat;
1839 }
1840
1841 const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0);
1842 const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0);
1843
1844 const real_t infinite = 1e20;
1845 const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite;
1846 const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite;
1847
1848 real_t cross_x; // At which value of `param` we will cross a x-axis lane?
1849 real_t cross_z; // At which value of `param` we will cross a z-axis lane?
1850
1851 // X initialization.
1852 if (x_step != 0) {
1853 if (x_step == 1) {
1854 cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x;
1855 } else {
1856 cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x;
1857 }
1858 } else {
1859 cross_x = infinite; // Will never cross on X.
1860 }
1861
1862 // Z initialization.
1863 if (z_step != 0) {
1864 if (z_step == 1) {
1865 cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z;
1866 } else {
1867 cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z;
1868 }
1869 } else {
1870 cross_z = infinite; // Will never cross on Z.
1871 }
1872
1873 int x = Math::floor(local_begin.x);
1874 int z = Math::floor(local_begin.z);
1875
1876 // Workaround cases where the ray starts at an integer position.
1877 if (Math::is_zero_approx(cross_x)) {
1878 cross_x += delta_x;
1879 // If going backwards, we should ignore the position we would get by the above flooring,
1880 // because the ray is not heading in that direction.
1881 if (x_step == -1) {
1882 x -= 1;
1883 }
1884 }
1885
1886 if (Math::is_zero_approx(cross_z)) {
1887 cross_z += delta_z;
1888 if (z_step == -1) {
1889 z -= 1;
1890 }
1891 }
1892
1893 // Start inside the grid.
1894 int x_start = MAX(MIN(x, p_width - 2), 0);
1895 int z_start = MAX(MIN(z, p_depth - 2), 0);
1896
1897 // Adjust initial cross values.
1898 cross_x += delta_x * x_step * (x_start - x);
1899 cross_z += delta_z * z_step * (z_start - z);
1900
1901 x = x_start;
1902 z = z_start;
1903
1904 while (true) {
1905 state.prev_dist = state.dist;
1906 state.x = x;
1907 state.z = z;
1908
1909 if (cross_x < cross_z) {
1910 // X lane.
1911 x += x_step;
1912 // Assign before advancing the param,
1913 // to be in sync with the initialization step.
1914 state.dist = cross_x;
1915 cross_x += delta_x;
1916 } else {
1917 // Z lane.
1918 z += z_step;
1919 state.dist = cross_z;
1920 cross_z += delta_z;
1921 }
1922
1923 if (state.dist > state.length_flat) {
1924 state.dist = state.length_flat;
1925 if (p_process(params, state)) {
1926 r_point = params.result;
1927 r_normal = params.normal;
1928 return true;
1929 }
1930 break;
1931 }
1932
1933 if (p_process(params, state)) {
1934 r_point = params.result;
1935 r_normal = params.normal;
1936 return true;
1937 }
1938
1939 // Stop when outside the grid.
1940 if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) {
1941 break;
1942 }
1943 }
1944
1945 return false;
1946}
1947
1948bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, int &r_face_index, bool p_hit_back_faces) const {
1949 if (heights.is_empty()) {
1950 return false;
1951 }
1952
1953 Vector3 local_begin = p_begin + local_origin;
1954 Vector3 local_end = p_end + local_origin;
1955
1956 // Quantize the ray begin/end.
1957 int begin_x = Math::floor(local_begin.x);
1958 int begin_z = Math::floor(local_begin.z);
1959 int end_x = Math::floor(local_end.x);
1960 int end_z = Math::floor(local_end.z);
1961
1962 if ((begin_x == end_x) && (begin_z == end_z)) {
1963 // Simple case for rays that don't traverse the grid horizontally.
1964 // Just perform a test on the given cell.
1965 GodotFaceShape3D face;
1966 face.backface_collision = p_hit_back_faces;
1967
1968 _HeightmapSegmentCullParams params;
1969 params.from = p_begin;
1970 params.to = p_end;
1971 params.dir = (p_end - p_begin).normalized();
1972
1973 params.heightmap = this;
1974 params.face = &face;
1975
1976 _HeightmapGridCullState state;
1977 state.x = MAX(MIN(begin_x, width - 2), 0);
1978 state.z = MAX(MIN(begin_z, depth - 2), 0);
1979 if (_heightmap_cell_cull_segment(params, state)) {
1980 r_point = params.result;
1981 r_normal = params.normal;
1982 return true;
1983 }
1984 } else if (bounds_grid.is_empty()) {
1985 // Process all cells intersecting the flat projection of the ray.
1986 return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
1987 } else {
1988 Vector3 ray_diff = (p_end - p_begin);
1989 real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z;
1990 if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) {
1991 // Don't use chunks, the ray is too short in the plane.
1992 return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
1993 } else {
1994 // The ray is long, run raycast on a higher-level grid.
1995 Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE;
1996 Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE;
1997 Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE;
1998 return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal);
1999 }
2000 }
2001
2002 return false;
2003}
2004
2005bool GodotHeightMapShape3D::intersect_point(const Vector3 &p_point) const {
2006 return false;
2007}
2008
2009Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) const {
2010 return Vector3();
2011}
2012
2013void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
2014 const AABB &shape_aabb = get_aabb();
2015
2016 Vector3 pos_local = shape_aabb.position + local_origin;
2017
2018 Vector3 clamped_point(p_point);
2019 clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x);
2020 clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y);
2021 clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.z + shape_aabb.size.z);
2022
2023 r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
2024 r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
2025 r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
2026}
2027
2028void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
2029 if (heights.is_empty()) {
2030 return;
2031 }
2032
2033 AABB local_aabb = p_local_aabb;
2034 local_aabb.position += local_origin;
2035
2036 // Quantize the aabb, and adjust the start/end ranges.
2037 int aabb_min[3];
2038 int aabb_max[3];
2039 _get_cell(local_aabb.position, aabb_min[0], aabb_min[1], aabb_min[2]);
2040 _get_cell(local_aabb.position + local_aabb.size, aabb_max[0], aabb_max[1], aabb_max[2]);
2041
2042 // Expand the min/max quantized values.
2043 // This is to catch the case where the input aabb falls between grid points.
2044 for (int i = 0; i < 3; ++i) {
2045 aabb_min[i]--;
2046 aabb_max[i]++;
2047 }
2048
2049 int start_x = MAX(0, aabb_min[0]);
2050 int end_x = MIN(width - 1, aabb_max[0]);
2051 int start_z = MAX(0, aabb_min[2]);
2052 int end_z = MIN(depth - 1, aabb_max[2]);
2053
2054 GodotFaceShape3D face;
2055 face.backface_collision = !p_invert_backface_collision;
2056 face.invert_backface_collision = p_invert_backface_collision;
2057
2058 for (int z = start_z; z < end_z; z++) {
2059 for (int x = start_x; x < end_x; x++) {
2060 // First triangle.
2061 _get_point(x, z, face.vertex[0]);
2062 _get_point(x + 1, z, face.vertex[1]);
2063 _get_point(x, z + 1, face.vertex[2]);
2064 face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
2065 if (p_callback(p_userdata, &face)) {
2066 return;
2067 }
2068
2069 // Second triangle.
2070 face.vertex[0] = face.vertex[1];
2071 _get_point(x + 1, z + 1, face.vertex[1]);
2072 face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
2073 if (p_callback(p_userdata, &face)) {
2074 return;
2075 }
2076 }
2077 }
2078}
2079
2080Vector3 GodotHeightMapShape3D::get_moment_of_inertia(real_t p_mass) const {
2081 // use bad AABB approximation
2082 Vector3 extents = get_aabb().size * 0.5;
2083
2084 return Vector3(
2085 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
2086 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
2087 (p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
2088}
2089
2090void GodotHeightMapShape3D::_build_accelerator() {
2091 bounds_grid.clear();
2092
2093 bounds_grid_width = width / BOUNDS_CHUNK_SIZE;
2094 bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE;
2095
2096 if (width % BOUNDS_CHUNK_SIZE > 0) {
2097 ++bounds_grid_width; // In case terrain size isn't dividable by chunk size.
2098 }
2099
2100 if (depth % BOUNDS_CHUNK_SIZE > 0) {
2101 ++bounds_grid_depth;
2102 }
2103
2104 uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth);
2105
2106 if (bound_grid_size < 2) {
2107 // Grid is empty or just one chunk.
2108 return;
2109 }
2110
2111 bounds_grid.resize(bound_grid_size);
2112
2113 // Compute min and max height for all chunks.
2114 for (int cz = 0; cz < bounds_grid_depth; ++cz) {
2115 int z0 = cz * BOUNDS_CHUNK_SIZE;
2116
2117 for (int cx = 0; cx < bounds_grid_width; ++cx) {
2118 int x0 = cx * BOUNDS_CHUNK_SIZE;
2119
2120 Range r;
2121
2122 r.min = _get_height(x0, z0);
2123 r.max = r.min;
2124
2125 // Compute min and max height for this chunk.
2126 // We have to include one extra cell to account for neighbors.
2127 // Here is why:
2128 // Say we have a flat terrain, and a plateau that fits a chunk perfectly.
2129 //
2130 // Left Right
2131 // 0---0---0---1---1---1
2132 // | | | | | |
2133 // 0---0---0---1---1---1
2134 // | | | | | |
2135 // 0---0---0---1---1---1
2136 // x
2137 //
2138 // If the AABB for the Left chunk did not share vertices with the Right,
2139 // then we would fail collision tests at x due to a gap.
2140 //
2141 int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth);
2142 int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width);
2143 for (int z = z0; z < z_max; ++z) {
2144 for (int x = x0; x < x_max; ++x) {
2145 real_t height = _get_height(x, z);
2146 if (height < r.min) {
2147 r.min = height;
2148 } else if (height > r.max) {
2149 r.max = height;
2150 }
2151 }
2152 }
2153
2154 bounds_grid[cx + cz * bounds_grid_width] = r;
2155 }
2156 }
2157}
2158
2159void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
2160 heights = p_heights;
2161 width = p_width;
2162 depth = p_depth;
2163
2164 // Initialize aabb.
2165 AABB aabb_new;
2166 aabb_new.position = Vector3(0.0, p_min_height, 0.0);
2167 aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
2168
2169 // Initialize origin as the aabb center.
2170 local_origin = aabb_new.position + 0.5 * aabb_new.size;
2171 local_origin.y = 0.0;
2172
2173 aabb_new.position -= local_origin;
2174
2175 _build_accelerator();
2176
2177 configure(aabb_new);
2178}
2179
2180void GodotHeightMapShape3D::set_data(const Variant &p_data) {
2181 ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
2182
2183 Dictionary d = p_data;
2184 ERR_FAIL_COND(!d.has("width"));
2185 ERR_FAIL_COND(!d.has("depth"));
2186 ERR_FAIL_COND(!d.has("heights"));
2187
2188 int width_new = d["width"];
2189 int depth_new = d["depth"];
2190
2191 ERR_FAIL_COND(width_new <= 0.0);
2192 ERR_FAIL_COND(depth_new <= 0.0);
2193
2194 Variant heights_variant = d["heights"];
2195 Vector<real_t> heights_buffer;
2196#ifdef REAL_T_IS_DOUBLE
2197 if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) {
2198#else
2199 if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) {
2200#endif
2201 // Ready-to-use heights can be passed.
2202 heights_buffer = heights_variant;
2203 } else if (heights_variant.get_type() == Variant::OBJECT) {
2204 // If an image is passed, we have to convert it.
2205 // This would be expensive to do with a script, so it's nice to have it here.
2206 Ref<Image> image = heights_variant;
2207 ERR_FAIL_COND(image.is_null());
2208 ERR_FAIL_COND(image->get_format() != Image::FORMAT_RF);
2209
2210 PackedByteArray im_data = image->get_data();
2211 heights_buffer.resize(image->get_width() * image->get_height());
2212
2213 real_t *w = heights_buffer.ptrw();
2214 real_t *rp = (real_t *)im_data.ptr();
2215 for (int i = 0; i < heights_buffer.size(); ++i) {
2216 w[i] = rp[i];
2217 }
2218 } else {
2219#ifdef REAL_T_IS_DOUBLE
2220 ERR_FAIL_MSG("Expected PackedFloat64Array or float Image.");
2221#else
2222 ERR_FAIL_MSG("Expected PackedFloat32Array or float Image.");
2223#endif
2224 }
2225
2226 // Compute min and max heights or use precomputed values.
2227 real_t min_height = 0.0;
2228 real_t max_height = 0.0;
2229 if (d.has("min_height") && d.has("max_height")) {
2230 min_height = d["min_height"];
2231 max_height = d["max_height"];
2232 } else {
2233 int heights_size = heights.size();
2234 for (int i = 0; i < heights_size; ++i) {
2235 real_t h = heights[i];
2236 if (h < min_height) {
2237 min_height = h;
2238 } else if (h > max_height) {
2239 max_height = h;
2240 }
2241 }
2242 }
2243
2244 ERR_FAIL_COND(min_height > max_height);
2245
2246 ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new));
2247
2248 // If specified, min and max height will be used as precomputed values.
2249 _setup(heights_buffer, width_new, depth_new, min_height, max_height);
2250}
2251
2252Variant GodotHeightMapShape3D::get_data() const {
2253 Dictionary d;
2254 d["width"] = width;
2255 d["depth"] = depth;
2256
2257 const AABB &shape_aabb = get_aabb();
2258 d["min_height"] = shape_aabb.position.y;
2259 d["max_height"] = shape_aabb.position.y + shape_aabb.size.y;
2260
2261 d["heights"] = heights;
2262
2263 return d;
2264}
2265
2266GodotHeightMapShape3D::GodotHeightMapShape3D() {
2267}
2268