From d7d65fa2f2b51d03f7bdfcbceedca99188ce979c Mon Sep 17 00:00:00 2001
From: Juan Linietsky
Date: Wed, 19 Feb 2014 11:57:14 -0300
Subject: -improved physics ccd -html5 exporter works again -disable repeat on
 image loader by default -can change shape offset en tileset, texture offset
 was broken

---
 servers/physics_2d/shape_2d_sw.cpp | 2113 ++++++++++++++++++------------------
 1 file changed, 1056 insertions(+), 1057 deletions(-)

(limited to 'servers/physics_2d/shape_2d_sw.cpp')

diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 5ad05a18a..012c4ed40 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;
+		}
+	}
+
+}
+
+
-- 
cgit v1.2.3-70-g09d2