asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
virtualx-engine/servers/physics_2d/shape_2d_sw.cpp

1152 lines
23 KiB
C++
Raw Blame History

/*************************************************************************/
/* shape_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* 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) {
PoolVector<Vector2> arr=p_data;
ERR_FAIL_COND(arr.size()==0);
point_count=arr.size();
points = memnew_arr(Point,point_count);
PoolVector<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 {
PoolVector<real_t> dvr = p_data;
point_count=dvr.size()/4;
ERR_FAIL_COND(point_count==0);
points = memnew_arr(Point,point_count);
PoolVector<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 {
PoolVector<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) {
PoolVector<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;
}
PoolVector<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 {
PoolVector<Vector2> rsegments;
int len = segments.size();
rsegments.resize(len*2);
PoolVector<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=PoolVector<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 a new issue View git blame Copy permalink