/*************************************************************************/
/*  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(real_t p_mass, const Size2 &p_scale) const {

	return 0;
}


void LineShape2DSW::set_data(const Variant& p_data) {

	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
	Array arr = p_data;
	ERR_FAIL_COND(arr.size()!=2);
	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(real_t 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;

	}

	real_t 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(real_t 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.0 + 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(real_t 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;
		real_t dp = ag.dot(p_normal);
		if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
			continue;

		real_t 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(real_t p_mass,const Size2& p_scale) const {

	Vector2 he2=half_extents*2*p_scale;
	return p_mass*he2.dot(he2)/12.0;
}

void RectangleShape2DSW::set_data(const Variant& p_data) {

	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);

	half_extents=p_data;
	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;

	real_t 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 {

		real_t 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 {


	real_t d = 1e10;
	Vector2 n = (p_end-p_begin).normalized();
	bool collided=false;

	//try spheres
	for(int i=0;i<2;i++) {

		Vector2 begin = p_begin;
		Vector2 end = p_end;
		real_t ofs = (i==0)?-height*0.5:height*0.5;
		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(real_t p_mass, const Size2 &p_scale) const {

	Vector2 he2=Vector2(radius*2,height+radius*2)*p_scale;
	return p_mass*he2.dot(he2)/12.0;
}

void CapsuleShape2DSW::set_data(const Variant& p_data) {

	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);

	if (p_data.get_type()==Variant::ARRAY) {
		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++) {

		real_t 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;

		real_t 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(real_t 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.0 + 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::POOL_VECTOR2_ARRAY && p_data.get_type()!=Variant::POOL_REAL_ARRAY);


	if (points)
		memdelete_arr(points);
	points=NULL;
	point_count=0;

	if (p_data.get_type()==Variant::POOL_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)) {

							real_t 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::POOL_VECTOR2_ARRAY && p_data.get_type()!=Variant::POOL_REAL_ARRAY);

	Rect2 aabb;

	if (p_data.get_type()==Variant::POOL_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;
		}
	}

}