/*************************************************************************/ /* godot_shape_2d.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ /* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "godot_shape_2d.h" #include "core/math/geometry_2d.h" #include "core/templates/sort_array.h" void GodotShape2D::configure(const Rect2 &p_aabb) { aabb = p_aabb; configured = true; for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) { GodotShapeOwner2D *co = const_cast<GodotShapeOwner2D *>(E.key); co->_shape_changed(); } } Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const { Vector2 res[2]; int amnt; get_supports(p_normal, res, amnt); return res[0]; } void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) { HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); if (E) { E->value++; } else { owners[p_owner] = 1; } } void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { HashMap<GodotShapeOwner2D *, int>::Iterator E = owners.find(p_owner); ERR_FAIL_COND(!E); E->value--; if (E->value == 0) { owners.remove(E); } } bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { return owners.has(p_owner); } const HashMap<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const { return owners; } GodotShape2D::~GodotShape2D() { ERR_FAIL_COND(owners.size()); } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 0; } bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const { return normal.dot(p_point) < d; } bool GodotWorldBoundaryShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { Vector2 segment = p_begin - p_end; real_t den = normal.dot(segment); //printf("den is %i\n",den); if (Math::abs(den) <= CMP_EPSILON) { return false; } real_t dist = (normal.dot(p_begin) - d) / den; //printf("dist is %i\n",dist); if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) { return false; } r_point = p_begin + segment * -dist; r_normal = normal; return true; } real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; } void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); Array arr = p_data; ERR_FAIL_COND(arr.size() != 2); normal = arr[0]; d = arr[1]; configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); } Variant GodotWorldBoundaryShape2D::get_data() const { Array arr; arr.resize(2); arr[0] = normal; arr[1] = d; return arr; } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 1; if (p_normal.y > 0) { *r_supports = Vector2(0, length); } else { *r_supports = Vector2(); } } bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const { return false; } bool GodotSeparationRayShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { return false; //rays can't be intersected } real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return 0; //rays are mass-less } void GodotSeparationRayShape2D::set_data(const Variant &p_data) { Dictionary d = p_data; length = d["length"]; slide_on_slope = d["slide_on_slope"]; configure(Rect2(0, 0, 0.001, length)); } Variant GodotSeparationRayShape2D::get_data() const { Dictionary d; d["length"] = length; d["slide_on_slope"] = slide_on_slope; return d; } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotSegmentShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { r_supports[0] = a; r_supports[1] = b; r_amount = 2; return; } real_t dp = p_normal.dot(b - a); if (dp > 0) { *r_supports = b; } else { *r_supports = a; } r_amount = 1; } bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const { return false; } bool GodotSegmentShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) { return false; } if (n.dot(p_begin) > n.dot(a)) { r_normal = n; } else { r_normal = -n; } return true; } real_t GodotSegmentShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12; } void GodotSegmentShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); Rect2 r = p_data; a = r.position; b = r.size; n = (b - a).orthogonal(); Rect2 aabb; aabb.position = a; aabb.expand_to(b); if (aabb.size.x == 0) { aabb.size.x = 0.001; } if (aabb.size.y == 0) { aabb.size.y = 0.001; } configure(aabb); } Variant GodotSegmentShape2D::get_data() const { Rect2 r; r.position = a; r.size = b; return r; } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { r_amount = 1; *r_supports = p_normal * radius; } bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const { return p_point.length_squared() < radius * radius; } bool GodotCircleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { Vector2 line_vec = p_end - p_begin; real_t a, b, c; a = line_vec.dot(line_vec); b = 2 * p_begin.dot(line_vec); c = p_begin.dot(p_begin) - radius * radius; real_t sqrtterm = b * b - 4 * a * c; if (sqrtterm < 0) { return false; } sqrtterm = Math::sqrt(sqrtterm); real_t res = (-b - sqrtterm) / (2 * a); if (res < 0 || res > 1 + CMP_EPSILON) { return false; } r_point = p_begin + line_vec * res; r_normal = r_point.normalized(); return true; } real_t GodotCircleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { real_t a = radius * p_scale.x; real_t b = radius * p_scale.y; return p_mass * (a * a + b * b) / 4; } void GodotCircleShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(!p_data.is_num()); radius = p_data; configure(Rect2(-radius, -radius, radius * 2, radius * 2)); } Variant GodotCircleShape2D::get_data() const { return radius; } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { for (int i = 0; i < 2; i++) { Vector2 ag; ag[i] = 1.0; real_t dp = ag.dot(p_normal); if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { continue; } real_t sgn = dp > 0 ? 1.0 : -1.0; r_amount = 2; r_supports[0][i] = half_extents[i] * sgn; r_supports[0][i ^ 1] = half_extents[i ^ 1]; r_supports[1][i] = half_extents[i] * sgn; r_supports[1][i ^ 1] = -half_extents[i ^ 1]; return; } /* USE POINT */ r_amount = 1; r_supports[0] = Vector2( (p_normal.x < 0) ? -half_extents.x : half_extents.x, (p_normal.y < 0) ? -half_extents.y : half_extents.y); } bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const { real_t x = p_point.x; real_t y = p_point.y; real_t edge_x = half_extents.x; real_t edge_y = half_extents.y; return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); } bool GodotRectangleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal); } real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { Vector2 he2 = half_extents * 2 * p_scale; return p_mass * he2.dot(he2) / 12.0; } void GodotRectangleShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); half_extents = p_data; configure(Rect2(-half_extents, half_extents * 2.0)); } Variant GodotRectangleShape2D::get_data() const { return half_extents; } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { Vector2 n = p_normal; real_t d = n.y; if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_THRESHOLD)) { // make it flat n.y = 0.0; n.normalize(); n *= radius; r_amount = 2; r_supports[0] = n; r_supports[0].y += height * 0.5 - radius; r_supports[1] = n; r_supports[1].y -= height * 0.5 - radius; } else { real_t h = height * 0.5 - radius; n *= radius; n.y += (d > 0) ? h : -h; r_amount = 1; *r_supports = n; } } bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const { Vector2 p = p_point; p.y = Math::abs(p.y); p.y -= height * 0.5 - radius; if (p.y < 0) { p.y = 0; } return p.length_squared() < radius * radius; } bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { real_t d = 1e10; Vector2 n = (p_end - p_begin).normalized(); bool collided = false; //try spheres for (int i = 0; i < 2; i++) { Vector2 begin = p_begin; Vector2 end = p_end; real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius; begin.y += ofs; end.y += ofs; Vector2 line_vec = end - begin; real_t a, b, c; a = line_vec.dot(line_vec); b = 2 * begin.dot(line_vec); c = begin.dot(begin) - radius * radius; real_t sqrtterm = b * b - 4 * a * c; if (sqrtterm < 0) { continue; } sqrtterm = Math::sqrt(sqrtterm); real_t res = (-b - sqrtterm) / (2 * a); if (res < 0 || res > 1 + CMP_EPSILON) { continue; } Vector2 point = begin + line_vec * res; Vector2 pointf(point.x, point.y - ofs); real_t pd = n.dot(pointf); if (pd < d) { r_point = pointf; r_normal = point.normalized(); d = pd; collided = true; } } Vector2 rpos, rnorm; if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) { real_t pd = n.dot(rpos); if (pd < d) { r_point = rpos; r_normal = rnorm; d = pd; collided = true; } } //return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal); return collided; //todo } real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { Vector2 he2 = Vector2(radius * 2, height) * p_scale; return p_mass * he2.dot(he2) / 12.0; } void GodotCapsuleShape2D::set_data(const Variant &p_data) { ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); if (p_data.get_type() == Variant::ARRAY) { Array arr = p_data; ERR_FAIL_COND(arr.size() != 2); height = arr[0]; radius = arr[1]; } else { Point2 p = p_data; radius = p.x; height = p.y; } Point2 he(radius, height * 0.5); configure(Rect2(-he, he * 2)); } Variant GodotCapsuleShape2D::get_data() const { return Point2(height, radius); } /*********************************************************/ /*********************************************************/ /*********************************************************/ void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { int support_idx = -1; real_t d = -1e10; r_amount = 0; for (int i = 0; i < point_count; i++) { //test point real_t ld = p_normal.dot(points[i].pos); if (ld > d) { support_idx = i; d = ld; } //test segment if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; r_supports[0] = points[i].pos; r_supports[1] = points[(i + 1) % point_count].pos; return; } } ERR_FAIL_COND_MSG(support_idx == -1, "Convex polygon shape support not found."); r_amount = 1; r_supports[0] = points[support_idx].pos; } bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const { bool out = false; bool in = false; for (int i = 0; i < point_count; i++) { real_t d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos); if (d > 0) { out = true; } else { in = true; } } return in != out; } bool GodotConvexPolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { Vector2 n = (p_end - p_begin).normalized(); real_t d = 1e10; bool inters = false; for (int i = 0; i < point_count; i++) { Vector2 res; if (!Geometry2D::segment_intersects_segment(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res)) { continue; } real_t nd = n.dot(res); if (nd < d) { d = nd; r_point = res; r_normal = points[i].normal; inters = true; } } return inters; } real_t GodotConvexPolygonShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points."); Rect2 aabb; aabb.position = points[0].pos * p_scale; for (int i = 0; i < point_count; i++) { aabb.expand_to(points[i].pos * p_scale); } return p_mass * aabb.size.dot(aabb.size) / 12.0; } void GodotConvexPolygonShape2D::set_data(const Variant &p_data) { #ifdef REAL_T_IS_DOUBLE ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); #else ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); #endif if (points) { memdelete_arr(points); } points = nullptr; point_count = 0; if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { Vector<Vector2> arr = p_data; ERR_FAIL_COND(arr.size() == 0); point_count = arr.size(); points = memnew_arr(Point, point_count); const Vector2 *r = arr.ptr(); for (int i = 0; i < point_count; i++) { points[i].pos = r[i]; } for (int i = 0; i < point_count; i++) { Vector2 p = points[i].pos; Vector2 pn = points[(i + 1) % point_count].pos; points[i].normal = (pn - p).orthogonal().normalized(); } } else { Vector<real_t> dvr = p_data; point_count = dvr.size() / 4; ERR_FAIL_COND(point_count == 0); points = memnew_arr(Point, point_count); const real_t *r = dvr.ptr(); for (int i = 0; i < point_count; i++) { int idx = i << 2; points[i].pos.x = r[idx + 0]; points[i].pos.y = r[idx + 1]; points[i].normal.x = r[idx + 2]; points[i].normal.y = r[idx + 3]; } } ERR_FAIL_COND(point_count == 0); Rect2 aabb; aabb.position = points[0].pos; for (int i = 1; i < point_count; i++) { aabb.expand_to(points[i].pos); } configure(aabb); } Variant GodotConvexPolygonShape2D::get_data() const { Vector<Vector2> dvr; dvr.resize(point_count); for (int i = 0; i < point_count; i++) { dvr.set(i, points[i].pos); } return dvr; } GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() { if (points) { memdelete_arr(points); } } ////////////////////////////////////////////////// void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { real_t d = -1e10; int idx = -1; for (int i = 0; i < points.size(); i++) { real_t ld = p_normal.dot(points[i]); if (ld > d) { d = ld; idx = i; } } r_amount = 1; ERR_FAIL_COND(idx == -1); *r_supports = points[idx]; } bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const { return false; //sorry } bool GodotConcavePolygonShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { if (segments.size() == 0 || points.size() == 0) { return false; } uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { TEST_AABB_BIT = 0, VISIT_LEFT_BIT = 1, VISIT_RIGHT_BIT = 2, VISIT_DONE_BIT = 3, VISITED_BIT_SHIFT = 29, NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, VISITED_BIT_MASK = ~NODE_IDX_MASK, }; Vector2 n = (p_end - p_begin).normalized(); real_t d = 1e10; bool inters = false; /* for(int i=0;i<bvh_depth;i++) stack[i]=0; */ int level = 0; const Segment *segmentptr = &segments[0]; const Vector2 *pointptr = &points[0]; const BVH *bvhptr = &bvh[0]; stack[0] = 0; while (true) { uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &bvh = bvhptr[node]; bool done = false; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { bool valid = bvh.aabb.intersects_segment(p_begin, p_end); if (!valid) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { if (bvh.left < 0) { const Segment &s = segmentptr[bvh.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; Vector2 res; if (Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &res)) { real_t nd = n.dot(res); if (nd < d) { d = nd; r_point = res; r_normal = (b - a).orthogonal().normalized(); inters = true; } } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } } continue; case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.left | TEST_AABB_BIT; level++; } continue; case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.right | TEST_AABB_BIT; level++; } continue; case VISIT_DONE_BIT: { if (level == 0) { done = true; break; } else { level--; } } continue; } if (done) { break; } } if (inters) { if (n.dot(r_normal) > 0) { r_normal = -r_normal; } } return inters; } int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { if (p_len == 1) { bvh_depth = MAX(p_depth, bvh_depth); bvh.push_back(*p_bvh); return bvh.size() - 1; } //else sort best Rect2 global_aabb = p_bvh[0].aabb; for (int i = 1; i < p_len; i++) { global_aabb = global_aabb.merge(p_bvh[i].aabb); } if (global_aabb.size.x > global_aabb.size.y) { SortArray<BVH, BVH_CompareX> sort; sort.sort(p_bvh, p_len); } else { SortArray<BVH, BVH_CompareY> sort; sort.sort(p_bvh, p_len); } int median = p_len / 2; BVH node; node.aabb = global_aabb; int node_idx = bvh.size(); bvh.push_back(node); int l = _generate_bvh(p_bvh, median, p_depth + 1); int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1); bvh.write[node_idx].left = l; bvh.write[node_idx].right = r; return node_idx; } void GodotConcavePolygonShape2D::set_data(const Variant &p_data) { #ifdef REAL_T_IS_DOUBLE ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); #else ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY); #endif Rect2 aabb; if (p_data.get_type() == Variant::PACKED_VECTOR2_ARRAY) { Vector<Vector2> p2arr = p_data; int len = p2arr.size(); ERR_FAIL_COND(len % 2); segments.clear(); points.clear(); bvh.clear(); bvh_depth = 1; if (len == 0) { configure(aabb); return; } const Vector2 *arr = p2arr.ptr(); HashMap<Point2, int> pointmap; for (int i = 0; i < len; i += 2) { Point2 p1 = arr[i]; Point2 p2 = arr[i + 1]; int idx_p1, idx_p2; if (pointmap.has(p1)) { idx_p1 = pointmap[p1]; } else { idx_p1 = pointmap.size(); pointmap[p1] = idx_p1; } if (pointmap.has(p2)) { idx_p2 = pointmap[p2]; } else { idx_p2 = pointmap.size(); pointmap[p2] = idx_p2; } Segment s; s.points[0] = idx_p1; s.points[1] = idx_p2; segments.push_back(s); } points.resize(pointmap.size()); aabb.position = pointmap.begin()->key; for (const KeyValue<Point2, int> &E : pointmap) { aabb.expand_to(E.key); points.write[E.value] = E.key; } Vector<BVH> main_vbh; main_vbh.resize(segments.size()); for (int i = 0; i < main_vbh.size(); i++) { main_vbh.write[i].aabb.position = points[segments[i].points[0]]; main_vbh.write[i].aabb.expand_to(points[segments[i].points[1]]); main_vbh.write[i].left = -1; main_vbh.write[i].right = i; } _generate_bvh(main_vbh.ptrw(), main_vbh.size(), 1); } else { //dictionary with arrays } configure(aabb); } Variant GodotConcavePolygonShape2D::get_data() const { Vector<Vector2> rsegments; int len = segments.size(); rsegments.resize(len * 2); Vector2 *w = rsegments.ptrw(); for (int i = 0; i < len; i++) { w[(i << 1) + 0] = points[segments[i].points[0]]; w[(i << 1) + 1] = points[segments[i].points[1]]; } return rsegments; } void GodotConcavePolygonShape2D::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth); enum { TEST_AABB_BIT = 0, VISIT_LEFT_BIT = 1, VISIT_RIGHT_BIT = 2, VISIT_DONE_BIT = 3, VISITED_BIT_SHIFT = 29, NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1, VISITED_BIT_MASK = ~NODE_IDX_MASK, }; /* for(int i=0;i<bvh_depth;i++) stack[i]=0; */ if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) { return; } int level = 0; const Segment *segmentptr = &segments[0]; const Vector2 *pointptr = &points[0]; const BVH *bvhptr = &bvh[0]; stack[0] = 0; while (true) { uint32_t node = stack[level] & NODE_IDX_MASK; const BVH &bvh = bvhptr[node]; switch (stack[level] >> VISITED_BIT_SHIFT) { case TEST_AABB_BIT: { bool valid = p_local_aabb.intersects(bvh.aabb); if (!valid) { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { if (bvh.left < 0) { const Segment &s = segmentptr[bvh.right]; Vector2 a = pointptr[s.points[0]]; Vector2 b = pointptr[s.points[1]]; GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized()); if (p_callback(p_userdata, &ss)) { return; } stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; } else { stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node; } } } continue; case VISIT_LEFT_BIT: { stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.left | TEST_AABB_BIT; level++; } continue; case VISIT_RIGHT_BIT: { stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node; stack[level + 1] = bvh.right | TEST_AABB_BIT; level++; } continue; case VISIT_DONE_BIT: { if (level == 0) { return; } else { level--; } } continue; } } }