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.cpp2113
1 files changed, 1056 insertions, 1057 deletions
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 5ad05a18ac..012c4ed404 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -26,1060 +26,1059 @@
/* 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"
-
-#define _SEGMENT_IS_VALID_SUPPORT_TRESHOLD 0.99998
-
-
-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::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 {
-
- 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::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 {
-
- 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::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;
-
- Vector2 d = p_end-p_begin;
- 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 {
-
- real_t l = b.distance_to(a);
- Vector2 ofs = (a+b)*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::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 {
-
- return radius*radius;
-}
-
-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::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 {
-
- Vector2 he2=half_extents*2;
- 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::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 {
-
- Vector2 he2(radius*2,height+radius*2);
- 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::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 {
-
- Rect2 aabb;
- aabb.pos=points[0].pos;
- for(int i=0;i<point_count;i++) {
-
- aabb.expand_to(points[i].pos);
- }
-
- return p_mass*aabb.size.dot(aabb.size)/12.0f;
-}
-
-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::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];
- int pos=bvh.size()-1;
-
-
- 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);
-
- segments.clear();;
- points.clear();;
- bvh.clear();;
- bvh_depth=1;
-
- Rect2 aabb;
-
- if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
-
- DVector<Vector2> p2arr = p_data;
- int len = p2arr.size();
- 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 (p1==p2)
- continue; //don't want it
-
- 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];
- int pos=bvh.size()-1;
-
-
- 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;
- }
- }
-
-}
-
-
+#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::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 {
+
+ 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::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 {
+
+ 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::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;
+
+ Vector2 d = p_end-p_begin;
+ 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 {
+
+ real_t l = b.distance_to(a);
+ Vector2 ofs = (a+b)*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::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 {
+
+ return radius*radius;
+}
+
+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::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 {
+
+ Vector2 he2=half_extents*2;
+ 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::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 {
+
+ Vector2 he2(radius*2,height+radius*2);
+ 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::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 {
+
+ Rect2 aabb;
+ aabb.pos=points[0].pos;
+ for(int i=0;i<point_count;i++) {
+
+ aabb.expand_to(points[i].pos);
+ }
+
+ return p_mass*aabb.size.dot(aabb.size)/12.0f;
+}
+
+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::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];
+ int pos=bvh.size()-1;
+
+
+ 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);
+
+ segments.clear();;
+ points.clear();;
+ bvh.clear();;
+ bvh_depth=1;
+
+ Rect2 aabb;
+
+ if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+
+ DVector<Vector2> p2arr = p_data;
+ int len = p2arr.size();
+ 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 (p1==p2)
+ continue; //don't want it
+
+ 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];
+ int pos=bvh.size()-1;
+
+
+ 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;
+ }
+ }
+
+}
+
+
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-09 17:30:39 +0000