1 /*************************************************************************/
2 /* shape_sw.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
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 "shape_sw.h"
32
33 #include "core/math/geometry.h"
34 #include "core/math/quick_hull.h"
35 #include "core/sort_array.h"
36
37 #define _POINT_SNAP 0.001953125
38 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002
39 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998
40
configure(const AABB & p_aabb)41 void ShapeSW::configure(const AABB &p_aabb) {
42 aabb = p_aabb;
43 configured = true;
44 for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) {
45 ShapeOwnerSW *co = (ShapeOwnerSW *)E->key();
46 co->_shape_changed();
47 }
48 }
49
get_support(const Vector3 & p_normal) const50 Vector3 ShapeSW::get_support(const Vector3 &p_normal) const {
51
52 Vector3 res;
53 int amnt;
54 get_supports(p_normal, 1, &res, amnt);
55 return res;
56 }
57
add_owner(ShapeOwnerSW * p_owner)58 void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {
59
60 Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner);
61 if (E) {
62 E->get()++;
63 } else {
64 owners[p_owner] = 1;
65 }
66 }
67
remove_owner(ShapeOwnerSW * p_owner)68 void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) {
69
70 Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner);
71 ERR_FAIL_COND(!E);
72 E->get()--;
73 if (E->get() == 0) {
74 owners.erase(E);
75 }
76 }
77
is_owner(ShapeOwnerSW * p_owner) const78 bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const {
79
80 return owners.has(p_owner);
81 }
82
get_owners() const83 const Map<ShapeOwnerSW *, int> &ShapeSW::get_owners() const {
84 return owners;
85 }
86
ShapeSW()87 ShapeSW::ShapeSW() {
88
89 custom_bias = 0;
90 configured = false;
91 }
92
~ShapeSW()93 ShapeSW::~ShapeSW() {
94
95 ERR_FAIL_COND(owners.size());
96 }
97
get_plane() const98 Plane PlaneShapeSW::get_plane() const {
99
100 return plane;
101 }
102
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const103 void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
104
105 // gibberish, a plane is infinity
106 r_min = -1e7;
107 r_max = 1e7;
108 }
109
get_support(const Vector3 & p_normal) const110 Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const {
111
112 return p_normal * 1e15;
113 }
114
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const115 bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
116
117 bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
118 if (inters)
119 r_normal = plane.normal;
120 return inters;
121 }
122
intersect_point(const Vector3 & p_point) const123 bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const {
124
125 return plane.distance_to(p_point) < 0;
126 }
127
get_closest_point_to(const Vector3 & p_point) const128 Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const {
129
130 if (plane.is_point_over(p_point)) {
131 return plane.project(p_point);
132 } else {
133 return p_point;
134 }
135 }
136
get_moment_of_inertia(real_t p_mass) const137 Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const {
138
139 return Vector3(); //wtf
140 }
141
_setup(const Plane & p_plane)142 void PlaneShapeSW::_setup(const Plane &p_plane) {
143
144 plane = p_plane;
145 configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
146 }
147
set_data(const Variant & p_data)148 void PlaneShapeSW::set_data(const Variant &p_data) {
149
150 _setup(p_data);
151 }
152
get_data() const153 Variant PlaneShapeSW::get_data() const {
154
155 return plane;
156 }
157
PlaneShapeSW()158 PlaneShapeSW::PlaneShapeSW() {
159 }
160
161 //
162
get_length() const163 real_t RayShapeSW::get_length() const {
164
165 return length;
166 }
167
get_slips_on_slope() const168 bool RayShapeSW::get_slips_on_slope() const {
169 return slips_on_slope;
170 }
171
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const172 void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
173
174 // don't think this will be even used
175 r_min = 0;
176 r_max = 1;
177 }
178
get_support(const Vector3 & p_normal) const179 Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const {
180
181 if (p_normal.z > 0)
182 return Vector3(0, 0, length);
183 else
184 return Vector3(0, 0, 0);
185 }
186
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const187 void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
188
189 if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
190
191 r_amount = 2;
192 r_supports[0] = Vector3(0, 0, 0);
193 r_supports[1] = Vector3(0, 0, length);
194 } else if (p_normal.z > 0) {
195 r_amount = 1;
196 *r_supports = Vector3(0, 0, length);
197 } else {
198 r_amount = 1;
199 *r_supports = Vector3(0, 0, 0);
200 }
201 }
202
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const203 bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
204
205 return false; //simply not possible
206 }
207
intersect_point(const Vector3 & p_point) const208 bool RayShapeSW::intersect_point(const Vector3 &p_point) const {
209
210 return false; //simply not possible
211 }
212
get_closest_point_to(const Vector3 & p_point) const213 Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const {
214
215 Vector3 s[2] = {
216 Vector3(0, 0, 0),
217 Vector3(0, 0, length)
218 };
219
220 return Geometry::get_closest_point_to_segment(p_point, s);
221 }
222
get_moment_of_inertia(real_t p_mass) const223 Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const {
224
225 return Vector3();
226 }
227
_setup(real_t p_length,bool p_slips_on_slope)228 void RayShapeSW::_setup(real_t p_length, bool p_slips_on_slope) {
229
230 length = p_length;
231 slips_on_slope = p_slips_on_slope;
232 configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
233 }
234
set_data(const Variant & p_data)235 void RayShapeSW::set_data(const Variant &p_data) {
236
237 Dictionary d = p_data;
238 _setup(d["length"], d["slips_on_slope"]);
239 }
240
get_data() const241 Variant RayShapeSW::get_data() const {
242
243 Dictionary d;
244 d["length"] = length;
245 d["slips_on_slope"] = slips_on_slope;
246 return d;
247 }
248
RayShapeSW()249 RayShapeSW::RayShapeSW() {
250
251 length = 1;
252 slips_on_slope = false;
253 }
254
255 /********** SPHERE *************/
256
get_radius() const257 real_t SphereShapeSW::get_radius() const {
258
259 return radius;
260 }
261
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const262 void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
263
264 real_t d = p_normal.dot(p_transform.origin);
265
266 // figure out scale at point
267 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
268 real_t scale = local_normal.length();
269
270 r_min = d - (radius)*scale;
271 r_max = d + (radius)*scale;
272 }
273
get_support(const Vector3 & p_normal) const274 Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const {
275
276 return p_normal * radius;
277 }
278
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const279 void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
280
281 *r_supports = p_normal * radius;
282 r_amount = 1;
283 }
284
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const285 bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
286
287 return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
288 }
289
intersect_point(const Vector3 & p_point) const290 bool SphereShapeSW::intersect_point(const Vector3 &p_point) const {
291
292 return p_point.length() < radius;
293 }
294
get_closest_point_to(const Vector3 & p_point) const295 Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const {
296
297 Vector3 p = p_point;
298 float l = p.length();
299 if (l < radius)
300 return p_point;
301 return (p / l) * radius;
302 }
303
get_moment_of_inertia(real_t p_mass) const304 Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const {
305
306 real_t s = 0.4 * p_mass * radius * radius;
307 return Vector3(s, s, s);
308 }
309
_setup(real_t p_radius)310 void SphereShapeSW::_setup(real_t p_radius) {
311
312 radius = p_radius;
313 configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
314 }
315
set_data(const Variant & p_data)316 void SphereShapeSW::set_data(const Variant &p_data) {
317
318 _setup(p_data);
319 }
320
get_data() const321 Variant SphereShapeSW::get_data() const {
322
323 return radius;
324 }
325
SphereShapeSW()326 SphereShapeSW::SphereShapeSW() {
327
328 radius = 0;
329 }
330
331 /********** BOX *************/
332
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const333 void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
334
335 // no matter the angle, the box is mirrored anyway
336 Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
337
338 real_t length = local_normal.abs().dot(half_extents);
339 real_t distance = p_normal.dot(p_transform.origin);
340
341 r_min = distance - length;
342 r_max = distance + length;
343 }
344
get_support(const Vector3 & p_normal) const345 Vector3 BoxShapeSW::get_support(const Vector3 &p_normal) const {
346
347 Vector3 point(
348 (p_normal.x < 0) ? -half_extents.x : half_extents.x,
349 (p_normal.y < 0) ? -half_extents.y : half_extents.y,
350 (p_normal.z < 0) ? -half_extents.z : half_extents.z);
351
352 return point;
353 }
354
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const355 void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
356
357 static const int next[3] = { 1, 2, 0 };
358 static const int next2[3] = { 2, 0, 1 };
359
360 for (int i = 0; i < 3; i++) {
361
362 Vector3 axis;
363 axis[i] = 1.0;
364 real_t dot = p_normal.dot(axis);
365 if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
366
367 //Vector3 axis_b;
368
369 bool neg = dot < 0;
370 r_amount = 4;
371
372 Vector3 point;
373 point[i] = half_extents[i];
374
375 int i_n = next[i];
376 int i_n2 = next2[i];
377
378 static const real_t sign[4][2] = {
379
380 { -1.0, 1.0 },
381 { 1.0, 1.0 },
382 { 1.0, -1.0 },
383 { -1.0, -1.0 },
384 };
385
386 for (int j = 0; j < 4; j++) {
387
388 point[i_n] = sign[j][0] * half_extents[i_n];
389 point[i_n2] = sign[j][1] * half_extents[i_n2];
390 r_supports[j] = neg ? -point : point;
391 }
392
393 if (neg) {
394 SWAP(r_supports[1], r_supports[2]);
395 SWAP(r_supports[0], r_supports[3]);
396 }
397
398 return;
399 }
400
401 r_amount = 0;
402 }
403
404 for (int i = 0; i < 3; i++) {
405
406 Vector3 axis;
407 axis[i] = 1.0;
408
409 if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
410
411 r_amount = 2;
412
413 int i_n = next[i];
414 int i_n2 = next2[i];
415
416 Vector3 point = half_extents;
417
418 if (p_normal[i_n] < 0) {
419 point[i_n] = -point[i_n];
420 }
421 if (p_normal[i_n2] < 0) {
422 point[i_n2] = -point[i_n2];
423 }
424
425 r_supports[0] = point;
426 point[i] = -point[i];
427 r_supports[1] = point;
428 return;
429 }
430 }
431 /* USE POINT */
432
433 Vector3 point(
434 (p_normal.x < 0) ? -half_extents.x : half_extents.x,
435 (p_normal.y < 0) ? -half_extents.y : half_extents.y,
436 (p_normal.z < 0) ? -half_extents.z : half_extents.z);
437
438 r_amount = 1;
439 r_supports[0] = point;
440 }
441
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const442 bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
443
444 AABB aabb(-half_extents, half_extents * 2.0);
445
446 return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
447 }
448
intersect_point(const Vector3 & p_point) const449 bool BoxShapeSW::intersect_point(const Vector3 &p_point) const {
450
451 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);
452 }
453
get_closest_point_to(const Vector3 & p_point) const454 Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const {
455
456 int outside = 0;
457 Vector3 min_point;
458
459 for (int i = 0; i < 3; i++) {
460
461 if (Math::abs(p_point[i]) > half_extents[i]) {
462 outside++;
463 if (outside == 1) {
464 //use plane if only one side matches
465 Vector3 n;
466 n[i] = SGN(p_point[i]);
467
468 Plane p(n, half_extents[i]);
469 min_point = p.project(p_point);
470 }
471 }
472 }
473
474 if (!outside)
475 return p_point; //it's inside, don't do anything else
476
477 if (outside == 1) //if only above one plane, this plane clearly wins
478 return min_point;
479
480 //check segments
481 float min_distance = 1e20;
482 Vector3 closest_vertex = half_extents * p_point.sign();
483 Vector3 s[2] = {
484 closest_vertex,
485 closest_vertex
486 };
487
488 for (int i = 0; i < 3; i++) {
489
490 s[1] = closest_vertex;
491 s[1][i] = -s[1][i]; //edge
492
493 Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s);
494
495 float d = p_point.distance_to(closest_edge);
496 if (d < min_distance) {
497 min_point = closest_edge;
498 min_distance = d;
499 }
500 }
501
502 return min_point;
503 }
504
get_moment_of_inertia(real_t p_mass) const505 Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const {
506
507 real_t lx = half_extents.x;
508 real_t ly = half_extents.y;
509 real_t lz = half_extents.z;
510
511 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));
512 }
513
_setup(const Vector3 & p_half_extents)514 void BoxShapeSW::_setup(const Vector3 &p_half_extents) {
515
516 half_extents = p_half_extents.abs();
517
518 configure(AABB(-half_extents, half_extents * 2));
519 }
520
set_data(const Variant & p_data)521 void BoxShapeSW::set_data(const Variant &p_data) {
522
523 _setup(p_data);
524 }
525
get_data() const526 Variant BoxShapeSW::get_data() const {
527
528 return half_extents;
529 }
530
BoxShapeSW()531 BoxShapeSW::BoxShapeSW() {
532 }
533
534 /********** CAPSULE *************/
535
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const536 void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
537
538 Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
539 real_t h = (n.z > 0) ? height : -height;
540
541 n *= radius;
542 n.z += h * 0.5;
543
544 r_max = p_normal.dot(p_transform.xform(n));
545 r_min = p_normal.dot(p_transform.xform(-n));
546 }
547
get_support(const Vector3 & p_normal) const548 Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const {
549
550 Vector3 n = p_normal;
551
552 real_t h = (n.z > 0) ? height : -height;
553
554 n *= radius;
555 n.z += h * 0.5;
556 return n;
557 }
558
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const559 void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
560
561 Vector3 n = p_normal;
562
563 real_t d = n.z;
564
565 if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
566
567 // make it flat
568 n.z = 0.0;
569 n.normalize();
570 n *= radius;
571
572 r_amount = 2;
573 r_supports[0] = n;
574 r_supports[0].z += height * 0.5;
575 r_supports[1] = n;
576 r_supports[1].z -= height * 0.5;
577
578 } else {
579
580 real_t h = (d > 0) ? height : -height;
581
582 n *= radius;
583 n.z += h * 0.5;
584 r_amount = 1;
585 *r_supports = n;
586 }
587 }
588
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const589 bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
590
591 Vector3 norm = (p_end - p_begin).normalized();
592 real_t min_d = 1e20;
593
594 Vector3 res, n;
595 bool collision = false;
596
597 Vector3 auxres, auxn;
598 bool collided;
599
600 // test against cylinder and spheres :-|
601
602 collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn);
603
604 if (collided) {
605 real_t d = norm.dot(auxres);
606 if (d < min_d) {
607 min_d = d;
608 res = auxres;
609 n = auxn;
610 collision = true;
611 }
612 }
613
614 collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn);
615
616 if (collided) {
617 real_t d = norm.dot(auxres);
618 if (d < min_d) {
619 min_d = d;
620 res = auxres;
621 n = auxn;
622 collision = true;
623 }
624 }
625
626 collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn);
627
628 if (collided) {
629 real_t d = norm.dot(auxres);
630
631 if (d < min_d) {
632 min_d = d;
633 res = auxres;
634 n = auxn;
635 collision = true;
636 }
637 }
638
639 if (collision) {
640
641 r_result = res;
642 r_normal = n;
643 }
644 return collision;
645 }
646
intersect_point(const Vector3 & p_point) const647 bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const {
648
649 if (Math::abs(p_point.z) < height * 0.5) {
650 return Vector3(p_point.x, p_point.y, 0).length() < radius;
651 } else {
652 Vector3 p = p_point;
653 p.z = Math::abs(p.z) - height * 0.5;
654 return p.length() < radius;
655 }
656 }
657
get_closest_point_to(const Vector3 & p_point) const658 Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const {
659
660 Vector3 s[2] = {
661 Vector3(0, 0, -height * 0.5),
662 Vector3(0, 0, height * 0.5),
663 };
664
665 Vector3 p = Geometry::get_closest_point_to_segment(p_point, s);
666
667 if (p.distance_to(p_point) < radius)
668 return p_point;
669
670 return p + (p_point - p).normalized() * radius;
671 }
672
get_moment_of_inertia(real_t p_mass) const673 Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const {
674
675 // use bad AABB approximation
676 Vector3 extents = get_aabb().size * 0.5;
677
678 return Vector3(
679 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
680 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
681 (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
682 }
683
_setup(real_t p_height,real_t p_radius)684 void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) {
685
686 height = p_height;
687 radius = p_radius;
688 configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0)));
689 }
690
set_data(const Variant & p_data)691 void CapsuleShapeSW::set_data(const Variant &p_data) {
692
693 Dictionary d = p_data;
694 ERR_FAIL_COND(!d.has("radius"));
695 ERR_FAIL_COND(!d.has("height"));
696 _setup(d["height"], d["radius"]);
697 }
698
get_data() const699 Variant CapsuleShapeSW::get_data() const {
700
701 Dictionary d;
702 d["radius"] = radius;
703 d["height"] = height;
704 return d;
705 }
706
CapsuleShapeSW()707 CapsuleShapeSW::CapsuleShapeSW() {
708
709 height = radius = 0;
710 }
711
712 /********** CONVEX POLYGON *************/
713
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const714 void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
715
716 int vertex_count = mesh.vertices.size();
717 if (vertex_count == 0)
718 return;
719
720 const Vector3 *vrts = &mesh.vertices[0];
721
722 for (int i = 0; i < vertex_count; i++) {
723
724 real_t d = p_normal.dot(p_transform.xform(vrts[i]));
725
726 if (i == 0 || d > r_max)
727 r_max = d;
728 if (i == 0 || d < r_min)
729 r_min = d;
730 }
731 }
732
get_support(const Vector3 & p_normal) const733 Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const {
734
735 Vector3 n = p_normal;
736
737 int vert_support_idx = -1;
738 real_t support_max = 0;
739
740 int vertex_count = mesh.vertices.size();
741 if (vertex_count == 0)
742 return Vector3();
743
744 const Vector3 *vrts = &mesh.vertices[0];
745
746 for (int i = 0; i < vertex_count; i++) {
747
748 real_t d = n.dot(vrts[i]);
749
750 if (i == 0 || d > support_max) {
751 support_max = d;
752 vert_support_idx = i;
753 }
754 }
755
756 return vrts[vert_support_idx];
757 }
758
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const759 void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
760
761 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
762 int fc = mesh.faces.size();
763
764 const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
765 int ec = mesh.edges.size();
766
767 const Vector3 *vertices = mesh.vertices.ptr();
768 int vc = mesh.vertices.size();
769
770 //find vertex first
771 real_t max = 0;
772 int vtx = 0;
773
774 for (int i = 0; i < vc; i++) {
775
776 real_t d = p_normal.dot(vertices[i]);
777
778 if (i == 0 || d > max) {
779 max = d;
780 vtx = i;
781 }
782 }
783
784 for (int i = 0; i < fc; i++) {
785
786 if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
787
788 int ic = faces[i].indices.size();
789 const int *ind = faces[i].indices.ptr();
790
791 bool valid = false;
792 for (int j = 0; j < ic; j++) {
793 if (ind[j] == vtx) {
794 valid = true;
795 break;
796 }
797 }
798
799 if (!valid)
800 continue;
801
802 int m = MIN(p_max, ic);
803 for (int j = 0; j < m; j++) {
804
805 r_supports[j] = vertices[ind[j]];
806 }
807 r_amount = m;
808 return;
809 }
810 }
811
812 for (int i = 0; i < ec; i++) {
813
814 real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
815 dot = ABS(dot);
816 if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) {
817
818 r_amount = 2;
819 r_supports[0] = vertices[edges[i].a];
820 r_supports[1] = vertices[edges[i].b];
821 return;
822 }
823 }
824
825 r_supports[0] = vertices[vtx];
826 r_amount = 1;
827 }
828
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const829 bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
830
831 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
832 int fc = mesh.faces.size();
833
834 const Vector3 *vertices = mesh.vertices.ptr();
835
836 Vector3 n = p_end - p_begin;
837 real_t min = 1e20;
838 bool col = false;
839
840 for (int i = 0; i < fc; i++) {
841
842 if (faces[i].plane.normal.dot(n) > 0)
843 continue; //opposing face
844
845 int ic = faces[i].indices.size();
846 const int *ind = faces[i].indices.ptr();
847
848 for (int j = 1; j < ic - 1; j++) {
849
850 Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]);
851 Vector3 result;
852 if (f.intersects_segment(p_begin, p_end, &result)) {
853 real_t d = n.dot(result);
854 if (d < min) {
855 min = d;
856 r_result = result;
857 r_normal = faces[i].plane.normal;
858 col = true;
859 }
860
861 break;
862 }
863 }
864 }
865
866 return col;
867 }
868
intersect_point(const Vector3 & p_point) const869 bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const {
870
871 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
872 int fc = mesh.faces.size();
873
874 for (int i = 0; i < fc; i++) {
875
876 if (faces[i].plane.distance_to(p_point) >= 0)
877 return false;
878 }
879
880 return true;
881 }
882
get_closest_point_to(const Vector3 & p_point) const883 Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
884
885 const Geometry::MeshData::Face *faces = mesh.faces.ptr();
886 int fc = mesh.faces.size();
887 const Vector3 *vertices = mesh.vertices.ptr();
888
889 bool all_inside = true;
890 for (int i = 0; i < fc; i++) {
891
892 if (!faces[i].plane.is_point_over(p_point))
893 continue;
894
895 all_inside = false;
896 bool is_inside = true;
897 int ic = faces[i].indices.size();
898 const int *indices = faces[i].indices.ptr();
899
900 for (int j = 0; j < ic; j++) {
901
902 Vector3 a = vertices[indices[j]];
903 Vector3 b = vertices[indices[(j + 1) % ic]];
904 Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
905 if (Plane(a, n).is_point_over(p_point)) {
906 is_inside = false;
907 break;
908 }
909 }
910
911 if (is_inside) {
912 return faces[i].plane.project(p_point);
913 }
914 }
915
916 if (all_inside) {
917 return p_point;
918 }
919
920 float min_distance = 1e20;
921 Vector3 min_point;
922
923 //check edges
924 const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
925 int ec = mesh.edges.size();
926 for (int i = 0; i < ec; i++) {
927
928 Vector3 s[2] = {
929 vertices[edges[i].a],
930 vertices[edges[i].b]
931 };
932
933 Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s);
934 float d = closest.distance_to(p_point);
935 if (d < min_distance) {
936 min_distance = d;
937 min_point = closest;
938 }
939 }
940
941 return min_point;
942 }
943
get_moment_of_inertia(real_t p_mass) const944 Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
945
946 // use bad AABB approximation
947 Vector3 extents = get_aabb().size * 0.5;
948
949 return Vector3(
950 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
951 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
952 (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
953 }
954
_setup(const Vector<Vector3> & p_vertices)955 void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) {
956
957 Error err = QuickHull::build(p_vertices, mesh);
958 if (err != OK)
959 ERR_PRINT("Failed to build QuickHull");
960
961 AABB _aabb;
962
963 for (int i = 0; i < mesh.vertices.size(); i++) {
964
965 if (i == 0)
966 _aabb.position = mesh.vertices[i];
967 else
968 _aabb.expand_to(mesh.vertices[i]);
969 }
970
971 configure(_aabb);
972 }
973
set_data(const Variant & p_data)974 void ConvexPolygonShapeSW::set_data(const Variant &p_data) {
975
976 _setup(p_data);
977 }
978
get_data() const979 Variant ConvexPolygonShapeSW::get_data() const {
980
981 return mesh.vertices;
982 }
983
ConvexPolygonShapeSW()984 ConvexPolygonShapeSW::ConvexPolygonShapeSW() {
985 }
986
987 /********** FACE POLYGON *************/
988
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const989 void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
990
991 for (int i = 0; i < 3; i++) {
992
993 Vector3 v = p_transform.xform(vertex[i]);
994 real_t d = p_normal.dot(v);
995
996 if (i == 0 || d > r_max)
997 r_max = d;
998
999 if (i == 0 || d < r_min)
1000 r_min = d;
1001 }
1002 }
1003
get_support(const Vector3 & p_normal) const1004 Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const {
1005
1006 int vert_support_idx = -1;
1007 real_t support_max = 0;
1008
1009 for (int i = 0; i < 3; i++) {
1010
1011 real_t d = p_normal.dot(vertex[i]);
1012
1013 if (i == 0 || d > support_max) {
1014 support_max = d;
1015 vert_support_idx = i;
1016 }
1017 }
1018
1019 return vertex[vert_support_idx];
1020 }
1021
get_supports(const Vector3 & p_normal,int p_max,Vector3 * r_supports,int & r_amount) const1022 void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
1023
1024 Vector3 n = p_normal;
1025
1026 /** TEST FACE AS SUPPORT **/
1027 if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
1028
1029 r_amount = 3;
1030 for (int i = 0; i < 3; i++) {
1031
1032 r_supports[i] = vertex[i];
1033 }
1034 return;
1035 }
1036
1037 /** FIND SUPPORT VERTEX **/
1038
1039 int vert_support_idx = -1;
1040 real_t support_max = 0;
1041
1042 for (int i = 0; i < 3; i++) {
1043
1044 real_t d = n.dot(vertex[i]);
1045
1046 if (i == 0 || d > support_max) {
1047 support_max = d;
1048 vert_support_idx = i;
1049 }
1050 }
1051
1052 /** TEST EDGES AS SUPPORT **/
1053
1054 for (int i = 0; i < 3; i++) {
1055
1056 int nx = (i + 1) % 3;
1057 if (i != vert_support_idx && nx != vert_support_idx)
1058 continue;
1059
1060 // check if edge is valid as a support
1061 real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n);
1062 dot = ABS(dot);
1063 if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
1064
1065 r_amount = 2;
1066 r_supports[0] = vertex[i];
1067 r_supports[1] = vertex[nx];
1068 return;
1069 }
1070 }
1071
1072 r_amount = 1;
1073 r_supports[0] = vertex[vert_support_idx];
1074 }
1075
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const1076 bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
1077
1078 bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
1079 if (c) {
1080 r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
1081 if (r_normal.dot(p_end - p_begin) > 0) {
1082 r_normal = -r_normal;
1083 }
1084 }
1085
1086 return c;
1087 }
1088
intersect_point(const Vector3 & p_point) const1089 bool FaceShapeSW::intersect_point(const Vector3 &p_point) const {
1090
1091 return false; //face is flat
1092 }
1093
get_closest_point_to(const Vector3 & p_point) const1094 Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const {
1095
1096 return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
1097 }
1098
get_moment_of_inertia(real_t p_mass) const1099 Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const {
1100
1101 return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
1102 }
1103
FaceShapeSW()1104 FaceShapeSW::FaceShapeSW() {
1105
1106 configure(AABB());
1107 }
1108
get_faces() const1109 PoolVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
1110
1111 PoolVector<Vector3> rfaces;
1112 rfaces.resize(faces.size() * 3);
1113
1114 for (int i = 0; i < faces.size(); i++) {
1115
1116 Face f = faces.get(i);
1117
1118 for (int j = 0; j < 3; j++) {
1119
1120 rfaces.set(i * 3 + j, vertices.get(f.indices[j]));
1121 }
1122 }
1123
1124 return rfaces;
1125 }
1126
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const1127 void ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
1128
1129 int count = vertices.size();
1130 if (count == 0) {
1131 r_min = 0;
1132 r_max = 0;
1133 return;
1134 }
1135 PoolVector<Vector3>::Read r = vertices.read();
1136 const Vector3 *vptr = r.ptr();
1137
1138 for (int i = 0; i < count; i++) {
1139
1140 real_t d = p_normal.dot(p_transform.xform(vptr[i]));
1141
1142 if (i == 0 || d > r_max)
1143 r_max = d;
1144 if (i == 0 || d < r_min)
1145 r_min = d;
1146 }
1147 }
1148
get_support(const Vector3 & p_normal) const1149 Vector3 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const {
1150
1151 int count = vertices.size();
1152 if (count == 0)
1153 return Vector3();
1154
1155 PoolVector<Vector3>::Read r = vertices.read();
1156 const Vector3 *vptr = r.ptr();
1157
1158 Vector3 n = p_normal;
1159
1160 int vert_support_idx = -1;
1161 real_t support_max = 0;
1162
1163 for (int i = 0; i < count; i++) {
1164
1165 real_t d = n.dot(vptr[i]);
1166
1167 if (i == 0 || d > support_max) {
1168 support_max = d;
1169 vert_support_idx = i;
1170 }
1171 }
1172
1173 return vptr[vert_support_idx];
1174 }
1175
_cull_segment(int p_idx,_SegmentCullParams * p_params) const1176 void ConcavePolygonShapeSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
1177
1178 const BVH *bvh = &p_params->bvh[p_idx];
1179
1180 /*
1181 if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
1182 return; //test against whole AABB, which isn't very costly
1183 */
1184
1185 //printf("addr: %p\n",bvh);
1186 if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
1187
1188 return;
1189 }
1190
1191 if (bvh->face_index >= 0) {
1192
1193 Vector3 res;
1194 Vector3 vertices[3] = {
1195 p_params->vertices[p_params->faces[bvh->face_index].indices[0]],
1196 p_params->vertices[p_params->faces[bvh->face_index].indices[1]],
1197 p_params->vertices[p_params->faces[bvh->face_index].indices[2]]
1198 };
1199
1200 if (Geometry::segment_intersects_triangle(
1201 p_params->from,
1202 p_params->to,
1203 vertices[0],
1204 vertices[1],
1205 vertices[2],
1206 &res)) {
1207
1208 real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
1209 //TODO, seems segmen/triangle intersection is broken :(
1210 if (d > 0 && d < p_params->min_d) {
1211
1212 p_params->min_d = d;
1213 p_params->result = res;
1214 p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal;
1215 p_params->collisions++;
1216 }
1217 }
1218
1219 } else {
1220
1221 if (bvh->left >= 0)
1222 _cull_segment(bvh->left, p_params);
1223 if (bvh->right >= 0)
1224 _cull_segment(bvh->right, p_params);
1225 }
1226 }
1227
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_result,Vector3 & r_normal) const1228 bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
1229
1230 if (faces.size() == 0)
1231 return false;
1232
1233 // unlock data
1234 PoolVector<Face>::Read fr = faces.read();
1235 PoolVector<Vector3>::Read vr = vertices.read();
1236 PoolVector<BVH>::Read br = bvh.read();
1237
1238 _SegmentCullParams params;
1239 params.from = p_begin;
1240 params.to = p_end;
1241 params.collisions = 0;
1242 params.dir = (p_end - p_begin).normalized();
1243
1244 params.faces = fr.ptr();
1245 params.vertices = vr.ptr();
1246 params.bvh = br.ptr();
1247
1248 params.min_d = 1e20;
1249 // cull
1250 _cull_segment(0, ¶ms);
1251
1252 if (params.collisions > 0) {
1253
1254 r_result = params.result;
1255 r_normal = params.normal;
1256 return true;
1257 } else {
1258
1259 return false;
1260 }
1261 }
1262
intersect_point(const Vector3 & p_point) const1263 bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const {
1264
1265 return false; //face is flat
1266 }
1267
get_closest_point_to(const Vector3 & p_point) const1268 Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
1269
1270 return Vector3();
1271 }
1272
_cull(int p_idx,_CullParams * p_params) const1273 void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
1274
1275 const BVH *bvh = &p_params->bvh[p_idx];
1276
1277 if (!p_params->aabb.intersects(bvh->aabb))
1278 return;
1279
1280 if (bvh->face_index >= 0) {
1281
1282 const Face *f = &p_params->faces[bvh->face_index];
1283 FaceShapeSW *face = p_params->face;
1284 face->normal = f->normal;
1285 face->vertex[0] = p_params->vertices[f->indices[0]];
1286 face->vertex[1] = p_params->vertices[f->indices[1]];
1287 face->vertex[2] = p_params->vertices[f->indices[2]];
1288 p_params->callback(p_params->userdata, face);
1289
1290 } else {
1291
1292 if (bvh->left >= 0) {
1293
1294 _cull(bvh->left, p_params);
1295 }
1296
1297 if (bvh->right >= 0) {
1298
1299 _cull(bvh->right, p_params);
1300 }
1301 }
1302 }
1303
cull(const AABB & p_local_aabb,Callback p_callback,void * p_userdata) const1304 void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
1305
1306 // make matrix local to concave
1307 if (faces.size() == 0)
1308 return;
1309
1310 AABB local_aabb = p_local_aabb;
1311
1312 // unlock data
1313 PoolVector<Face>::Read fr = faces.read();
1314 PoolVector<Vector3>::Read vr = vertices.read();
1315 PoolVector<BVH>::Read br = bvh.read();
1316
1317 FaceShapeSW face; // use this to send in the callback
1318
1319 _CullParams params;
1320 params.aabb = local_aabb;
1321 params.face = &face;
1322 params.faces = fr.ptr();
1323 params.vertices = vr.ptr();
1324 params.bvh = br.ptr();
1325 params.callback = p_callback;
1326 params.userdata = p_userdata;
1327
1328 // cull
1329 _cull(0, ¶ms);
1330 }
1331
get_moment_of_inertia(real_t p_mass) const1332 Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
1333
1334 // use bad AABB approximation
1335 Vector3 extents = get_aabb().size * 0.5;
1336
1337 return Vector3(
1338 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
1339 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
1340 (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
1341 }
1342
1343 struct _VolumeSW_BVH_Element {
1344
1345 AABB aabb;
1346 Vector3 center;
1347 int face_index;
1348 };
1349
1350 struct _VolumeSW_BVH_CompareX {
1351
operator ()_VolumeSW_BVH_CompareX1352 _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
1353
1354 return a.center.x < b.center.x;
1355 }
1356 };
1357
1358 struct _VolumeSW_BVH_CompareY {
1359
operator ()_VolumeSW_BVH_CompareY1360 _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
1361
1362 return a.center.y < b.center.y;
1363 }
1364 };
1365
1366 struct _VolumeSW_BVH_CompareZ {
1367
operator ()_VolumeSW_BVH_CompareZ1368 _FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
1369
1370 return a.center.z < b.center.z;
1371 }
1372 };
1373
1374 struct _VolumeSW_BVH {
1375
1376 AABB aabb;
1377 _VolumeSW_BVH *left;
1378 _VolumeSW_BVH *right;
1379
1380 int face_index;
1381 };
1382
_volume_sw_build_bvh(_VolumeSW_BVH_Element * p_elements,int p_size,int & count)1383 _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
1384
1385 _VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
1386
1387 if (p_size == 1) {
1388 //leaf
1389 bvh->aabb = p_elements[0].aabb;
1390 bvh->left = NULL;
1391 bvh->right = NULL;
1392 bvh->face_index = p_elements->face_index;
1393 count++;
1394 return bvh;
1395 } else {
1396
1397 bvh->face_index = -1;
1398 }
1399
1400 AABB aabb;
1401 for (int i = 0; i < p_size; i++) {
1402
1403 if (i == 0)
1404 aabb = p_elements[i].aabb;
1405 else
1406 aabb.merge_with(p_elements[i].aabb);
1407 }
1408 bvh->aabb = aabb;
1409 switch (aabb.get_longest_axis_index()) {
1410
1411 case 0: {
1412
1413 SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
1414 sort_x.sort(p_elements, p_size);
1415
1416 } break;
1417 case 1: {
1418
1419 SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
1420 sort_y.sort(p_elements, p_size);
1421 } break;
1422 case 2: {
1423
1424 SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
1425 sort_z.sort(p_elements, p_size);
1426 } break;
1427 }
1428
1429 int split = p_size / 2;
1430 bvh->left = _volume_sw_build_bvh(p_elements, split, count);
1431 bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
1432
1433 //printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
1434 count++;
1435 return bvh;
1436 }
1437
_fill_bvh(_VolumeSW_BVH * p_bvh_tree,BVH * p_bvh_array,int & p_idx)1438 void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
1439
1440 int idx = p_idx;
1441
1442 p_bvh_array[idx].aabb = p_bvh_tree->aabb;
1443 p_bvh_array[idx].face_index = p_bvh_tree->face_index;
1444 //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);
1445
1446 if (p_bvh_tree->left) {
1447 p_bvh_array[idx].left = ++p_idx;
1448 _fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx);
1449
1450 } else {
1451
1452 p_bvh_array[p_idx].left = -1;
1453 }
1454
1455 if (p_bvh_tree->right) {
1456 p_bvh_array[idx].right = ++p_idx;
1457 _fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx);
1458
1459 } else {
1460
1461 p_bvh_array[p_idx].right = -1;
1462 }
1463
1464 memdelete(p_bvh_tree);
1465 }
1466
_setup(PoolVector<Vector3> p_faces)1467 void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) {
1468
1469 int src_face_count = p_faces.size();
1470 if (src_face_count == 0) {
1471 configure(AABB());
1472 return;
1473 }
1474 ERR_FAIL_COND(src_face_count % 3);
1475 src_face_count /= 3;
1476
1477 PoolVector<Vector3>::Read r = p_faces.read();
1478 const Vector3 *facesr = r.ptr();
1479
1480 PoolVector<_VolumeSW_BVH_Element> bvh_array;
1481 bvh_array.resize(src_face_count);
1482
1483 PoolVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
1484 _VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr();
1485
1486 faces.resize(src_face_count);
1487 PoolVector<Face>::Write w = faces.write();
1488 Face *facesw = w.ptr();
1489
1490 vertices.resize(src_face_count * 3);
1491
1492 PoolVector<Vector3>::Write vw = vertices.write();
1493 Vector3 *verticesw = vw.ptr();
1494
1495 AABB _aabb;
1496
1497 for (int i = 0; i < src_face_count; i++) {
1498
1499 Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
1500
1501 bvh_arrayw[i].aabb = face.get_aabb();
1502 bvh_arrayw[i].center = bvh_arrayw[i].aabb.position + bvh_arrayw[i].aabb.size * 0.5;
1503 bvh_arrayw[i].face_index = i;
1504 facesw[i].indices[0] = i * 3 + 0;
1505 facesw[i].indices[1] = i * 3 + 1;
1506 facesw[i].indices[2] = i * 3 + 2;
1507 facesw[i].normal = face.get_plane().normal;
1508 verticesw[i * 3 + 0] = face.vertex[0];
1509 verticesw[i * 3 + 1] = face.vertex[1];
1510 verticesw[i * 3 + 2] = face.vertex[2];
1511 if (i == 0)
1512 _aabb = bvh_arrayw[i].aabb;
1513 else
1514 _aabb.merge_with(bvh_arrayw[i].aabb);
1515 }
1516
1517 w.release();
1518 vw.release();
1519
1520 int count = 0;
1521 _VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
1522
1523 bvh.resize(count + 1);
1524
1525 PoolVector<BVH>::Write bvhw2 = bvh.write();
1526 BVH *bvh_arrayw2 = bvhw2.ptr();
1527
1528 int idx = 0;
1529 _fill_bvh(bvh_tree, bvh_arrayw2, idx);
1530
1531 configure(_aabb); // this type of shape has no margin
1532 }
1533
set_data(const Variant & p_data)1534 void ConcavePolygonShapeSW::set_data(const Variant &p_data) {
1535
1536 _setup(p_data);
1537 }
1538
get_data() const1539 Variant ConcavePolygonShapeSW::get_data() const {
1540
1541 return get_faces();
1542 }
1543
ConcavePolygonShapeSW()1544 ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
1545 }
1546
1547 /* HEIGHT MAP SHAPE */
1548
get_heights() const1549 PoolVector<real_t> HeightMapShapeSW::get_heights() const {
1550
1551 return heights;
1552 }
get_width() const1553 int HeightMapShapeSW::get_width() const {
1554
1555 return width;
1556 }
get_depth() const1557 int HeightMapShapeSW::get_depth() const {
1558
1559 return depth;
1560 }
get_cell_size() const1561 real_t HeightMapShapeSW::get_cell_size() const {
1562
1563 return cell_size;
1564 }
1565
project_range(const Vector3 & p_normal,const Transform & p_transform,real_t & r_min,real_t & r_max) const1566 void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
1567
1568 //not very useful, but not very used either
1569 p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
1570 }
1571
get_support(const Vector3 & p_normal) const1572 Vector3 HeightMapShapeSW::get_support(const Vector3 &p_normal) const {
1573
1574 //not very useful, but not very used either
1575 return get_aabb().get_support(p_normal);
1576 }
1577
intersect_segment(const Vector3 & p_begin,const Vector3 & p_end,Vector3 & r_point,Vector3 & r_normal) const1578 bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
1579
1580 return false;
1581 }
1582
intersect_point(const Vector3 & p_point) const1583 bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const {
1584 return false;
1585 }
1586
get_closest_point_to(const Vector3 & p_point) const1587 Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const {
1588
1589 return Vector3();
1590 }
1591
cull(const AABB & p_local_aabb,Callback p_callback,void * p_userdata) const1592 void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
1593 }
1594
get_moment_of_inertia(real_t p_mass) const1595 Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const {
1596
1597 // use bad AABB approximation
1598 Vector3 extents = get_aabb().size * 0.5;
1599
1600 return Vector3(
1601 (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
1602 (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
1603 (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
1604 }
1605
_setup(PoolVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size)1606 void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {
1607
1608 heights = p_heights;
1609 width = p_width;
1610 depth = p_depth;
1611 cell_size = p_cell_size;
1612
1613 PoolVector<real_t>::Read r = heights.read();
1614
1615 AABB aabb;
1616
1617 for (int i = 0; i < depth; i++) {
1618
1619 for (int j = 0; j < width; j++) {
1620
1621 real_t h = r[i * width + j];
1622
1623 Vector3 pos(j * cell_size, h, i * cell_size);
1624 if (i == 0 || j == 0)
1625 aabb.position = pos;
1626 else
1627 aabb.expand_to(pos);
1628 }
1629 }
1630
1631 configure(aabb);
1632 }
1633
set_data(const Variant & p_data)1634 void HeightMapShapeSW::set_data(const Variant &p_data) {
1635
1636 ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
1637 Dictionary d = p_data;
1638 ERR_FAIL_COND(!d.has("width"));
1639 ERR_FAIL_COND(!d.has("depth"));
1640 ERR_FAIL_COND(!d.has("cell_size"));
1641 ERR_FAIL_COND(!d.has("heights"));
1642
1643 int width = d["width"];
1644 int depth = d["depth"];
1645 real_t cell_size = d["cell_size"];
1646 PoolVector<real_t> heights = d["heights"];
1647
1648 ERR_FAIL_COND(width <= 0);
1649 ERR_FAIL_COND(depth <= 0);
1650 ERR_FAIL_COND(cell_size <= CMP_EPSILON);
1651 ERR_FAIL_COND(heights.size() != (width * depth));
1652 _setup(heights, width, depth, cell_size);
1653 }
1654
get_data() const1655 Variant HeightMapShapeSW::get_data() const {
1656
1657 ERR_FAIL_V(Variant());
1658 }
1659
HeightMapShapeSW()1660 HeightMapShapeSW::HeightMapShapeSW() {
1661
1662 width = 0;
1663 depth = 0;
1664 cell_size = 0;
1665 }
1666