1/**************************************************************************/
2/* godot_shape_2d.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_2d.h"
32
33#include "core/math/geometry_2d.h"
34#include "core/templates/sort_array.h"
35
36void GodotShape2D::configure(const Rect2 &p_aabb) {
37 aabb = p_aabb;
38 configured = true;
39 for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) {
40 GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key);
41 co->_shape_changed();
42 }
43}
44
45Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const {
46 Vector2 res[2];
47 int amnt;
48 get_supports(p_normal, res, amnt);
49 return res[0];
50}
51
52void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) {
53 HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
54 if (E) {
55 E->value++;
56 } else {
57 owners[p_owner] = 1;
58 }
59}
60
61void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) {
62 HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner);
63 ERR_FAIL_COND(!E);
64 E->value--;
65 if (E->value == 0) {
66 owners.remove(E);
67 }
68}
69
70bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const {
71 return owners.has(p_owner);
72}
73
74const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
75 return owners;
76}
77
78GodotShape2D::~GodotShape2D() {
79 ERR_FAIL_COND(owners.size());
80}
81
82/*********************************************************/
83/*********************************************************/
84/*********************************************************/
85
86void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
87 r_amount = 0;
88}
89
90bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const {
91 return normal.dot(p_point) < d;
92}
93
94bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
95 Vector2 segment = p_begin - p_end;
96 real_t den = normal.dot(segment);
97
98 //printf("den is %i\n",den);
99 if (Math::abs(den) <= CMP_EPSILON) {
100 return false;
101 }
102
103 real_t dist = (normal.dot(p_begin) - d) / den;
104 //printf("dist is %i\n",dist);
105
106 if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
107 return false;
108 }
109
110 r_point = p_begin + segment * -dist;
111 r_normal = normal;
112
113 return true;
114}
115
116real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
117 return 0;
118}
119
120void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) {
121 ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
122 Array arr = p_data;
123 ERR_FAIL_COND(arr.size() != 2);
124 normal = arr[0];
125 d = arr[1];
126 configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2)));
127}
128
129Variant GodotWorldBoundaryShape2D::get_data() const {
130 Array arr;
131 arr.resize(2);
132 arr[0] = normal;
133 arr[1] = d;
134 return arr;
135}
136
137/*********************************************************/
138/*********************************************************/
139/*********************************************************/
140
141void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
142 r_amount = 1;
143
144 if (p_normal.y > 0) {
145 *r_supports = Vector2(0, length);
146 } else {
147 *r_supports = Vector2();
148 }
149}
150
151bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const {
152 return false;
153}
154
155bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
156 return false; //rays can't be intersected
157}
158
159real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
160 return 0; //rays are mass-less
161}
162
163void GodotSeparationRayShape2D::set_data(const Variant &p_data) {
164 Dictionary d = p_data;
165 length = d["length"];
166 slide_on_slope = d["slide_on_slope"];
167 configure(Rect2(0, 0, 0.001, length));
168}
169
170Variant GodotSeparationRayShape2D::get_data() const {
171 Dictionary d;
172 d["length"] = length;
173 d["slide_on_slope"] = slide_on_slope;
174 return d;
175}
176
177/*********************************************************/
178/*********************************************************/
179/*********************************************************/
180
181void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
182 if (Math::abs(p_normal.dot(n)) > segment_is_valid_support_threshold) {
183 r_supports[0] = a;
184 r_supports[1] = b;
185 r_amount = 2;
186 return;
187 }
188
189 real_t dp = p_normal.dot(b - a);
190 if (dp > 0) {
191 *r_supports = b;
192 } else {
193 *r_supports = a;
194 }
195 r_amount = 1;
196}
197
198bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const {
199 return false;
200}
201
202bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
203 if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) {
204 return false;
205 }
206
207 if (n.dot(p_begin) > n.dot(a)) {
208 r_normal = n;
209 } else {
210 r_normal = -n;
211 }
212
213 return true;
214}
215
216real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
217 return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
218}
219
220void GodotSegmentShape2D::set_data(const Variant &p_data) {
221 ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
222
223 Rect2 r = p_data;
224 a = r.position;
225 b = r.size;
226 n = (b - a).orthogonal();
227
228 Rect2 aabb_new;
229 aabb_new.position = a;
230 aabb_new.expand_to(b);
231 if (aabb_new.size.x == 0) {
232 aabb_new.size.x = 0.001;
233 }
234 if (aabb_new.size.y == 0) {
235 aabb_new.size.y = 0.001;
236 }
237 configure(aabb_new);
238}
239
240Variant GodotSegmentShape2D::get_data() const {
241 Rect2 r;
242 r.position = a;
243 r.size = b;
244 return r;
245}
246
247/*********************************************************/
248/*********************************************************/
249/*********************************************************/
250
251void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
252 r_amount = 1;
253 *r_supports = p_normal * radius;
254}
255
256bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const {
257 return p_point.length_squared() < radius * radius;
258}
259
260bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
261 Vector2 line_vec = p_end - p_begin;
262
263 real_t a, b, c;
264
265 a = line_vec.dot(line_vec);
266 b = 2 * p_begin.dot(line_vec);
267 c = p_begin.dot(p_begin) - radius * radius;
268
269 real_t sqrtterm = b * b - 4 * a * c;
270
271 if (sqrtterm < 0) {
272 return false;
273 }
274 sqrtterm = Math::sqrt(sqrtterm);
275 real_t res = (-b - sqrtterm) / (2 * a);
276
277 if (res < 0 || res > 1 + CMP_EPSILON) {
278 return false;
279 }
280
281 r_point = p_begin + line_vec * res;
282 r_normal = r_point.normalized();
283 return true;
284}
285
286real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
287 real_t a = radius * p_scale.x;
288 real_t b = radius * p_scale.y;
289 return p_mass * (a * a + b * b) / 4;
290}
291
292void GodotCircleShape2D::set_data(const Variant &p_data) {
293 ERR_FAIL_COND(!p_data.is_num());
294 radius = p_data;
295 configure(Rect2(-radius, -radius, radius * 2, radius * 2));
296}
297
298Variant GodotCircleShape2D::get_data() const {
299 return radius;
300}
301
302/*********************************************************/
303/*********************************************************/
304/*********************************************************/
305
306void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
307 for (int i = 0; i < 2; i++) {
308 Vector2 ag;
309 ag[i] = 1.0;
310 real_t dp = ag.dot(p_normal);
311 if (Math::abs(dp) <= segment_is_valid_support_threshold) {
312 continue;
313 }
314
315 real_t sgn = dp > 0 ? 1.0 : -1.0;
316
317 r_amount = 2;
318
319 r_supports[0][i] = half_extents[i] * sgn;
320 r_supports[0][i ^ 1] = half_extents[i ^ 1];
321
322 r_supports[1][i] = half_extents[i] * sgn;
323 r_supports[1][i ^ 1] = -half_extents[i ^ 1];
324
325 return;
326 }
327
328 /* USE POINT */
329
330 r_amount = 1;
331 r_supports[0] = Vector2(
332 (p_normal.x < 0) ? -half_extents.x : half_extents.x,
333 (p_normal.y < 0) ? -half_extents.y : half_extents.y);
334}
335
336bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const {
337 real_t x = p_point.x;
338 real_t y = p_point.y;
339 real_t edge_x = half_extents.x;
340 real_t edge_y = half_extents.y;
341 return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
342}
343
344bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
345 return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
346}
347
348real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
349 Vector2 he2 = half_extents * 2 * p_scale;
350 return p_mass * he2.dot(he2) / 12.0;
351}
352
353void GodotRectangleShape2D::set_data(const Variant &p_data) {
354 ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
355
356 half_extents = p_data;
357 configure(Rect2(-half_extents, half_extents * 2.0));
358}
359
360Variant GodotRectangleShape2D::get_data() const {
361 return half_extents;
362}
363
364/*********************************************************/
365/*********************************************************/
366/*********************************************************/
367
368void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
369 Vector2 n = p_normal;
370
371 real_t h = height * 0.5 - radius; // half-height of the rectangle part
372
373 if (h > 0 && Math::abs(n.x) > segment_is_valid_support_threshold) {
374 // make it flat
375 n.y = 0.0;
376 n.normalize();
377 n *= radius;
378
379 r_amount = 2;
380 r_supports[0] = n;
381 r_supports[0].y += h;
382 r_supports[1] = n;
383 r_supports[1].y -= h;
384 } else {
385 n *= radius;
386 n.y += (n.y > 0) ? h : -h;
387 r_amount = 1;
388 *r_supports = n;
389 }
390}
391
392bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const {
393 Vector2 p = p_point;
394 p.y = Math::abs(p.y);
395 p.y -= height * 0.5 - radius;
396 if (p.y < 0) {
397 p.y = 0;
398 }
399
400 return p.length_squared() < radius * radius;
401}
402
403bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
404 real_t d = 1e10;
405 Vector2 n = (p_end - p_begin).normalized();
406 bool collided = false;
407
408 //try spheres
409 for (int i = 0; i < 2; i++) {
410 Vector2 begin = p_begin;
411 Vector2 end = p_end;
412 real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius;
413 begin.y += ofs;
414 end.y += ofs;
415
416 Vector2 line_vec = end - begin;
417
418 real_t a, b, c;
419
420 a = line_vec.dot(line_vec);
421 b = 2 * begin.dot(line_vec);
422 c = begin.dot(begin) - radius * radius;
423
424 real_t sqrtterm = b * b - 4 * a * c;
425
426 if (sqrtterm < 0) {
427 continue;
428 }
429
430 sqrtterm = Math::sqrt(sqrtterm);
431 real_t res = (-b - sqrtterm) / (2 * a);
432
433 if (res < 0 || res > 1 + CMP_EPSILON) {
434 continue;
435 }
436
437 Vector2 point = begin + line_vec * res;
438 Vector2 pointf(point.x, point.y - ofs);
439 real_t pd = n.dot(pointf);
440 if (pd < d) {
441 r_point = pointf;
442 r_normal = point.normalized();
443 d = pd;
444 collided = true;
445 }
446 }
447
448 Vector2 rpos, rnorm;
449 if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
450 real_t pd = n.dot(rpos);
451 if (pd < d) {
452 r_point = rpos;
453 r_normal = rnorm;
454 d = pd;
455 collided = true;
456 }
457 }
458
459 //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
460 return collided; //todo
461}
462
463real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
464 Vector2 he2 = Vector2(radius * 2, height) * p_scale;
465 return p_mass * he2.dot(he2) / 12.0;
466}
467
468void GodotCapsuleShape2D::set_data(const Variant &p_data) {
469 ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
470
471 if (p_data.get_type() == Variant::ARRAY) {
472 Array arr = p_data;
473 ERR_FAIL_COND(arr.size() != 2);
474 height = arr[0];
475 radius = arr[1];
476 } else {
477 Point2 p = p_data;
478 radius = p.x;
479 height = p.y;
480 }
481
482 Point2 he(radius, height * 0.5);
483 configure(Rect2(-he, he * 2));
484}
485
486Variant GodotCapsuleShape2D::get_data() const {
487 return Point2(height, radius);
488}
489
490/*********************************************************/
491/*********************************************************/
492/*********************************************************/
493
494void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
495 int support_idx = -1;
496 real_t d = -1e10;
497 r_amount = 0;
498
499 for (int i = 0; i < point_count; i++) {
500 //test point
501 real_t ld = p_normal.dot(points[i].pos);
502 if (ld > d) {
503 support_idx = i;
504 d = ld;
505 }
506
507 //test segment
508 if (points[i].normal.dot(p_normal) > segment_is_valid_support_threshold) {
509 r_amount = 2;
510 r_supports[0] = points[i].pos;
511 r_supports[1] = points[(i + 1) % point_count].pos;
512 return;
513 }
514 }
515
516 ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found.");
517
518 r_amount = 1;
519 r_supports[0] = points[support_idx].pos;
520}
521
522bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const {
523 bool out = false;
524 bool in = false;
525
526 for (int i = 0; i < point_count; i++) {
527 real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
528 if (d > 0) {
529 out = true;
530 } else {
531 in = true;
532 }
533 }
534
535 return in != out;
536}
537
538bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
539 Vector2 n = (p_end - p_begin).normalized();
540 real_t d = 1e10;
541 bool inters = false;
542
543 for (int i = 0; i < point_count; i++) {
544 Vector2 res;
545
546 if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) {
547 continue;
548 }
549
550 real_t nd = n.dot(res);
551 if (nd < d) {
552 d = nd;
553 r_point = res;
554 r_normal = points[i].normal;
555 inters = true;
556 }
557 }
558
559 return inters;
560}
561
562real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
563 ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points.");
564 Rect2 aabb_new;
565 aabb_new.position = points[0].pos * p_scale;
566 for (int i = 0; i < point_count; i++) {
567 aabb_new.expand_to(points[i].pos * p_scale);
568 }
569
570 return p_mass * aabb_new.size.dot(aabb_new.size) / 12.0;
571}
572
573void GodotConvexPolygonShape2D::set_data(const Variant &p_data) {
574#ifdef REAL_T_IS_DOUBLE
575 ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
576#else
577 ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
578#endif
579
580 if (points) {
581 memdelete_arr(points);
582 }
583 points = nullptr;
584 point_count = 0;
585
586 if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
587 Vector<Vector2> arr = p_data;
588 ERR_FAIL_COND(arr.size() == 0);
589 point_count = arr.size();
590 points = memnew_arr(Point, point_count);
591 const Vector2 *r = arr.ptr();
592
593 for (int i = 0; i < point_count; i++) {
594 points[i].pos = r[i];
595 }
596
597 for (int i = 0; i < point_count; i++) {
598 Vector2 p = points[i].pos;
599 Vector2 pn = points[(i + 1) % point_count].pos;
600 points[i].normal = (pn - p).orthogonal().normalized();
601 }
602 } else {
603 Vector<real_t> dvr = p_data;
604 point_count = dvr.size() / 4;
605 ERR_FAIL_COND(point_count == 0);
606
607 points = memnew_arr(Point, point_count);
608 const real_t *r = dvr.ptr();
609
610 for (int i = 0; i < point_count; i++) {
611 int idx = i << 2;
612 points[i].pos.x = r[idx + 0];
613 points[i].pos.y = r[idx + 1];
614 points[i].normal.x = r[idx + 2];
615 points[i].normal.y = r[idx + 3];
616 }
617 }
618
619 ERR_FAIL_COND(point_count == 0);
620 Rect2 aabb_new;
621 aabb_new.position = points[0].pos;
622 for (int i = 1; i < point_count; i++) {
623 aabb_new.expand_to(points[i].pos);
624 }
625
626 configure(aabb_new);
627}
628
629Variant GodotConvexPolygonShape2D::get_data() const {
630 Vector<Vector2> dvr;
631
632 dvr.resize(point_count);
633
634 for (int i = 0; i < point_count; i++) {
635 dvr.set(i, points[i].pos);
636 }
637
638 return dvr;
639}
640
641GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() {
642 if (points) {
643 memdelete_arr(points);
644 }
645}
646
647//////////////////////////////////////////////////
648
649void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
650 real_t d = -1e10;
651 int idx = -1;
652 for (int i = 0; i < points.size(); i++) {
653 real_t ld = p_normal.dot(points[i]);
654 if (ld > d) {
655 d = ld;
656 idx = i;
657 }
658 }
659
660 r_amount = 1;
661 ERR_FAIL_COND(idx == -1);
662 *r_supports = points[idx];
663}
664
665bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const {
666 return false; //sorry
667}
668
669bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
670 if (segments.size() == 0 || points.size() == 0) {
671 return false;
672 }
673
674 uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
675
676 enum {
677 TEST_AABB_BIT = 0,
678 VISIT_LEFT_BIT = 1,
679 VISIT_RIGHT_BIT = 2,
680 VISIT_DONE_BIT = 3,
681 VISITED_BIT_SHIFT = 29,
682 NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
683 VISITED_BIT_MASK = ~NODE_IDX_MASK,
684
685 };
686
687 Vector2 n = (p_end - p_begin).normalized();
688 real_t d = 1e10;
689 bool inters = false;
690
691 /*
692 for(int i=0;i<bvh_depth;i++)
693 stack[i]=0;
694 */
695
696 int level = 0;
697
698 const Segment *segmentptr = &segments[0];
699 const Vector2 *pointptr = &points[0];
700 const BVH *bvhptr = &bvh[0];
701
702 stack[0] = 0;
703 while (true) {
704 uint32_t node = stack[level] & NODE_IDX_MASK;
705 const BVH &bvh2 = bvhptr[node];
706 bool done = false;
707
708 switch (stack[level] >> VISITED_BIT_SHIFT) {
709 case TEST_AABB_BIT: {
710 bool valid = bvh2.aabb.intersects_segment(p_begin, p_end);
711 if (!valid) {
712 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
713
714 } else {
715 if (bvh2.left < 0) {
716 const Segment &s = segmentptr[bvh2.right];
717 Vector2 a = pointptr[s.points[0]];
718 Vector2 b = pointptr[s.points[1]];
719
720 Vector2 res;
721
722 if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) {
723 real_t nd = n.dot(res);
724 if (nd < d) {
725 d = nd;
726 r_point = res;
727 r_normal = (b - a).orthogonal().normalized();
728 inters = true;
729 }
730 }
731
732 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
733
734 } else {
735 stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
736 }
737 }
738 }
739 continue;
740 case VISIT_LEFT_BIT: {
741 stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
742 stack[level + 1] = bvh2.left | TEST_AABB_BIT;
743 level++;
744 }
745 continue;
746 case VISIT_RIGHT_BIT: {
747 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
748 stack[level + 1] = bvh2.right | TEST_AABB_BIT;
749 level++;
750 }
751 continue;
752 case VISIT_DONE_BIT: {
753 if (level == 0) {
754 done = true;
755 break;
756 } else {
757 level--;
758 }
759 }
760 continue;
761 }
762
763 if (done) {
764 break;
765 }
766 }
767
768 if (inters) {
769 if (n.dot(r_normal) > 0) {
770 r_normal = -r_normal;
771 }
772 }
773
774 return inters;
775}
776
777int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
778 if (p_len == 1) {
779 bvh_depth = MAX(p_depth, bvh_depth);
780 bvh.push_back(*p_bvh);
781 return bvh.size() - 1;
782 }
783
784 //else sort best
785
786 Rect2 global_aabb = p_bvh[0].aabb;
787 for (int i = 1; i < p_len; i++) {
788 global_aabb = global_aabb.merge(p_bvh[i].aabb);
789 }
790
791 if (global_aabb.size.x > global_aabb.size.y) {
792 SortArray<BVH, BVH_CompareX> sort;
793 sort.sort(p_bvh, p_len);
794
795 } else {
796 SortArray<BVH, BVH_CompareY> sort;
797 sort.sort(p_bvh, p_len);
798 }
799
800 int median = p_len / 2;
801
802 BVH node;
803 node.aabb = global_aabb;
804 int node_idx = bvh.size();
805 bvh.push_back(node);
806
807 int l = _generate_bvh(p_bvh, median, p_depth + 1);
808 int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1);
809 bvh.write[node_idx].left = l;
810 bvh.write[node_idx].right = r;
811
812 return node_idx;
813}
814
815void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
816#ifdef REAL_T_IS_DOUBLE
817 ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
818#else
819 ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
820#endif
821
822 Rect2 aabb_new;
823
824 if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) {
825 Vector<Vector2> p2arr = p_data;
826 int len = p2arr.size();
827 ERR_FAIL_COND(len % 2);
828
829 segments.clear();
830 points.clear();
831 bvh.clear();
832 bvh_depth = 1;
833
834 if (len == 0) {
835 configure(aabb_new);
836 return;
837 }
838
839 const Vector2 *arr = p2arr.ptr();
840
841 HashMap<Point2, int> pointmap;
842 for (int i = 0; i < len; i += 2) {
843 Point2 p1 = arr[i];
844 Point2 p2 = arr[i + 1];
845 int idx_p1, idx_p2;
846
847 if (pointmap.has(p1)) {
848 idx_p1 = pointmap[p1];
849 } else {
850 idx_p1 = pointmap.size();
851 pointmap[p1] = idx_p1;
852 }
853
854 if (pointmap.has(p2)) {
855 idx_p2 = pointmap[p2];
856 } else {
857 idx_p2 = pointmap.size();
858 pointmap[p2] = idx_p2;
859 }
860
861 Segment s;
862 s.points[0] = idx_p1;
863 s.points[1] = idx_p2;
864 segments.push_back(s);
865 }
866
867 points.resize(pointmap.size());
868 aabb_new.position = pointmap.begin()->key;
869 for (const KeyValue<Point2, int> &E : pointmap) {
870 aabb_new.expand_to(E.key);
871 points.write[E.value] = E.key;
872 }
873
874 Vector<BVH> main_vbh;
875 main_vbh.resize(segments.size());
876 for (int i = 0; i < main_vbh.size(); i++) {
877 main_vbh.write[i].aabb.position = points[segments[i].points[0]];
878 main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]);
879 main_vbh.write[i].left = -1;
880 main_vbh.write[i].right = i;
881 }
882
883 _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1);
884
885 } else {
886 //dictionary with arrays
887 }
888
889 configure(aabb_new);
890}
891
892Variant GodotConcavePolygonShape2D::get_data() const {
893 Vector<Vector2> rsegments;
894 int len = segments.size();
895 rsegments.resize(len * 2);
896 Vector2 *w = rsegments.ptrw();
897 for (int i = 0; i < len; i++) {
898 w[(i << 1) + 0] = points[segments[i].points[0]];
899 w[(i << 1) + 1] = points[segments[i].points[1]];
900 }
901
902 return rsegments;
903}
904
905void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
906 uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
907
908 enum {
909 TEST_AABB_BIT = 0,
910 VISIT_LEFT_BIT = 1,
911 VISIT_RIGHT_BIT = 2,
912 VISIT_DONE_BIT = 3,
913 VISITED_BIT_SHIFT = 29,
914 NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
915 VISITED_BIT_MASK = ~NODE_IDX_MASK,
916
917 };
918
919 /*
920 for(int i=0;i<bvh_depth;i++)
921 stack[i]=0;
922 */
923
924 if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) {
925 return;
926 }
927
928 int level = 0;
929
930 const Segment *segmentptr = &segments[0];
931 const Vector2 *pointptr = &points[0];
932 const BVH *bvhptr = &bvh[0];
933
934 stack[0] = 0;
935 while (true) {
936 uint32_t node = stack[level] & NODE_IDX_MASK;
937 const BVH &bvh2 = bvhptr[node];
938
939 switch (stack[level] >> VISITED_BIT_SHIFT) {
940 case TEST_AABB_BIT: {
941 bool valid = p_local_aabb.intersects(bvh2.aabb);
942 if (!valid) {
943 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
944
945 } else {
946 if (bvh2.left < 0) {
947 const Segment &s = segmentptr[bvh2.right];
948 Vector2 a = pointptr[s.points[0]];
949 Vector2 b = pointptr[s.points[1]];
950
951 GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized());
952
953 if (p_callback(p_userdata, &ss)) {
954 return;
955 }
956 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
957
958 } else {
959 stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
960 }
961 }
962 }
963 continue;
964 case VISIT_LEFT_BIT: {
965 stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
966 stack[level + 1] = bvh2.left | TEST_AABB_BIT;
967 level++;
968 }
969 continue;
970 case VISIT_RIGHT_BIT: {
971 stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
972 stack[level + 1] = bvh2.right | TEST_AABB_BIT;
973 level++;
974 }
975 continue;
976 case VISIT_DONE_BIT: {
977 if (level == 0) {
978 return;
979 } else {
980 level--;
981 }
982 }
983 continue;
984 }
985 }
986}
987