diff options
Diffstat (limited to 'servers/physics_2d/godot_shape_2d.cpp')
| -rw-r--r-- | servers/physics_2d/godot_shape_2d.cpp | 995 |
1 files changed, 995 insertions, 0 deletions
diff --git a/servers/physics_2d/godot_shape_2d.cpp b/servers/physics_2d/godot_shape_2d.cpp new file mode 100644 index 0000000000..3604004324 --- /dev/null +++ b/servers/physics_2d/godot_shape_2d.cpp @@ -0,0 +1,995 @@ +/*************************************************************************/ +/* godot_shape_2d.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 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 = (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) { + Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner); + if (E) { + E->get()++; + } else { + owners[p_owner] = 1; + } +} + +void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) { + Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner); + ERR_FAIL_COND(!E); + E->get()--; + if (E->get() == 0) { + owners.erase(E); + } +} + +bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const { + return owners.has(p_owner); +} + +const Map<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++) { + //hmm.. no can do.. + /* + if (d.dot(points[i].normal)>=0) + continue; + */ + + 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(); + + Map<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.front()->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; + } + } +} |