summaryrefslogtreecommitdiff
path: root/servers/physics_2d/shape_2d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/shape_2d_sw.cpp')
-rw-r--r--servers/physics_2d/shape_2d_sw.cpp170
1 files changed, 43 insertions, 127 deletions
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 06096d674a..c0589b9804 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -43,7 +43,6 @@ void Shape2DSW::configure(const Rect2 &p_aabb) {
}
Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const {
-
Vector2 res[2];
int amnt;
get_supports(p_normal, res, amnt);
@@ -51,7 +50,6 @@ Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const {
}
void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
-
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
if (E) {
E->get()++;
@@ -61,7 +59,6 @@ void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
}
void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
-
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
ERR_FAIL_COND(!E);
E->get()--;
@@ -71,7 +68,6 @@ void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
}
bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const {
-
return owners.has(p_owner);
}
@@ -80,13 +76,11 @@ const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const {
}
Shape2DSW::Shape2DSW() {
-
custom_bias = 0;
configured = false;
}
Shape2DSW::~Shape2DSW() {
-
ERR_FAIL_COND(owners.size());
}
@@ -95,23 +89,19 @@ Shape2DSW::~Shape2DSW() {
/*********************************************************/
void LineShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
-
r_amount = 0;
}
bool LineShape2DSW::contains_point(const Vector2 &p_point) const {
-
return normal.dot(p_point) < d;
}
bool LineShape2DSW::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;
}
@@ -119,7 +109,6 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e
//printf("dist is %i\n",dist);
if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
-
return false;
}
@@ -130,12 +119,10 @@ bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_e
}
real_t LineShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
-
return 0;
}
void LineShape2DSW::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);
@@ -145,7 +132,6 @@ void LineShape2DSW::set_data(const Variant &p_data) {
}
Variant LineShape2DSW::get_data() const {
-
Array arr;
arr.resize(2);
arr[0] = normal;
@@ -158,32 +144,28 @@ Variant LineShape2DSW::get_data() const {
/*********************************************************/
void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
-
r_amount = 1;
- if (p_normal.y > 0)
+ if (p_normal.y > 0) {
*r_supports = Vector2(0, length);
- else
+ } else {
*r_supports = Vector2();
+ }
}
bool RayShape2DSW::contains_point(const Vector2 &p_point) const {
-
return false;
}
bool RayShape2DSW::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 RayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
-
return 0; //rays are mass-less
}
void RayShape2DSW::set_data(const Variant &p_data) {
-
Dictionary d = p_data;
length = d["length"];
slips_on_slope = d["slips_on_slope"];
@@ -191,7 +173,6 @@ void RayShape2DSW::set_data(const Variant &p_data) {
}
Variant RayShape2DSW::get_data() const {
-
Dictionary d;
d["length"] = length;
d["slips_on_slope"] = slips_on_slope;
@@ -203,7 +184,6 @@ Variant RayShape2DSW::get_data() const {
/*********************************************************/
void SegmentShape2DSW::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;
@@ -212,22 +192,22 @@ void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
}
real_t dp = p_normal.dot(b - a);
- if (dp > 0)
+ if (dp > 0) {
*r_supports = b;
- else
+ } else {
*r_supports = a;
+ }
r_amount = 1;
}
bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const {
-
return false;
}
bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
-
- if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point))
+ if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point)) {
return false;
+ }
if (n.dot(p_begin) > n.dot(a)) {
r_normal = n;
@@ -239,12 +219,10 @@ bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
}
real_t SegmentShape2DSW::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 SegmentShape2DSW::set_data(const Variant &p_data) {
-
ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
Rect2 r = p_data;
@@ -255,15 +233,16 @@ void SegmentShape2DSW::set_data(const Variant &p_data) {
Rect2 aabb;
aabb.position = a;
aabb.expand_to(b);
- if (aabb.size.x == 0)
+ if (aabb.size.x == 0) {
aabb.size.x = 0.001;
- if (aabb.size.y == 0)
+ }
+ if (aabb.size.y == 0) {
aabb.size.y = 0.001;
+ }
configure(aabb);
}
Variant SegmentShape2DSW::get_data() const {
-
Rect2 r;
r.position = a;
r.size = b;
@@ -275,18 +254,15 @@ Variant SegmentShape2DSW::get_data() const {
/*********************************************************/
void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
-
r_amount = 1;
*r_supports = p_normal * radius;
}
bool CircleShape2DSW::contains_point(const Vector2 &p_point) const {
-
return p_point.length_squared() < radius * radius;
}
bool CircleShape2DSW::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;
@@ -297,8 +273,9 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p
real_t sqrtterm = b * b - 4 * a * c;
- if (sqrtterm < 0)
+ if (sqrtterm < 0) {
return false;
+ }
sqrtterm = Math::sqrt(sqrtterm);
real_t res = (-b - sqrtterm) / (2 * a);
@@ -312,21 +289,18 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p
}
real_t CircleShape2DSW::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 CircleShape2DSW::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 CircleShape2DSW::get_data() const {
-
return radius;
}
@@ -335,14 +309,13 @@ Variant CircleShape2DSW::get_data() const {
/*********************************************************/
void RectangleShape2DSW::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)
+ if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) {
continue;
+ }
real_t sgn = dp > 0 ? 1.0 : -1.0;
@@ -374,18 +347,15 @@ bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
}
bool RectangleShape2DSW::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 RectangleShape2DSW::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 RectangleShape2DSW::set_data(const Variant &p_data) {
-
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
half_extents = p_data;
@@ -393,7 +363,6 @@ void RectangleShape2DSW::set_data(const Variant &p_data) {
}
Variant RectangleShape2DSW::get_data() const {
-
return half_extents;
}
@@ -402,13 +371,11 @@ Variant RectangleShape2DSW::get_data() const {
/*********************************************************/
void CapsuleShape2DSW::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();
@@ -421,7 +388,6 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
r_supports[1].y -= height * 0.5;
} else {
-
real_t h = (d > 0) ? height : -height;
n *= radius;
@@ -432,25 +398,23 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
}
bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const {
-
Vector2 p = p_point;
p.y = Math::abs(p.y);
p.y -= height * 0.5;
- if (p.y < 0)
+ if (p.y < 0) {
p.y = 0;
+ }
return p.length_squared() < radius * radius;
}
bool CapsuleShape2DSW::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 : height * 0.5;
@@ -467,8 +431,9 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
real_t sqrtterm = b * b - 4 * a * c;
- if (sqrtterm < 0)
+ if (sqrtterm < 0) {
continue;
+ }
sqrtterm = Math::sqrt(sqrtterm);
real_t res = (-b - sqrtterm) / (2 * a);
@@ -490,7 +455,6 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
Vector2 rpos, rnorm;
if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
-
real_t pd = n.dot(rpos);
if (pd < d) {
r_point = rpos;
@@ -505,13 +469,11 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
}
real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
-
Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale;
return p_mass * he2.dot(he2) / 12.0;
}
void CapsuleShape2DSW::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) {
@@ -520,7 +482,6 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) {
height = arr[0];
radius = arr[1];
} else {
-
Point2 p = p_data;
radius = p.x;
height = p.y;
@@ -531,7 +492,6 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) {
}
Variant CapsuleShape2DSW::get_data() const {
-
return Point2(height, radius);
}
@@ -540,12 +500,10 @@ Variant CapsuleShape2DSW::get_data() const {
/*********************************************************/
void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
-
int support_idx = -1;
real_t d = -1e10;
for (int i = 0; i < point_count; i++) {
-
//test point
real_t ld = p_normal.dot(points[i].pos);
if (ld > d) {
@@ -555,7 +513,6 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su
//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;
@@ -570,30 +527,27 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su
}
bool ConvexPolygonShape2DSW::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)
+ if (d > 0) {
out = true;
- else
+ } else {
in = true;
+ }
}
return in != out;
}
bool ConvexPolygonShape2DSW::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)
@@ -602,12 +556,12 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
Vector2 res;
- if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res))
+ if (!Geometry::segment_intersects_segment_2d(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;
@@ -616,9 +570,9 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
}
if (inters) {
-
- if (n.dot(r_normal) > 0)
+ if (n.dot(r_normal) > 0) {
r_normal = -r_normal;
+ }
}
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
@@ -626,11 +580,9 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
}
real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
-
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);
}
@@ -638,11 +590,11 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2
}
void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
-
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
- if (points)
+ if (points) {
memdelete_arr(points);
+ }
points = nullptr;
point_count = 0;
@@ -658,13 +610,11 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
}
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).tangent().normalized();
}
} else {
-
Vector<real_t> dvr = p_data;
point_count = dvr.size() / 4;
ERR_FAIL_COND(point_count == 0);
@@ -673,7 +623,6 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
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];
@@ -685,14 +634,14 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
ERR_FAIL_COND(point_count == 0);
Rect2 aabb;
aabb.position = points[0].pos;
- for (int i = 1; i < point_count; i++)
+ for (int i = 1; i < point_count; i++) {
aabb.expand_to(points[i].pos);
+ }
configure(aabb);
}
Variant ConvexPolygonShape2DSW::get_data() const {
-
Vector<Vector2> dvr;
dvr.resize(point_count);
@@ -705,25 +654,22 @@ Variant ConvexPolygonShape2DSW::get_data() const {
}
ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
-
points = nullptr;
point_count = 0;
}
ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() {
-
- if (points)
+ if (points) {
memdelete_arr(points);
+ }
}
//////////////////////////////////////////////////
void ConcavePolygonShape2DSW::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;
@@ -737,12 +683,10 @@ void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_s
}
bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const {
-
return false; //sorry
}
bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
-
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
enum {
@@ -773,23 +717,18 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
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]];
@@ -797,10 +736,8 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
Vector2 res;
if (Geometry::segment_intersects_segment_2d(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).tangent().normalized();
@@ -811,54 +748,50 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
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
+ } else {
level--;
+ }
}
continue;
}
- if (done)
+ if (done) {
break;
+ }
}
if (inters) {
-
- if (n.dot(r_normal) > 0)
+ if (n.dot(r_normal) > 0) {
r_normal = -r_normal;
+ }
}
return inters;
}
int ConcavePolygonShape2DSW::_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;
@@ -872,12 +805,10 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
}
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);
}
@@ -898,13 +829,11 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
}
void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
-
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
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);
@@ -923,7 +852,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
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;
@@ -951,7 +879,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
points.resize(pointmap.size());
aabb.position = pointmap.front()->key();
for (Map<Point2, int>::Element *E = pointmap.front(); E; E = E->next()) {
-
aabb.expand_to(E->key());
points.write[E->get()] = E->key();
}
@@ -959,7 +886,6 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
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;
@@ -974,14 +900,13 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
configure(aabb);
}
-Variant ConcavePolygonShape2DSW::get_data() const {
+Variant ConcavePolygonShape2DSW::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]];
}
@@ -990,7 +915,6 @@ Variant ConcavePolygonShape2DSW::get_data() const {
}
void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const {
-
uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
enum {
@@ -1021,22 +945,17 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
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]];
@@ -1047,32 +966,29 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
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)
+ if (level == 0) {
return;
- else
+ } else {
level--;
+ }
}
continue;
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-30 05:08:23 +0000