godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
godot/servers/physics_2d/shape_2d_sw.cpp

1066 lines
24 KiB
C++
Raw Blame History

/*************************************************************************/
/* shape_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2018 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2018 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 "shape_2d_sw.h"
#include "geometry.h"
#include "sort.h"
void Shape2DSW::configure(const Rect2 &p_aabb) {
aabb = p_aabb;
configured = true;
for (Map<ShapeOwner2DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
ShapeOwner2DSW *co = (ShapeOwner2DSW *)E->key();
co->_shape_changed();
}
}
Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const {
Vector2 res[2];
int amnt;
get_supports(p_normal, res, amnt);
return res[0];
}
void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
if (E) {
E->get()++;
} else {
owners[p_owner] = 1;
}
}
void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
ERR_FAIL_COND(!E);
E->get()--;
if (E->get() == 0) {
owners.erase(E);
}
}
bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const {
return owners.has(p_owner);
}
const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const {
return owners;
}
Shape2DSW::Shape2DSW() {
custom_bias = 0;
configured = false;
}
Shape2DSW::~Shape2DSW() {
ERR_FAIL_COND(owners.size());
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
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;
}
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 LineShape2DSW::get_moment_of_inertia(float 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);
normal = arr[0];
d = arr[1];
configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2)));
}
Variant LineShape2DSW::get_data() const {
Array arr;
arr.resize(2);
arr[0] = normal;
arr[1] = d;
return arr;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void RayShape2DSW::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 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(float p_mass, const Size2 &p_scale) const {
return 0; //rays are mass-less
}
void RayShape2DSW::set_data(const Variant &p_data) {
length = p_data;
configure(Rect2(0, 0, 0.001, length));
}
Variant RayShape2DSW::get_data() const {
return length;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
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_TRESHOLD) {
r_supports[0] = a;
r_supports[1] = b;
r_amount = 2;
return;
}
float dp = p_normal.dot(b - a);
if (dp > 0)
*r_supports = b;
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))
return false;
if (n.dot(p_begin) > n.dot(a)) {
r_normal = n;
} else {
r_normal = -n;
}
return true;
}
real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
Vector2 s[2] = { a * p_scale, b * p_scale };
real_t l = s[1].distance_to(s[0]);
Vector2 ofs = (s[0] + s[1]) * 0.5;
return p_mass * (l * l / 12.0f + ofs.length_squared());
}
void SegmentShape2DSW::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
Rect2 r = p_data;
a = r.pos;
b = r.size;
n = (b - a).tangent();
Rect2 aabb;
aabb.pos = 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 SegmentShape2DSW::get_data() const {
Rect2 r;
r.pos = a;
r.size = b;
return r;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
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;
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 CircleShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
return (radius * radius) * (p_scale.x * 0.5 + p_scale.y * 0.5);
}
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;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
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;
float dp = ag.dot(p_normal);
if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
continue;
float 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 RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
return Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y;
}
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(float p_mass, const Size2 &p_scale) const {
Vector2 he2 = half_extents * 2 * p_scale;
return p_mass * he2.dot(he2) / 12.0f;
}
void RectangleShape2DSW::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 RectangleShape2DSW::get_data() const {
return half_extents;
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
Vector2 n = p_normal;
float d = n.y;
if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) {
// 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;
r_supports[1] = n;
r_supports[1].y -= height * 0.5;
} else {
float h = (d > 0) ? height : -height;
n *= radius;
n.y += h * 0.5;
r_amount = 1;
*r_supports = n;
}
}
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)
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 {
float 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;
float ofs = (i == 0) ? -height * 0.5 : height * 0.5;
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), 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;
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 CapsuleShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale;
return p_mass * he2.dot(he2) / 12.0f;
}
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) {
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 + radius);
configure(Rect2(-he, he * 2));
}
Variant CapsuleShape2DSW::get_data() const {
return Point2(height, radius);
}
/*********************************************************/
/*********************************************************/
/*********************************************************/
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) {
support_idx = i;
d = ld;
}
//test segment
if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
r_amount = 2;
r_supports[0] = points[i].pos;
r_supports[1] = points[(i + 1) % point_count].pos;
return;
}
}
ERR_FAIL_COND(support_idx == -1);
r_amount = 1;
r_supports[0] = points[support_idx].pos;
}
bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const {
bool out = false;
bool in = false;
for (int i = 0; i < point_count; i++) {
float 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) || (!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 crap.. no can do..
//if (d.dot(points[i].normal)>=0)
// continue;
Vector2 res;
if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res))
continue;
float nd = n.dot(res);
if (nd < d) {
d = nd;
r_point = res;
r_normal = points[i].normal;
inters = true;
}
}
if (inters) {
if (n.dot(r_normal) > 0)
r_normal = -r_normal;
}
//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
return inters; //todo
}
real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
Rect2 aabb;
aabb.pos = 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.0f + p_mass * (aabb.pos + aabb.size * 0.5).length_squared();
}
void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2_ARRAY && p_data.get_type() != Variant::REAL_ARRAY);
if (points)
memdelete_arr(points);
points = NULL;
point_count = 0;
if (p_data.get_type() == Variant::VECTOR2_ARRAY) {
DVector<Vector2> arr = p_data;
ERR_FAIL_COND(arr.size() == 0);
point_count = arr.size();
points = memnew_arr(Point, point_count);
DVector<Vector2>::Read r = arr.read();
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).tangent().normalized();
}
} else {
DVector<real_t> dvr = p_data;
point_count = dvr.size() / 4;
ERR_FAIL_COND(point_count == 0);
points = memnew_arr(Point, point_count);
DVector<real_t>::Read r = dvr.read();
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.pos = points[0].pos;
for (int i = 1; i < point_count; i++)
aabb.expand_to(points[i].pos);
configure(aabb);
}
Variant ConvexPolygonShape2DSW::get_data() const {
DVector<Vector2> dvr;
dvr.resize(point_count);
for (int i = 0; i < point_count; i++) {
dvr.set(i, points[i].pos);
}
return dvr;
}
ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
points = NULL;
point_count = 0;
}
ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() {
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;
idx = i;
}
}
r_amount = 1;
ERR_FAIL_COND(idx == -1);
*r_supports = points[idx];
}
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 {
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 &b = bvhptr[node];
bool done = false;
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
bool valid = b.aabb.intersects_segment(p_begin, p_end);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
if (b.left < 0) {
const Segment &s = segmentptr[b.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
Vector2 res;
if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) {
float nd = n.dot(res);
if (nd < d) {
d = nd;
r_point = res;
r_normal = (b - a).tangent().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] = b.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = b.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 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;
}
//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[node_idx].left = l;
bvh[node_idx].right = r;
return node_idx;
}
void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2_ARRAY && p_data.get_type() != Variant::REAL_ARRAY);
Rect2 aabb;
if (p_data.get_type() == Variant::VECTOR2_ARRAY) {
DVector<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;
}
DVector<Vector2>::Read arr = p2arr.read();
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.pos = pointmap.front()->key();
for (Map<Point2, int>::Element *E = pointmap.front(); E; E = E->next()) {
aabb.expand_to(E->key());
points[E->get()] = E->key();
}
Vector<BVH> main_vbh;
main_vbh.resize(segments.size());
for (int i = 0; i < main_vbh.size(); i++) {
main_vbh[i].aabb.pos = points[segments[i].points[0]];
main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
main_vbh[i].left = -1;
main_vbh[i].right = i;
}
_generate_bvh(&main_vbh[0], main_vbh.size(), 1);
} else {
//dictionary with arrays
}
configure(aabb);
}
Variant ConcavePolygonShape2DSW::get_data() const {
DVector<Vector2> rsegments;
int len = segments.size();
rsegments.resize(len * 2);
DVector<Vector2>::Write w = rsegments.write();
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]];
}
w = DVector<Vector2>::Write();
return rsegments;
}
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 {
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;
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 &b = bvhptr[node];
switch (stack[level] >> VISITED_BIT_SHIFT) {
case TEST_AABB_BIT: {
bool valid = p_local_aabb.intersects(b.aabb);
if (!valid) {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
} else {
if (b.left < 0) {
const Segment &s = segmentptr[b.right];
Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]];
SegmentShape2DSW ss(a, b, (b - a).tangent().normalized());
p_callback(p_userdata, &ss);
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] = b.left | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_RIGHT_BIT: {
stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
stack[level + 1] = b.right | TEST_AABB_BIT;
level++;
}
continue;
case VISIT_DONE_BIT: {
if (level == 0)
return;
else
level--;
}
continue;
}
}
}
Reference in New Issue View Git Blame Copy Permalink