Little Bits

-=-=-=-=-=-

-Fixed small bugs all around
-Added ability to show/hide entire sections of the spatial (3D) tree
-WIP new vehicle (not ready yet) based on Bullet

11 files changed, 1683 insertions, 1641 deletions

diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 094f56a421..c50bfac472 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -227,6 +227,7 @@ bool BodyPairSW::setup(float p_step) {
 		Vector3 global_A = xform_Au.xform(c.local_A);
 		Vector3 global_B = xform_Bu.xform(c.local_B);
 
+
 		real_t depth = c.normal.dot(global_A - global_B);
 
 		if (depth<=0) {
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 8923899278..8d30069777 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -323,6 +323,7 @@ public:
 	virtual Transform get_transform() const {  return body->get_transform();  }
 
 	virtual void add_force(const Vector3& p_force, const Vector3& p_pos) {  body->add_force(p_force,p_pos); }
+	virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { body->apply_impulse(p_pos,p_j); }
 
 	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
 	virtual bool is_sleeping() const {  return !body->is_active();  }
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index 70e5c00b92..1312b7da95 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -26,1639 +26,1639 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#include "shape_sw.h"

-#include "geometry.h"

-#include "sort.h"

-#include "quick_hull.h"

-#define _POINT_SNAP 0.001953125

-#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002

-#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998

-

-

-void ShapeSW::configure(const AABB& p_aabb) {

-	aabb=p_aabb;

-	configured=true;

-	for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {

-		ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();

-		co->_shape_changed();

-	}

-}

-

-

-Vector3 ShapeSW::get_support(const Vector3& p_normal) const {

-

-	Vector3 res;

-	int amnt;

-	get_supports(p_normal,1,&res,amnt);

-	return res;

-}

-

-void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {

-

-	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);

-	if (E) {

-		E->get()++;

-	} else {

-		owners[p_owner]=1;

-	}

-}

-

-void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){

-

-	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);

-	ERR_FAIL_COND(!E);

-	E->get()--;

-	if (E->get()==0) {

-		owners.erase(E);

-	}

-

-}

-

-bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{

-

-	return owners.has(p_owner);

-

-}

-

-const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{

-	return owners;

-}

-

-

-ShapeSW::ShapeSW() {

-

-	custom_bias=0;

-	configured=false;

-}

-

-

-ShapeSW::~ShapeSW() {

-

-	ERR_FAIL_COND(owners.size());

-}

-

-

-

-Plane PlaneShapeSW::get_plane() const {

-

-	return plane;

-}

-

-void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	// gibberish, a plane is infinity

-	r_min=-1e7;

-	r_max=1e7;

-}

-

-Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {

-

-	return p_normal*1e15;

-}

-

-

-bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	bool inters=plane.intersects_segment(p_begin,p_end,&r_result);

-	if(inters)

-		r_normal=plane.normal;

-	return inters;

-}

-

-Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	return Vector3(); //wtf

-}

-

-void PlaneShapeSW::_setup(const Plane& p_plane)  {

-

-	plane=p_plane;

-	configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));

-}

-

-void PlaneShapeSW::set_data(const Variant& p_data) {

-

-	_setup(p_data);

-

-}

-

-Variant PlaneShapeSW::get_data() const {

-

-	return plane;

-}

-

-PlaneShapeSW::PlaneShapeSW()  {

-

-

-}

-

-//

-

-float RayShapeSW::get_length() const {

-

-	return length;

-}

-

-void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	// don't think this will be even used

-	r_min=0;

-	r_max=1;

-}

-

-Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {

-

-	if (p_normal.z>0)

-		return Vector3(0,0,length);

-	else

-		return Vector3(0,0,0);

-}

-

-void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {

-

-		r_amount=2;

-		r_supports[0]=Vector3(0,0,0);

-		r_supports[1]=Vector3(0,0,length);

-	} if (p_normal.z>0) {

-		r_amount=1;

-		*r_supports=Vector3(0,0,length);

-	} else {

-		r_amount=1;

-		*r_supports=Vector3(0,0,0);

-	}

-}

-

-

-bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	return false; //simply not possible

-}

-

-Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	return Vector3();

-}

-

-void RayShapeSW::_setup(float p_length)  {

-

-	length=p_length;

-	configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));

-}

-

-void RayShapeSW::set_data(const Variant& p_data) {

-

-	_setup(p_data);

-

-}

-

-Variant RayShapeSW::get_data() const {

-

-	return length;

-}

-

-RayShapeSW::RayShapeSW()  {

-

-	length=1;

-}

-

-

-

-/********** SPHERE *************/

-

-real_t SphereShapeSW::get_radius() const {

-

-	return radius;

-}

-

-void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	float d = p_normal.dot( p_transform.origin );

-

-	// figure out scale at point

-	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);

-	float scale = local_normal.length();

-

-	r_min = d - (radius) * scale;

-	r_max = d + (radius) * scale;

-

-}

-

-Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {

-

-	return p_normal*radius;

-}

-

-void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-	*r_supports=p_normal*radius;

-	r_amount=1;

-}

-

-bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);

-}

-

-Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	float s = 0.4 * p_mass * radius * radius;

-	return Vector3(s,s,s);

-}

-

-void SphereShapeSW::_setup(real_t p_radius) {

-

-

-	radius=p_radius;

-	configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));

-

-}

-

-void SphereShapeSW::set_data(const Variant& p_data) {

-

-	_setup(p_data);

-}

-

-Variant SphereShapeSW::get_data() const {

-

-	return radius;

-}

-

-SphereShapeSW::SphereShapeSW() {

-

-	radius=0;

-}

-

-

-/********** BOX *************/

-

-

-void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	// no matter the angle, the box is mirrored anyway

-	Vector3 local_normal=p_transform.basis.xform_inv(p_normal);

-

-	float length = local_normal.abs().dot(half_extents);

-	float distance = p_normal.dot( p_transform.origin );

-

-	r_min = distance - length;

-	r_max = distance + length;

-

-

-}

-

-Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {

-

-

-	Vector3 point(

-		(p_normal.x<0) ? -half_extents.x : half_extents.x,

-		(p_normal.y<0) ? -half_extents.y : half_extents.y,

-		(p_normal.z<0) ? -half_extents.z : half_extents.z

-	);

-

-	return point;

-}

-

-void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-	static const int next[3]={1,2,0};

-	static const int next2[3]={2,0,1};

-

-	for (int i=0;i<3;i++) {

-

-		Vector3 axis;

-		axis[i]=1.0;

-		float dot = p_normal.dot( axis );

-		if ( Math::abs( dot ) >  _FACE_IS_VALID_SUPPORT_TRESHOLD ) {

-

-			//Vector3 axis_b;

-

-			bool neg = dot<0;

-			r_amount = 4;

-

-			Vector3 point;

-			point[i]=half_extents[i];

-

-			int i_n=next[i];

-			int i_n2=next2[i];

-

-			static const float sign[4][2]={

-

-				{-1.0, 1.0},

-				{ 1.0, 1.0},

-				{ 1.0,-1.0},

-				{-1.0,-1.0},

-			};

-

-			for (int j=0;j<4;j++) {

-

-				point[i_n]=sign[j][0]*half_extents[i_n];

-				point[i_n2]=sign[j][1]*half_extents[i_n2];

-				r_supports[j]=neg?-point:point;

-

-			}

-

-			if (neg) {

-				SWAP( r_supports[1], r_supports[2] );

-				SWAP( r_supports[0], r_supports[3] );

-			}

-

-			return;

-		}

-

-		r_amount=0;

-

-	}

-

-	for (int i=0;i<3;i++) {

-

-		Vector3 axis;

-		axis[i]=1.0;

-

-		if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {

-

-			r_amount= 2;

-

-			int i_n=next[i];

-			int i_n2=next2[i];

-

-			Vector3 point=half_extents;

-

-			if (p_normal[i_n]<0) {

-				point[i_n]=-point[i_n];

-			}

-			if (p_normal[i_n2]<0) {

-				point[i_n2]=-point[i_n2];

-			}

-

-			r_supports[0] = point;

-			point[i]=-point[i];

-			r_supports[1] = point;

-			return;

-		}

-	}

-	/* USE POINT */

-

-	Vector3 point(

-		(p_normal.x<0) ? -half_extents.x : half_extents.x,

-		(p_normal.y<0) ? -half_extents.y : half_extents.y,

-		(p_normal.z<0) ? -half_extents.z : half_extents.z

-	);

-

-	r_amount=1;

-	r_supports[0]=point;

-}

-

-bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	AABB aabb(-half_extents,half_extents*2.0);

-

-	return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);

-

-}

-

-Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	float lx=half_extents.x;

-	float ly=half_extents.y;

-	float lz=half_extents.z;

-

-	return Vector3(	(p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );

-

-}

-

-void BoxShapeSW::_setup(const Vector3& p_half_extents)  {

-

-	half_extents=p_half_extents.abs();

-

-	configure(AABB(-half_extents,half_extents*2));

-

-

-}

-

-void BoxShapeSW::set_data(const Variant& p_data) {

-

-

-	_setup(p_data);

-}

-

-Variant BoxShapeSW::get_data() const {

-

-	return half_extents;

-}

-

-BoxShapeSW::BoxShapeSW()  {

-

-

-}

-

-

-/********** CAPSULE *************/

-

-

-void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();

-	float h = (n.z > 0) ? height : -height;

-

-	n *= radius;

-	n.z += h * 0.5;

-

-	r_max=p_normal.dot(p_transform.xform(n));

-	r_min=p_normal.dot(p_transform.xform(-n));

-	return;

-

-	n = p_transform.basis.xform(n);

-

-	float distance = p_normal.dot( p_transform.origin );

-	float length = Math::abs(p_normal.dot(n));

-	r_min = distance - length;

-	r_max = distance + length;

-

-	ERR_FAIL_COND( r_max < r_min );

-

-}

-

-Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {

-

-	Vector3 n=p_normal;

-

-	float h = (n.z > 0) ? height : -height;

-

-	n*=radius;

-	n.z += h*0.5;

-	return n;

-}

-

-void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-

-	Vector3 n=p_normal;

-

-	float d = n.z;

-

-	if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {

-

-		// make it flat

-		n.z=0.0;

-		n.normalize();

-		n*=radius;

-

-		r_amount=2;

-		r_supports[0]=n;

-		r_supports[0].z+=height*0.5;

-		r_supports[1]=n;

-		r_supports[1].z-=height*0.5;

-

-	} else {

-

-		float h = (d > 0) ? height : -height;

-

-		n*=radius;

-		n.z += h*0.5;

-		r_amount=1;

-		*r_supports=n;

-

-	}

-

-}

-

-

-bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	Vector3 norm=(p_end-p_begin).normalized();

-	float min_d=1e20;

-

-

-	Vector3 res,n;

-	bool collision=false;

-

-	Vector3 auxres,auxn;

-	bool collided;

-

-	// test against cylinder and spheres :-|

-

-	collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);

-

-	if (collided) {

-		float d=norm.dot(auxres);

-		if (d<min_d) {

-			min_d=d;

-			res=auxres;

-			n=auxn;

-			collision=true;

-		}

-	}

-

-	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);

-

-	if (collided) {

-		float d=norm.dot(auxres);

-		if (d<min_d) {

-			min_d=d;

-			res=auxres;

-			n=auxn;

-			collision=true;

-		}

-	}

-

-	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);

-

-	if (collided) {

-		float d=norm.dot(auxres);

-

-		if (d<min_d) {

-			min_d=d;

-			res=auxres;

-			n=auxn;

-			collision=true;

-		}

-	}

-

-	if (collision) {

-

-		r_result=res;

-		r_normal=n;

-	}

-	return collision;

-}

-

-Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	// use crappy AABB approximation

-	Vector3 extents=get_aabb().size*0.5;

-

-	return Vector3(

-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

-	);

-

-}

-

-

-

-

-void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius)  {

-

-	height=p_height;

-	radius=p_radius;

-	configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));

-

-}

-

-void CapsuleShapeSW::set_data(const Variant& p_data) {

-

-	Dictionary d = p_data;

-	ERR_FAIL_COND(!d.has("radius"));

-	ERR_FAIL_COND(!d.has("height"));

-	_setup(d["height"],d["radius"]);

-

-}

-

-Variant CapsuleShapeSW::get_data() const {

-

-	Dictionary d;

-	d["radius"]=radius;

-	d["height"]=height;

-	return d;

-

-}

-

-

-CapsuleShapeSW::CapsuleShapeSW()  {

-

-	height=radius=0;

-

-}

-

-/********** CONVEX POLYGON *************/

-

-

-void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-

-	int vertex_count=mesh.vertices.size();

-	if (vertex_count==0)

-		return;

-

-	const Vector3 *vrts=&mesh.vertices[0];

-

-	for (int i=0;i<vertex_count;i++) {

-

-		float d=p_normal.dot( p_transform.xform( vrts[i] ) );

-

-		if (i==0 || d > r_max)

-			r_max=d;

-		if (i==0 || d < r_min)

-			r_min=d;

-	}

-}

-

-Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {

-

-	Vector3 n=p_normal;

-

-	int vert_support_idx=-1;

-	float support_max;

-

-	int vertex_count=mesh.vertices.size();

-	if (vertex_count==0)

-		return Vector3();

-

-	const Vector3 *vrts=&mesh.vertices[0];

-

-	for (int i=0;i<vertex_count;i++) {

-

-		float d=n.dot(vrts[i]);

-

-		if (i==0 || d > support_max) {

-			support_max=d;

-			vert_support_idx=i;

-		}

-	}

-

-	return  vrts[vert_support_idx];

-

-}

-

-

-

-void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-	const Geometry::MeshData::Face *faces = mesh.faces.ptr();

-	int fc = mesh.faces.size();

-

-	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();

-	int ec = mesh.edges.size();

-

-	const Vector3 *vertices = mesh.vertices.ptr();

-	int vc = mesh.vertices.size();

-

-	//find vertex first

-	real_t max;

-	int vtx;

-

-	for (int i=0;i<vc;i++) {

-

-		float d=p_normal.dot(vertices[i]);

-

-		if (i==0 || d > max) {

-			max=d;

-			vtx=i;

-		}

-	}

-

-

-	for(int i=0;i<fc;i++) {

-

-		if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {

-

-			int ic = faces[i].indices.size();

-			const int *ind=faces[i].indices.ptr();

-

-			bool valid=false;

-			for(int j=0;j<ic;j++) {

-				if (ind[j]==vtx) {

-					valid=true;

-					break;

-				}

-			}

-

-			if (!valid)

-				continue;

-

-			int m = MIN(p_max,ic);

-			for(int j=0;j<m;j++) {

-

-				r_supports[j]=vertices[ind[j]];

-			}

-			r_amount=m;

-			return;

-		}

-	}

-

-	for(int i=0;i<ec;i++) {

-

-

-		float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);

-		dot=ABS(dot);

-		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {

-

-			r_amount=2;

-			r_supports[0]=vertices[edges[i].a];

-			r_supports[1]=vertices[edges[i].b];

-			return;

-		}

-	}

-

-

-	r_supports[0]=vertices[vtx];

-	r_amount=1;

-}

-

-bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-

-

-	const Geometry::MeshData::Face *faces = mesh.faces.ptr();

-	int fc = mesh.faces.size();

-

-	const Vector3 *vertices = mesh.vertices.ptr();

-	int vc = mesh.vertices.size();

-

-	Vector3 n = p_end-p_begin;

-	float min = 1e20;

-	bool col=false;

-

-	for(int i=0;i<fc;i++) {

-

-		if (faces[i].plane.normal.dot(n) > 0)

-			continue; //opposing face

-

-		int ic = faces[i].indices.size();

-		const int *ind=faces[i].indices.ptr();

-

-		for(int j=1;j<ic-1;j++) {

-

-			Face3 f(vertices[ind[0]],vertices[ind[i]],vertices[ind[i+1]]);

-			Vector3 result;

-			if (f.intersects_segment(p_begin,p_end,&result)) {

-				float d = n.dot(result);

-				if (d<min) {

-					min=d;

-					r_result=result;

-					r_normal=faces[i].plane.normal;

-					col=true;

-				}

-

-				break;

-			}

-

-		}

-	}

-

-	return col;

-

-}

-

-Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	// use crappy AABB approximation

-	Vector3 extents=get_aabb().size*0.5;

-

-	return  Vector3(

-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

-	);

-

-}

-

-void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices)  {

-

-	Error err = QuickHull::build(p_vertices,mesh);

-	AABB _aabb;

-

-	for(int i=0;i<mesh.vertices.size();i++) {

-

-		if (i==0)

-			_aabb.pos=mesh.vertices[i];

-		else

-			_aabb.expand_to(mesh.vertices[i]);

-	}

-

-	configure(_aabb);

-

-

-}

-

-void ConvexPolygonShapeSW::set_data(const Variant& p_data) {

-

-	_setup(p_data);

-

-}

-

-Variant ConvexPolygonShapeSW::get_data() const {

-

-	return mesh.vertices;

-}

-

-

-ConvexPolygonShapeSW::ConvexPolygonShapeSW()  {

-

-

-}

-

-

-/********** FACE POLYGON *************/

-

-

-void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	for (int i=0;i<3;i++) {

-

-		Vector3 v=p_transform.xform(vertex[i]);

-		float d=p_normal.dot(v);

-

-		if (i==0 || d > r_max)

-			r_max=d;

-

-		if (i==0 || d < r_min)

-			r_min=d;

-	}

-}

-

-Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {

-

-

-	Vector3 n=p_normal;

-

-	int vert_support_idx=-1;

-	float support_max;

-

-	for (int i=0;i<3;i++) {

-

-		//float d=n.dot(vertex[i]);

-		float d=p_normal.dot(vertex[i]);

-

-		if (i==0 || d > support_max) {

-			support_max=d;

-			vert_support_idx=i;

-		}

-	}

-

-	return vertex[vert_support_idx];

-}

-

-void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {

-

-	Vector3 n=p_normal;

-

-	/** TEST FACE AS SUPPORT **/

-	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {

-

-		r_amount=3;

-		for (int i=0;i<3;i++) {

-

-			r_supports[i]=vertex[i];

-		}

-		return;

-

-	}

-

-	/** FIND SUPPORT VERTEX **/

-

-	int vert_support_idx=-1;

-	float support_max;

-

-	for (int i=0;i<3;i++) {

-

-		float d=n.dot(vertex[i]);

-

-		if (i==0 || d > support_max) {

-			support_max=d;

-			vert_support_idx=i;

-		}

-	}

-

-	/** TEST EDGES AS SUPPORT **/

-

-	for (int i=0;i<3;i++) {

-

-		int nx=(i+1)%3;

-		//if (i!=vert_support_idx && nx!=vert_support_idx)

-		//	continue;

-

-	// check if edge is valid as a support

-		float dot=(vertex[i]-vertex[nx]).normalized().dot(n);

-		dot=ABS(dot);

-		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {

-

-			r_amount=2;

-			r_supports[0]=vertex[i];

-			r_supports[1]=vertex[nx];

-			return;

-		}

-	}

-

-	r_amount=1;

-	r_supports[0]=vertex[vert_support_idx];

-}

-

-bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-

-	bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);

-	if (c)

-		r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;

-

-	return c;

-}

-

-Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!

-

-}

-

-FaceShapeSW::FaceShapeSW()  {

-

-	configure(AABB());

-

-}

-

-

-

-DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {

-

-

-	DVector<Vector3> rfaces;

-	rfaces.resize(faces.size()*3);

-

-	for(int i=0;i<faces.size();i++) {

-

-		Face f=faces.get(i);

-

-		for(int j=0;j<3;j++) {

-

-			rfaces.set(i*3+j, vertices.get( f.indices[j] ) );

-		}

-	}

-

-	return rfaces;

-}

-

-void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	int count=vertices.size();

-	DVector<Vector3>::Read r=vertices.read();

-	const Vector3 *vptr=r.ptr();

-

-	for (int i=0;i<count;i++) {

-

-		float d=p_normal.dot( p_transform.xform( vptr[i] ) );

-

-		if (i==0 || d > r_max)

-			r_max=d;

-		if (i==0 || d < r_min)

-			r_min=d;

-

-	}

-}

-

-Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {

-

-

-	int count=vertices.size();

-	DVector<Vector3>::Read r=vertices.read();

-	const Vector3 *vptr=r.ptr();

-

-	Vector3 n=p_normal;

-

-	int vert_support_idx=-1;

-	float support_max;

-

-	for (int i=0;i<count;i++) {

-

-		float d=n.dot(vptr[i]);

-

-		if (i==0 || d > support_max) {

-			support_max=d;

-			vert_support_idx=i;

-		}

-	}

-

-

-	return vptr[vert_support_idx];

-

-}

-

-void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {

-

-	const BVH *bvh=&p_params->bvh[p_idx];

-

-

-	//if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)

-	//	return; //test against whole AABB, which isn't very costly

-

-

-	//printf("addr: %p\n",bvh);

-	if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {

-

-		return;

-	}

-

-

-	if (bvh->face_index>=0) {

-

-

-		Vector3 res;

-		Vector3 vertices[3]={

-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],

-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],

-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]

-		};

-

-		if (Geometry::segment_intersects_triangle(

-				p_params->from,

-				p_params->to,

-				vertices[0],

-				vertices[1],

-				vertices[2],

-				&res)) {

-

-

-			float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);

-			//TODO, seems segmen/triangle intersection is broken :(

-			if (d>0 && d<p_params->min_d) {

-

-				p_params->min_d=d;

-				p_params->result=res;

-				p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;

-				p_params->collisions++;

-			}

-

-		}

-

-

-

-	} else {

-

-		if (bvh->left>=0)

-			_cull_segment(bvh->left,p_params);

-		if (bvh->right>=0)

-			_cull_segment(bvh->right,p_params);

-

-

-	}

-}

-

-bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {

-

-	// unlock data

-	DVector<Face>::Read fr=faces.read();

-	DVector<Vector3>::Read vr=vertices.read();

-	DVector<BVH>::Read br=bvh.read();

-

-

-	_SegmentCullParams params;

-	params.from=p_begin;

-	params.to=p_end;

-	params.collisions=0;

-	params.normal=(p_end-p_begin).normalized();

-

-	params.faces=fr.ptr();

-	params.vertices=vr.ptr();

-	params.bvh=br.ptr();

-

-	params.min_d=1e20;

-	// cull

-	_cull_segment(0,&params);

-

-	if (params.collisions>0) {

-

-

-		r_result=params.result;

-		r_normal=params.normal;

-		return true;

-	} else {

-

-		return false;

-	}

-}

-

-void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {

-

-	const BVH* bvh=&p_params->bvh[p_idx];

-

-	if (!p_params->aabb.intersects( bvh->aabb ))

-		return;

-

-	if (bvh->face_index>=0) {

-

-		const Face *f=&p_params->faces[ bvh->face_index ];

-		FaceShapeSW *face=p_params->face;

-		face->normal=f->normal;

-		face->vertex[0]=p_params->vertices[f->indices[0]];

-		face->vertex[1]=p_params->vertices[f->indices[1]];

-		face->vertex[2]=p_params->vertices[f->indices[2]];

-		p_params->callback(p_params->userdata,face);

-

-	} else {

-

-		if (bvh->left>=0) {

-

-			_cull(bvh->left,p_params);

-

-		}

-

-		if (bvh->right>=0) {

-

-			_cull(bvh->right,p_params);

-		}

-

-	}

-}

-

-void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {

-

-	// make matrix local to concave

-

-	AABB local_aabb=p_local_aabb;

-

-	// unlock data

-	DVector<Face>::Read fr=faces.read();

-	DVector<Vector3>::Read vr=vertices.read();

-	DVector<BVH>::Read br=bvh.read();

-

-	FaceShapeSW face; // use this to send in the callback

-

-	_CullParams params;

-	params.aabb=local_aabb;

-	params.face=&face;

-	params.faces=fr.ptr();

-	params.vertices=vr.ptr();

-	params.bvh=br.ptr();

-	params.callback=p_callback;

-	params.userdata=p_userdata;

-

-	// cull

-	_cull(0,&params);

-

-}

-

-Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {

-

-	// use crappy AABB approximation

-	Vector3 extents=get_aabb().size*0.5;

-

-	return Vector3(

-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

-	);

-}

-

-

-struct _VolumeSW_BVH_Element {

-

-	AABB aabb;

-	Vector3 center;

-	int face_index;

-};

-

-struct _VolumeSW_BVH_CompareX {

-

-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

-

-		return a.center.x<b.center.x;

-	}

-};

-

-

-struct _VolumeSW_BVH_CompareY {

-

-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

-

-		return a.center.y<b.center.y;

-	}

-};

-

-struct _VolumeSW_BVH_CompareZ {

-

-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {

-

-		return a.center.z<b.center.z;

-	}

-};

-

-struct _VolumeSW_BVH {

-

-	AABB aabb;

-	_VolumeSW_BVH *left;

-	_VolumeSW_BVH *right;

-

-	int face_index;

-};

-

-

-_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {

-

-	_VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );

-

-	if (p_size==1) {

-		//leaf

-		bvh->aabb=p_elements[0].aabb;

-		bvh->left=NULL;

-		bvh->right=NULL;

-		bvh->face_index=p_elements->face_index;

-		count++;

-		return bvh;

-	} else {

-

-		bvh->face_index=-1;

-	}

-

-	AABB aabb;

-	for(int i=0;i<p_size;i++) {

-

-		if (i==0)

-			aabb=p_elements[i].aabb;

-		else

-			aabb.merge_with(p_elements[i].aabb);

-	}

-	bvh->aabb=aabb;

-	switch(aabb.get_longest_axis_index()) {

-

-		case 0: {

-

-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;

-			sort_x.sort(p_elements,p_size);

-

-		} break;

-		case 1: {

-

-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;

-			sort_y.sort(p_elements,p_size);

-		} break;

-		case 2: {

-

-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;

-			sort_z.sort(p_elements,p_size);

-		} break;

-	}

-

-	int split=p_size/2;

-	bvh->left=_volume_sw_build_bvh(p_elements,split,count);

-	bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);

-

-//	printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);

-	count++;

-	return bvh;

-}

-

-

-void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {

-

-	int idx=p_idx;

-

-

-	p_bvh_array[idx].aabb=p_bvh_tree->aabb;

-	p_bvh_array[idx].face_index=p_bvh_tree->face_index;

-//	printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);

-

-

-	if (p_bvh_tree->left) {

-		p_bvh_array[idx].left=++p_idx;

-		_fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);

-

-	} else {

-

-		p_bvh_array[p_idx].left=-1;

-	}

-

-	if (p_bvh_tree->right) {

-		p_bvh_array[idx].right=++p_idx;

-		_fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);

-

-	} else {

-

-		p_bvh_array[p_idx].right=-1;

-	}

-

-	memdelete(p_bvh_tree);

-

-}

-

-void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {

-

-	int src_face_count=p_faces.size();

-	ERR_FAIL_COND(src_face_count%3);

-	src_face_count/=3;

-

-	DVector<Vector3>::Read r = p_faces.read();

-	const Vector3 * facesr= r.ptr();

-

-#if 0

-	Map<Vector3,int> point_map;

-	List<Face> face_list;

-

-

-	for(int i=0;i<src_face_count;i++) {

-

-		Face3 faceaux;

-

-		for(int j=0;j<3;j++) {

-

-			faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);

-			//faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);

-		}

-

-		ERR_CONTINUE( faceaux.is_degenerate() );

-

-		Face face;

-

-		for(int j=0;j<3;j++) {

-

-

-			Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);

-			if (E) {

-

-				face.indices[j]=E->value();

-			} else {

-

-				face.indices[j]=point_map.size();

-				point_map.insert(faceaux.vertex[j],point_map.size());

-

-			}

-		}

-

-		face_list.push_back(face);

-	}

-

-	vertices.resize( point_map.size() );

-

-	DVector<Vector3>::Write vw = vertices.write();

-	Vector3 *verticesw=vw.ptr();

-

-	AABB _aabb;

-

-	for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {

-

-		if (E==point_map.front()) {

-			_aabb.pos=E->key();

-		} else {

-

-			_aabb.expand_to(E->key());

-		}

-		verticesw[E->value()]=E->key();

-	}

-

-	point_map.clear(); // not needed anymore

-

-	faces.resize(face_list.size());

-	DVector<Face>::Write w = faces.write();

-	Face *facesw=w.ptr();

-

-	int fc=0;

-

-	for( List<Face>::Element *E=face_list.front();E;E=E->next()) {

-

-		facesw[fc++]=E->get();

-	}

-

-	face_list.clear();

-

-

-	DVector<_VolumeSW_BVH_Element> bvh_array;

-	bvh_array.resize( fc );

-

-	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();

-	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();

-

-

-	for(int i=0;i<fc;i++) {

-

-		AABB face_aabb;

-		face_aabb.pos=verticesw[facesw[i].indices[0]];

-		face_aabb.expand_to( verticesw[facesw[i].indices[1]] );

-		face_aabb.expand_to( verticesw[facesw[i].indices[2]] );

-

-		bvh_arrayw[i].face_index=i;

-		bvh_arrayw[i].aabb=face_aabb;

-		bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;

-

-	}

-

-	w=DVector<Face>::Write();

-	vw=DVector<Vector3>::Write();

-

-

-	int count=0;

-	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);

-

-	ERR_FAIL_COND(count==0);

-

-	bvhw=DVector<_VolumeSW_BVH_Element>::Write();

-

-	bvh.resize( count+1 );

-

-	DVector<BVH>::Write bvhw2 = bvh.write();

-	BVH*bvh_arrayw2=bvhw2.ptr();

-

-	int idx=0;

-	_fill_bvh(bvh_tree,bvh_arrayw2,idx);

-

-	set_aabb(_aabb);

-

-#else

-	DVector<_VolumeSW_BVH_Element> bvh_array;

-	bvh_array.resize( src_face_count );

-

-	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();

-	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();

-

-	faces.resize(src_face_count);

-	DVector<Face>::Write w = faces.write();

-	Face *facesw=w.ptr();

-

-	vertices.resize( src_face_count*3 );

-

-	DVector<Vector3>::Write vw = vertices.write();

-	Vector3 *verticesw=vw.ptr();

-

-	AABB _aabb;

-

-

-	for(int i=0;i<src_face_count;i++) {

-

-		Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );

-

-		bvh_arrayw[i].aabb=face.get_aabb();

-		bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;

-		bvh_arrayw[i].face_index=i;

-		facesw[i].indices[0]=i*3+0;

-		facesw[i].indices[1]=i*3+1;

-		facesw[i].indices[2]=i*3+2;

-		facesw[i].normal=face.get_plane().normal;

-		verticesw[i*3+0]=face.vertex[0];

-		verticesw[i*3+1]=face.vertex[1];

-		verticesw[i*3+2]=face.vertex[2];

-		if (i==0)

-			_aabb=bvh_arrayw[i].aabb;

-		else

-			_aabb.merge_with(bvh_arrayw[i].aabb);

-

-	}

-

-	w=DVector<Face>::Write();

-	vw=DVector<Vector3>::Write();

-

-	int count=0;

-	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);

-

-	bvh.resize( count+1 );

-

-	DVector<BVH>::Write bvhw2 = bvh.write();

-	BVH*bvh_arrayw2=bvhw2.ptr();

-

-	int idx=0;

-	_fill_bvh(bvh_tree,bvh_arrayw2,idx);

-

-	configure(_aabb); // this type of shape has no margin

-

-

-#endif

-}

-

-

-void ConcavePolygonShapeSW::set_data(const Variant& p_data) {

-

-

-	_setup(p_data);

-}

-

-Variant ConcavePolygonShapeSW::get_data() const {

-

-	return get_faces();

-}

-

-ConcavePolygonShapeSW::ConcavePolygonShapeSW() {

-

-

-}

-

-

-

-/* HEIGHT MAP SHAPE */

-

-DVector<float> HeightMapShapeSW::get_heights() const {

-

-	return heights;

-}

-int HeightMapShapeSW::get_width() const {

-

-	return width;

-}

-int HeightMapShapeSW::get_depth() const {

-

-	return depth;

-}

-float HeightMapShapeSW::get_cell_size() const {

-

-	return cell_size;

-}

-

-

-void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

-

-	//not very useful, but not very used either

-	p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );

-

-}

-

-Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {

-

-

-	//not very useful, but not very used either

-	return get_aabb().get_support(p_normal);

-

-}

-

-bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {

-

-

-	return false;

-}

-

-

-void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {

-

-

-

-}

-

-

-Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {

-

-

-	// use crappy AABB approximation

-	Vector3 extents=get_aabb().size*0.5;

-

-	return Vector3(

-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),

-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),

-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)

-	);

-}

-

-

-void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {

-

-	heights=p_heights;

-	width=p_width;

-	depth=p_depth;;

-	cell_size=p_cell_size;

-

-	DVector<real_t>::Read r = heights. read();

-

-	AABB aabb;

-

-	for(int i=0;i<depth;i++) {

-

-		for(int j=0;j<width;j++) {

-

-			float h = r[i*width+j];

-

-			Vector3 pos( j*cell_size, h, i*cell_size );

-			if (i==0 || j==0)

-				aabb.pos=pos;

-			else

-				aabb.expand_to(pos);

-

-		}

-	}

-

-

-	configure(aabb);

-}

-

-void HeightMapShapeSW::set_data(const Variant& p_data) {

-

-	ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );

-	Dictionary d=p_data;

-	ERR_FAIL_COND( !d.has("width") );

-	ERR_FAIL_COND( !d.has("depth") );

-	ERR_FAIL_COND( !d.has("cell_size") );

-	ERR_FAIL_COND( !d.has("heights") );

-

-	int width=d["width"];

-	int depth=d["depth"];

-	float cell_size=d["cell_size"];

-	DVector<float> heights=d["heights"];

-

-	ERR_FAIL_COND( width<= 0);

-	ERR_FAIL_COND( depth<= 0);

-	ERR_FAIL_COND( cell_size<= CMP_EPSILON);

-	ERR_FAIL_COND( heights.size() != (width*depth) );

-	_setup(heights, width, depth, cell_size );

-

-}

-

-Variant HeightMapShapeSW::get_data() const {

-

-	ERR_FAIL_V(Variant());

-

-}

-

-HeightMapShapeSW::HeightMapShapeSW() {

-

-	width=0;

-	depth=0;

-	cell_size=0;

-}

-

-

-

+#include "shape_sw.h"
+#include "geometry.h"
+#include "sort.h"
+#include "quick_hull.h"
+#define _POINT_SNAP 0.001953125
+#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002
+#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998
+
+
+void ShapeSW::configure(const AABB& p_aabb) {
+	aabb=p_aabb;
+	configured=true;
+	for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {
+		ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();
+		co->_shape_changed();
+	}
+}
+
+
+Vector3 ShapeSW::get_support(const Vector3& p_normal) const {
+
+	Vector3 res;
+	int amnt;
+	get_supports(p_normal,1,&res,amnt);
+	return res;
+}
+
+void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {
+
+	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+	if (E) {
+		E->get()++;
+	} else {
+		owners[p_owner]=1;
+	}
+}
+
+void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){
+
+	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+	ERR_FAIL_COND(!E);
+	E->get()--;
+	if (E->get()==0) {
+		owners.erase(E);
+	}
+
+}
+
+bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{
+
+	return owners.has(p_owner);
+
+}
+
+const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{
+	return owners;
+}
+
+
+ShapeSW::ShapeSW() {
+
+	custom_bias=0;
+	configured=false;
+}
+
+
+ShapeSW::~ShapeSW() {
+
+	ERR_FAIL_COND(owners.size());
+}
+
+
+
+Plane PlaneShapeSW::get_plane() const {
+
+	return plane;
+}
+
+void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	// gibberish, a plane is infinity
+	r_min=-1e7;
+	r_max=1e7;
+}
+
+Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {
+
+	return p_normal*1e15;
+}
+
+
+bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	bool inters=plane.intersects_segment(p_begin,p_end,&r_result);
+	if(inters)
+		r_normal=plane.normal;
+	return inters;
+}
+
+Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	return Vector3(); //wtf
+}
+
+void PlaneShapeSW::_setup(const Plane& p_plane)  {
+
+	plane=p_plane;
+	configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));
+}
+
+void PlaneShapeSW::set_data(const Variant& p_data) {
+
+	_setup(p_data);
+
+}
+
+Variant PlaneShapeSW::get_data() const {
+
+	return plane;
+}
+
+PlaneShapeSW::PlaneShapeSW()  {
+
+
+}
+
+//
+
+float RayShapeSW::get_length() const {
+
+	return length;
+}
+
+void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	// don't think this will be even used
+	r_min=0;
+	r_max=1;
+}
+
+Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {
+
+	if (p_normal.z>0)
+		return Vector3(0,0,length);
+	else
+		return Vector3(0,0,0);
+}
+
+void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+		r_amount=2;
+		r_supports[0]=Vector3(0,0,0);
+		r_supports[1]=Vector3(0,0,length);
+	} if (p_normal.z>0) {
+		r_amount=1;
+		*r_supports=Vector3(0,0,length);
+	} else {
+		r_amount=1;
+		*r_supports=Vector3(0,0,0);
+	}
+}
+
+
+bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	return false; //simply not possible
+}
+
+Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	return Vector3();
+}
+
+void RayShapeSW::_setup(float p_length)  {
+
+	length=p_length;
+	configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));
+}
+
+void RayShapeSW::set_data(const Variant& p_data) {
+
+	_setup(p_data);
+
+}
+
+Variant RayShapeSW::get_data() const {
+
+	return length;
+}
+
+RayShapeSW::RayShapeSW()  {
+
+	length=1;
+}
+
+
+
+/********** SPHERE *************/
+
+real_t SphereShapeSW::get_radius() const {
+
+	return radius;
+}
+
+void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	float d = p_normal.dot( p_transform.origin );
+
+	// figure out scale at point
+	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
+	float scale = local_normal.length();
+
+	r_min = d - (radius) * scale;
+	r_max = d + (radius) * scale;
+
+}
+
+Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {
+
+	return p_normal*radius;
+}
+
+void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+	*r_supports=p_normal*radius;
+	r_amount=1;
+}
+
+bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);
+}
+
+Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	float s = 0.4 * p_mass * radius * radius;
+	return Vector3(s,s,s);
+}
+
+void SphereShapeSW::_setup(real_t p_radius) {
+
+
+	radius=p_radius;
+	configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));
+
+}
+
+void SphereShapeSW::set_data(const Variant& p_data) {
+
+	_setup(p_data);
+}
+
+Variant SphereShapeSW::get_data() const {
+
+	return radius;
+}
+
+SphereShapeSW::SphereShapeSW() {
+
+	radius=0;
+}
+
+
+/********** BOX *************/
+
+
+void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	// no matter the angle, the box is mirrored anyway
+	Vector3 local_normal=p_transform.basis.xform_inv(p_normal);
+
+	float length = local_normal.abs().dot(half_extents);
+	float distance = p_normal.dot( p_transform.origin );
+
+	r_min = distance - length;
+	r_max = distance + length;
+
+
+}
+
+Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {
+
+
+	Vector3 point(
+		(p_normal.x<0) ? -half_extents.x : half_extents.x,
+		(p_normal.y<0) ? -half_extents.y : half_extents.y,
+		(p_normal.z<0) ? -half_extents.z : half_extents.z
+	);
+
+	return point;
+}
+
+void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+	static const int next[3]={1,2,0};
+	static const int next2[3]={2,0,1};
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis;
+		axis[i]=1.0;
+		float dot = p_normal.dot( axis );
+		if ( Math::abs( dot ) >  _FACE_IS_VALID_SUPPORT_TRESHOLD ) {
+
+			//Vector3 axis_b;
+
+			bool neg = dot<0;
+			r_amount = 4;
+
+			Vector3 point;
+			point[i]=half_extents[i];
+
+			int i_n=next[i];
+			int i_n2=next2[i];
+
+			static const float sign[4][2]={
+
+				{-1.0, 1.0},
+				{ 1.0, 1.0},
+				{ 1.0,-1.0},
+				{-1.0,-1.0},
+			};
+
+			for (int j=0;j<4;j++) {
+
+				point[i_n]=sign[j][0]*half_extents[i_n];
+				point[i_n2]=sign[j][1]*half_extents[i_n2];
+				r_supports[j]=neg?-point:point;
+
+			}
+
+			if (neg) {
+				SWAP( r_supports[1], r_supports[2] );
+				SWAP( r_supports[0], r_supports[3] );
+			}
+
+			return;
+		}
+
+		r_amount=0;
+
+	}
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 axis;
+		axis[i]=1.0;
+
+		if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+			r_amount= 2;
+
+			int i_n=next[i];
+			int i_n2=next2[i];
+
+			Vector3 point=half_extents;
+
+			if (p_normal[i_n]<0) {
+				point[i_n]=-point[i_n];
+			}
+			if (p_normal[i_n2]<0) {
+				point[i_n2]=-point[i_n2];
+			}
+
+			r_supports[0] = point;
+			point[i]=-point[i];
+			r_supports[1] = point;
+			return;
+		}
+	}
+	/* USE POINT */
+
+	Vector3 point(
+		(p_normal.x<0) ? -half_extents.x : half_extents.x,
+		(p_normal.y<0) ? -half_extents.y : half_extents.y,
+		(p_normal.z<0) ? -half_extents.z : half_extents.z
+	);
+
+	r_amount=1;
+	r_supports[0]=point;
+}
+
+bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	AABB aabb(-half_extents,half_extents*2.0);
+
+	return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);
+
+}
+
+Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	float lx=half_extents.x;
+	float ly=half_extents.y;
+	float lz=half_extents.z;
+
+	return Vector3(	(p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );
+
+}
+
+void BoxShapeSW::_setup(const Vector3& p_half_extents)  {
+
+	half_extents=p_half_extents.abs();
+
+	configure(AABB(-half_extents,half_extents*2));
+
+
+}
+
+void BoxShapeSW::set_data(const Variant& p_data) {
+
+
+	_setup(p_data);
+}
+
+Variant BoxShapeSW::get_data() const {
+
+	return half_extents;
+}
+
+BoxShapeSW::BoxShapeSW()  {
+
+
+}
+
+
+/********** CAPSULE *************/
+
+
+void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();
+	float h = (n.z > 0) ? height : -height;
+
+	n *= radius;
+	n.z += h * 0.5;
+
+	r_max=p_normal.dot(p_transform.xform(n));
+	r_min=p_normal.dot(p_transform.xform(-n));
+	return;
+
+	n = p_transform.basis.xform(n);
+
+	float distance = p_normal.dot( p_transform.origin );
+	float length = Math::abs(p_normal.dot(n));
+	r_min = distance - length;
+	r_max = distance + length;
+
+	ERR_FAIL_COND( r_max < r_min );
+
+}
+
+Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {
+
+	Vector3 n=p_normal;
+
+	float h = (n.z > 0) ? height : -height;
+
+	n*=radius;
+	n.z += h*0.5;
+	return n;
+}
+
+void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+
+	Vector3 n=p_normal;
+
+	float d = n.z;
+
+	if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
+
+		// make it flat
+		n.z=0.0;
+		n.normalize();
+		n*=radius;
+
+		r_amount=2;
+		r_supports[0]=n;
+		r_supports[0].z+=height*0.5;
+		r_supports[1]=n;
+		r_supports[1].z-=height*0.5;
+
+	} else {
+
+		float h = (d > 0) ? height : -height;
+
+		n*=radius;
+		n.z += h*0.5;
+		r_amount=1;
+		*r_supports=n;
+
+	}
+
+}
+
+
+bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	Vector3 norm=(p_end-p_begin).normalized();
+	float min_d=1e20;
+
+
+	Vector3 res,n;
+	bool collision=false;
+
+	Vector3 auxres,auxn;
+	bool collided;
+
+	// test against cylinder and spheres :-|
+
+	collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);
+
+	if (collided) {
+		float d=norm.dot(auxres);
+		if (d<min_d) {
+			min_d=d;
+			res=auxres;
+			n=auxn;
+			collision=true;
+		}
+	}
+
+	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);
+
+	if (collided) {
+		float d=norm.dot(auxres);
+		if (d<min_d) {
+			min_d=d;
+			res=auxres;
+			n=auxn;
+			collision=true;
+		}
+	}
+
+	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);
+
+	if (collided) {
+		float d=norm.dot(auxres);
+
+		if (d<min_d) {
+			min_d=d;
+			res=auxres;
+			n=auxn;
+			collision=true;
+		}
+	}
+
+	if (collision) {
+
+		r_result=res;
+		r_normal=n;
+	}
+	return collision;
+}
+
+Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	// use crappy AABB approximation
+	Vector3 extents=get_aabb().size*0.5;
+
+	return Vector3(
+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+	);
+
+}
+
+
+
+
+void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius)  {
+
+	height=p_height;
+	radius=p_radius;
+	configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));
+
+}
+
+void CapsuleShapeSW::set_data(const Variant& p_data) {
+
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("radius"));
+	ERR_FAIL_COND(!d.has("height"));
+	_setup(d["height"],d["radius"]);
+
+}
+
+Variant CapsuleShapeSW::get_data() const {
+
+	Dictionary d;
+	d["radius"]=radius;
+	d["height"]=height;
+	return d;
+
+}
+
+
+CapsuleShapeSW::CapsuleShapeSW()  {
+
+	height=radius=0;
+
+}
+
+/********** CONVEX POLYGON *************/
+
+
+void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+
+	int vertex_count=mesh.vertices.size();
+	if (vertex_count==0)
+		return;
+
+	const Vector3 *vrts=&mesh.vertices[0];
+
+	for (int i=0;i<vertex_count;i++) {
+
+		float d=p_normal.dot( p_transform.xform( vrts[i] ) );
+
+		if (i==0 || d > r_max)
+			r_max=d;
+		if (i==0 || d < r_min)
+			r_min=d;
+	}
+}
+
+Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {
+
+	Vector3 n=p_normal;
+
+	int vert_support_idx=-1;
+	float support_max;
+
+	int vertex_count=mesh.vertices.size();
+	if (vertex_count==0)
+		return Vector3();
+
+	const Vector3 *vrts=&mesh.vertices[0];
+
+	for (int i=0;i<vertex_count;i++) {
+
+		float d=n.dot(vrts[i]);
+
+		if (i==0 || d > support_max) {
+			support_max=d;
+			vert_support_idx=i;
+		}
+	}
+
+	return  vrts[vert_support_idx];
+
+}
+
+
+
+void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int ec = mesh.edges.size();
+
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vc = mesh.vertices.size();
+
+	//find vertex first
+	real_t max;
+	int vtx;
+
+	for (int i=0;i<vc;i++) {
+
+		float d=p_normal.dot(vertices[i]);
+
+		if (i==0 || d > max) {
+			max=d;
+			vtx=i;
+		}
+	}
+
+
+	for(int i=0;i<fc;i++) {
+
+		if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {
+
+			int ic = faces[i].indices.size();
+			const int *ind=faces[i].indices.ptr();
+
+			bool valid=false;
+			for(int j=0;j<ic;j++) {
+				if (ind[j]==vtx) {
+					valid=true;
+					break;
+				}
+			}
+
+			if (!valid)
+				continue;
+
+			int m = MIN(p_max,ic);
+			for(int j=0;j<m;j++) {
+
+				r_supports[j]=vertices[ind[j]];
+			}
+			r_amount=m;
+			return;
+		}
+	}
+
+	for(int i=0;i<ec;i++) {
+
+
+		float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);
+		dot=ABS(dot);
+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {
+
+			r_amount=2;
+			r_supports[0]=vertices[edges[i].a];
+			r_supports[1]=vertices[edges[i].b];
+			return;
+		}
+	}
+
+
+	r_supports[0]=vertices[vtx];
+	r_amount=1;
+}
+
+bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+
+	const Vector3 *vertices = mesh.vertices.ptr();
+	int vc = mesh.vertices.size();
+
+	Vector3 n = p_end-p_begin;
+	float min = 1e20;
+	bool col=false;
+
+	for(int i=0;i<fc;i++) {
+
+		if (faces[i].plane.normal.dot(n) > 0)
+			continue; //opposing face
+
+		int ic = faces[i].indices.size();
+		const int *ind=faces[i].indices.ptr();
+
+		for(int j=1;j<ic-1;j++) {
+
+			Face3 f(vertices[ind[0]],vertices[ind[j]],vertices[ind[j+1]]);
+			Vector3 result;
+			if (f.intersects_segment(p_begin,p_end,&result)) {
+				float d = n.dot(result);
+				if (d<min) {
+					min=d;
+					r_result=result;
+					r_normal=faces[i].plane.normal;
+					col=true;
+				}
+
+				break;
+			}
+
+		}
+	}
+
+	return col;
+
+}
+
+Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	// use crappy AABB approximation
+	Vector3 extents=get_aabb().size*0.5;
+
+	return  Vector3(
+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+	);
+
+}
+
+void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices)  {
+
+	Error err = QuickHull::build(p_vertices,mesh);
+	AABB _aabb;
+
+	for(int i=0;i<mesh.vertices.size();i++) {
+
+		if (i==0)
+			_aabb.pos=mesh.vertices[i];
+		else
+			_aabb.expand_to(mesh.vertices[i]);
+	}
+
+	configure(_aabb);
+
+
+}
+
+void ConvexPolygonShapeSW::set_data(const Variant& p_data) {
+
+	_setup(p_data);
+
+}
+
+Variant ConvexPolygonShapeSW::get_data() const {
+
+	return mesh.vertices;
+}
+
+
+ConvexPolygonShapeSW::ConvexPolygonShapeSW()  {
+
+
+}
+
+
+/********** FACE POLYGON *************/
+
+
+void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	for (int i=0;i<3;i++) {
+
+		Vector3 v=p_transform.xform(vertex[i]);
+		float d=p_normal.dot(v);
+
+		if (i==0 || d > r_max)
+			r_max=d;
+
+		if (i==0 || d < r_min)
+			r_min=d;
+	}
+}
+
+Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {
+
+
+	Vector3 n=p_normal;
+
+	int vert_support_idx=-1;
+	float support_max;
+
+	for (int i=0;i<3;i++) {
+
+		//float d=n.dot(vertex[i]);
+		float d=p_normal.dot(vertex[i]);
+
+		if (i==0 || d > support_max) {
+			support_max=d;
+			vert_support_idx=i;
+		}
+	}
+
+	return vertex[vert_support_idx];
+}
+
+void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+	Vector3 n=p_normal;
+
+	/** TEST FACE AS SUPPORT **/
+	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
+
+		r_amount=3;
+		for (int i=0;i<3;i++) {
+
+			r_supports[i]=vertex[i];
+		}
+		return;
+
+	}
+
+	/** FIND SUPPORT VERTEX **/
+
+	int vert_support_idx=-1;
+	float support_max;
+
+	for (int i=0;i<3;i++) {
+
+		float d=n.dot(vertex[i]);
+
+		if (i==0 || d > support_max) {
+			support_max=d;
+			vert_support_idx=i;
+		}
+	}
+
+	/** TEST EDGES AS SUPPORT **/
+
+	for (int i=0;i<3;i++) {
+
+		int nx=(i+1)%3;
+		//if (i!=vert_support_idx && nx!=vert_support_idx)
+		//	continue;
+
+	// check if edge is valid as a support
+		float dot=(vertex[i]-vertex[nx]).normalized().dot(n);
+		dot=ABS(dot);
+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+			r_amount=2;
+			r_supports[0]=vertex[i];
+			r_supports[1]=vertex[nx];
+			return;
+		}
+	}
+
+	r_amount=1;
+	r_supports[0]=vertex[vert_support_idx];
+}
+
+bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+
+	bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);
+	if (c)
+		r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;
+
+	return c;
+}
+
+Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
+
+}
+
+FaceShapeSW::FaceShapeSW()  {
+
+	configure(AABB());
+
+}
+
+
+
+DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
+
+
+	DVector<Vector3> rfaces;
+	rfaces.resize(faces.size()*3);
+
+	for(int i=0;i<faces.size();i++) {
+
+		Face f=faces.get(i);
+
+		for(int j=0;j<3;j++) {
+
+			rfaces.set(i*3+j, vertices.get( f.indices[j] ) );
+		}
+	}
+
+	return rfaces;
+}
+
+void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	int count=vertices.size();
+	DVector<Vector3>::Read r=vertices.read();
+	const Vector3 *vptr=r.ptr();
+
+	for (int i=0;i<count;i++) {
+
+		float d=p_normal.dot( p_transform.xform( vptr[i] ) );
+
+		if (i==0 || d > r_max)
+			r_max=d;
+		if (i==0 || d < r_min)
+			r_min=d;
+
+	}
+}
+
+Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {
+
+
+	int count=vertices.size();
+	DVector<Vector3>::Read r=vertices.read();
+	const Vector3 *vptr=r.ptr();
+
+	Vector3 n=p_normal;
+
+	int vert_support_idx=-1;
+	float support_max;
+
+	for (int i=0;i<count;i++) {
+
+		float d=n.dot(vptr[i]);
+
+		if (i==0 || d > support_max) {
+			support_max=d;
+			vert_support_idx=i;
+		}
+	}
+
+
+	return vptr[vert_support_idx];
+
+}
+
+void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {
+
+	const BVH *bvh=&p_params->bvh[p_idx];
+
+
+	//if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
+	//	return; //test against whole AABB, which isn't very costly
+
+
+	//printf("addr: %p\n",bvh);
+	if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {
+
+		return;
+	}
+
+
+	if (bvh->face_index>=0) {
+
+
+		Vector3 res;
+		Vector3 vertices[3]={
+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],
+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],
+			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]
+		};
+
+		if (Geometry::segment_intersects_triangle(
+				p_params->from,
+				p_params->to,
+				vertices[0],
+				vertices[1],
+				vertices[2],
+				&res)) {
+
+
+			float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);
+			//TODO, seems segmen/triangle intersection is broken :(
+			if (d>0 && d<p_params->min_d) {
+
+				p_params->min_d=d;
+				p_params->result=res;
+				p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;
+				p_params->collisions++;
+			}
+
+		}
+
+
+
+	} else {
+
+		if (bvh->left>=0)
+			_cull_segment(bvh->left,p_params);
+		if (bvh->right>=0)
+			_cull_segment(bvh->right,p_params);
+
+
+	}
+}
+
+bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+	// unlock data
+	DVector<Face>::Read fr=faces.read();
+	DVector<Vector3>::Read vr=vertices.read();
+	DVector<BVH>::Read br=bvh.read();
+
+
+	_SegmentCullParams params;
+	params.from=p_begin;
+	params.to=p_end;
+	params.collisions=0;
+	params.normal=(p_end-p_begin).normalized();
+
+	params.faces=fr.ptr();
+	params.vertices=vr.ptr();
+	params.bvh=br.ptr();
+
+	params.min_d=1e20;
+	// cull
+	_cull_segment(0,&params);
+
+	if (params.collisions>0) {
+
+
+		r_result=params.result;
+		r_normal=params.normal;
+		return true;
+	} else {
+
+		return false;
+	}
+}
+
+void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {
+
+	const BVH* bvh=&p_params->bvh[p_idx];
+
+	if (!p_params->aabb.intersects( bvh->aabb ))
+		return;
+
+	if (bvh->face_index>=0) {
+
+		const Face *f=&p_params->faces[ bvh->face_index ];
+		FaceShapeSW *face=p_params->face;
+		face->normal=f->normal;
+		face->vertex[0]=p_params->vertices[f->indices[0]];
+		face->vertex[1]=p_params->vertices[f->indices[1]];
+		face->vertex[2]=p_params->vertices[f->indices[2]];
+		p_params->callback(p_params->userdata,face);
+
+	} else {
+
+		if (bvh->left>=0) {
+
+			_cull(bvh->left,p_params);
+
+		}
+
+		if (bvh->right>=0) {
+
+			_cull(bvh->right,p_params);
+		}
+
+	}
+}
+
+void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+	// make matrix local to concave
+
+	AABB local_aabb=p_local_aabb;
+
+	// unlock data
+	DVector<Face>::Read fr=faces.read();
+	DVector<Vector3>::Read vr=vertices.read();
+	DVector<BVH>::Read br=bvh.read();
+
+	FaceShapeSW face; // use this to send in the callback
+
+	_CullParams params;
+	params.aabb=local_aabb;
+	params.face=&face;
+	params.faces=fr.ptr();
+	params.vertices=vr.ptr();
+	params.bvh=br.ptr();
+	params.callback=p_callback;
+	params.userdata=p_userdata;
+
+	// cull
+	_cull(0,&params);
+
+}
+
+Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {
+
+	// use crappy AABB approximation
+	Vector3 extents=get_aabb().size*0.5;
+
+	return Vector3(
+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+	);
+}
+
+
+struct _VolumeSW_BVH_Element {
+
+	AABB aabb;
+	Vector3 center;
+	int face_index;
+};
+
+struct _VolumeSW_BVH_CompareX {
+
+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+		return a.center.x<b.center.x;
+	}
+};
+
+
+struct _VolumeSW_BVH_CompareY {
+
+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+		return a.center.y<b.center.y;
+	}
+};
+
+struct _VolumeSW_BVH_CompareZ {
+
+	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+		return a.center.z<b.center.z;
+	}
+};
+
+struct _VolumeSW_BVH {
+
+	AABB aabb;
+	_VolumeSW_BVH *left;
+	_VolumeSW_BVH *right;
+
+	int face_index;
+};
+
+
+_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {
+
+	_VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );
+
+	if (p_size==1) {
+		//leaf
+		bvh->aabb=p_elements[0].aabb;
+		bvh->left=NULL;
+		bvh->right=NULL;
+		bvh->face_index=p_elements->face_index;
+		count++;
+		return bvh;
+	} else {
+
+		bvh->face_index=-1;
+	}
+
+	AABB aabb;
+	for(int i=0;i<p_size;i++) {
+
+		if (i==0)
+			aabb=p_elements[i].aabb;
+		else
+			aabb.merge_with(p_elements[i].aabb);
+	}
+	bvh->aabb=aabb;
+	switch(aabb.get_longest_axis_index()) {
+
+		case 0: {
+
+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;
+			sort_x.sort(p_elements,p_size);
+
+		} break;
+		case 1: {
+
+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;
+			sort_y.sort(p_elements,p_size);
+		} break;
+		case 2: {
+
+			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;
+			sort_z.sort(p_elements,p_size);
+		} break;
+	}
+
+	int split=p_size/2;
+	bvh->left=_volume_sw_build_bvh(p_elements,split,count);
+	bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);
+
+//	printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
+	count++;
+	return bvh;
+}
+
+
+void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {
+
+	int idx=p_idx;
+
+
+	p_bvh_array[idx].aabb=p_bvh_tree->aabb;
+	p_bvh_array[idx].face_index=p_bvh_tree->face_index;
+//	printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
+
+
+	if (p_bvh_tree->left) {
+		p_bvh_array[idx].left=++p_idx;
+		_fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);
+
+	} else {
+
+		p_bvh_array[p_idx].left=-1;
+	}
+
+	if (p_bvh_tree->right) {
+		p_bvh_array[idx].right=++p_idx;
+		_fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);
+
+	} else {
+
+		p_bvh_array[p_idx].right=-1;
+	}
+
+	memdelete(p_bvh_tree);
+
+}
+
+void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {
+
+	int src_face_count=p_faces.size();
+	ERR_FAIL_COND(src_face_count%3);
+	src_face_count/=3;
+
+	DVector<Vector3>::Read r = p_faces.read();
+	const Vector3 * facesr= r.ptr();
+
+#if 0
+	Map<Vector3,int> point_map;
+	List<Face> face_list;
+
+
+	for(int i=0;i<src_face_count;i++) {
+
+		Face3 faceaux;
+
+		for(int j=0;j<3;j++) {
+
+			faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);
+			//faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);
+		}
+
+		ERR_CONTINUE( faceaux.is_degenerate() );
+
+		Face face;
+
+		for(int j=0;j<3;j++) {
+
+
+			Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);
+			if (E) {
+
+				face.indices[j]=E->value();
+			} else {
+
+				face.indices[j]=point_map.size();
+				point_map.insert(faceaux.vertex[j],point_map.size());
+
+			}
+		}
+
+		face_list.push_back(face);
+	}
+
+	vertices.resize( point_map.size() );
+
+	DVector<Vector3>::Write vw = vertices.write();
+	Vector3 *verticesw=vw.ptr();
+
+	AABB _aabb;
+
+	for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {
+
+		if (E==point_map.front()) {
+			_aabb.pos=E->key();
+		} else {
+
+			_aabb.expand_to(E->key());
+		}
+		verticesw[E->value()]=E->key();
+	}
+
+	point_map.clear(); // not needed anymore
+
+	faces.resize(face_list.size());
+	DVector<Face>::Write w = faces.write();
+	Face *facesw=w.ptr();
+
+	int fc=0;
+
+	for( List<Face>::Element *E=face_list.front();E;E=E->next()) {
+
+		facesw[fc++]=E->get();
+	}
+
+	face_list.clear();
+
+
+	DVector<_VolumeSW_BVH_Element> bvh_array;
+	bvh_array.resize( fc );
+
+	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
+	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
+
+
+	for(int i=0;i<fc;i++) {
+
+		AABB face_aabb;
+		face_aabb.pos=verticesw[facesw[i].indices[0]];
+		face_aabb.expand_to( verticesw[facesw[i].indices[1]] );
+		face_aabb.expand_to( verticesw[facesw[i].indices[2]] );
+
+		bvh_arrayw[i].face_index=i;
+		bvh_arrayw[i].aabb=face_aabb;
+		bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;
+
+	}
+
+	w=DVector<Face>::Write();
+	vw=DVector<Vector3>::Write();
+
+
+	int count=0;
+	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);
+
+	ERR_FAIL_COND(count==0);
+
+	bvhw=DVector<_VolumeSW_BVH_Element>::Write();
+
+	bvh.resize( count+1 );
+
+	DVector<BVH>::Write bvhw2 = bvh.write();
+	BVH*bvh_arrayw2=bvhw2.ptr();
+
+	int idx=0;
+	_fill_bvh(bvh_tree,bvh_arrayw2,idx);
+
+	set_aabb(_aabb);
+
+#else
+	DVector<_VolumeSW_BVH_Element> bvh_array;
+	bvh_array.resize( src_face_count );
+
+	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
+	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
+
+	faces.resize(src_face_count);
+	DVector<Face>::Write w = faces.write();
+	Face *facesw=w.ptr();
+
+	vertices.resize( src_face_count*3 );
+
+	DVector<Vector3>::Write vw = vertices.write();
+	Vector3 *verticesw=vw.ptr();
+
+	AABB _aabb;
+
+
+	for(int i=0;i<src_face_count;i++) {
+
+		Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );
+
+		bvh_arrayw[i].aabb=face.get_aabb();
+		bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;
+		bvh_arrayw[i].face_index=i;
+		facesw[i].indices[0]=i*3+0;
+		facesw[i].indices[1]=i*3+1;
+		facesw[i].indices[2]=i*3+2;
+		facesw[i].normal=face.get_plane().normal;
+		verticesw[i*3+0]=face.vertex[0];
+		verticesw[i*3+1]=face.vertex[1];
+		verticesw[i*3+2]=face.vertex[2];
+		if (i==0)
+			_aabb=bvh_arrayw[i].aabb;
+		else
+			_aabb.merge_with(bvh_arrayw[i].aabb);
+
+	}
+
+	w=DVector<Face>::Write();
+	vw=DVector<Vector3>::Write();
+
+	int count=0;
+	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);
+
+	bvh.resize( count+1 );
+
+	DVector<BVH>::Write bvhw2 = bvh.write();
+	BVH*bvh_arrayw2=bvhw2.ptr();
+
+	int idx=0;
+	_fill_bvh(bvh_tree,bvh_arrayw2,idx);
+
+	configure(_aabb); // this type of shape has no margin
+
+
+#endif
+}
+
+
+void ConcavePolygonShapeSW::set_data(const Variant& p_data) {
+
+
+	_setup(p_data);
+}
+
+Variant ConcavePolygonShapeSW::get_data() const {
+
+	return get_faces();
+}
+
+ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
+
+
+}
+
+
+
+/* HEIGHT MAP SHAPE */
+
+DVector<float> HeightMapShapeSW::get_heights() const {
+
+	return heights;
+}
+int HeightMapShapeSW::get_width() const {
+
+	return width;
+}
+int HeightMapShapeSW::get_depth() const {
+
+	return depth;
+}
+float HeightMapShapeSW::get_cell_size() const {
+
+	return cell_size;
+}
+
+
+void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+	//not very useful, but not very used either
+	p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );
+
+}
+
+Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {
+
+
+	//not very useful, but not very used either
+	return get_aabb().get_support(p_normal);
+
+}
+
+bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {
+
+
+	return false;
+}
+
+
+void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+
+
+}
+
+
+Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {
+
+
+	// use crappy AABB approximation
+	Vector3 extents=get_aabb().size*0.5;
+
+	return Vector3(
+		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+	);
+}
+
+
+void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {
+
+	heights=p_heights;
+	width=p_width;
+	depth=p_depth;;
+	cell_size=p_cell_size;
+
+	DVector<real_t>::Read r = heights. read();
+
+	AABB aabb;
+
+	for(int i=0;i<depth;i++) {
+
+		for(int j=0;j<width;j++) {
+
+			float h = r[i*width+j];
+
+			Vector3 pos( j*cell_size, h, i*cell_size );
+			if (i==0 || j==0)
+				aabb.pos=pos;
+			else
+				aabb.expand_to(pos);
+
+		}
+	}
+
+
+	configure(aabb);
+}
+
+void HeightMapShapeSW::set_data(const Variant& p_data) {
+
+	ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );
+	Dictionary d=p_data;
+	ERR_FAIL_COND( !d.has("width") );
+	ERR_FAIL_COND( !d.has("depth") );
+	ERR_FAIL_COND( !d.has("cell_size") );
+	ERR_FAIL_COND( !d.has("heights") );
+
+	int width=d["width"];
+	int depth=d["depth"];
+	float cell_size=d["cell_size"];
+	DVector<float> heights=d["heights"];
+
+	ERR_FAIL_COND( width<= 0);
+	ERR_FAIL_COND( depth<= 0);
+	ERR_FAIL_COND( cell_size<= CMP_EPSILON);
+	ERR_FAIL_COND( heights.size() != (width*depth) );
+	_setup(heights, width, depth, cell_size );
+
+}
+
+Variant HeightMapShapeSW::get_data() const {
+
+	ERR_FAIL_V(Variant());
+
+}
+
+HeightMapShapeSW::HeightMapShapeSW() {
+
+	width=0;
+	depth=0;
+	cell_size=0;
+}
+
+
+
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 69c82519dd..070bc5e062 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -85,6 +85,7 @@ void PhysicsDirectBodyState::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform);
 
 	ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force);
+	ObjectTypeDB::bind_method(_MD("apply_impulse","pos","j"),&PhysicsDirectBodyState::apply_impulse);
 
 	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state);
 	ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index da51dbc8e1..955caffe5b 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -58,6 +58,7 @@ public:
 	virtual Transform get_transform() const=0;
 
 	virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0;
+	virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j)=0;
 
 	virtual void set_sleep_state(bool p_enable)=0;
 	virtual bool is_sleeping() const=0;
diff --git a/servers/visual/rasterizer.h b/servers/visual/rasterizer.h
index 2b02a81a44..77d4da81d9 100644
--- a/servers/visual/rasterizer.h
+++ b/servers/visual/rasterizer.h
@@ -507,6 +507,7 @@ public:
 		float octree_lattice_size;
 		float octree_lattice_divide;
 		float texture_multiplier;
+		float lightmap_multiplier;
 		int octree_steps;
 		Vector2 octree_tex_pixel_size;
 	};
@@ -520,6 +521,7 @@ public:
 		Vector<float> morph_values;
 		BakedLightData *baked_light;
 		Transform *baked_light_octree_xform;
+		int baked_lightmap_id;
 		bool mirror :8;
 		bool depth_scale :8;
 		bool billboard :8;
diff --git a/servers/visual/shader_language.cpp b/servers/visual/shader_language.cpp
index 16a725010d..489b5c3771 100644
--- a/servers/visual/shader_language.cpp
+++ b/servers/visual/shader_language.cpp
@@ -1005,6 +1005,11 @@ const ShaderLanguage::OperatorDef ShaderLanguage::operator_defs[]={
 
 const ShaderLanguage::BuiltinsDef ShaderLanguage::vertex_builtins_defs[]={
 
+	{ "SRC_VERTEX", TYPE_VEC3},
+	{ "SRC_NORMAL", TYPE_VEC3},
+	{ "SRC_TANGENT", TYPE_VEC3},
+	{ "SRC_BINORMALF", TYPE_FLOAT},
+
 	{ "VERTEX", TYPE_VEC3},
 	{ "NORMAL", TYPE_VEC3},
 	{ "TANGENT", TYPE_VEC3},
@@ -1023,6 +1028,7 @@ const ShaderLanguage::BuiltinsDef ShaderLanguage::vertex_builtins_defs[]={
 	{ "WORLD_MATRIX", TYPE_MAT4},
 	{ "INV_CAMERA_MATRIX", TYPE_MAT4},
 	{ "PROJECTION_MATRIX", TYPE_MAT4},
+	{ "MODELVIEW_MATRIX", TYPE_MAT4},
 	{ "INSTANCE_ID", TYPE_FLOAT},
 	{ "TIME", TYPE_FLOAT},
 	{ NULL, TYPE_VOID},
diff --git a/servers/visual/visual_server_raster.cpp b/servers/visual/visual_server_raster.cpp
index 7c1f03b71b..66862ece65 100644
--- a/servers/visual/visual_server_raster.cpp
+++ b/servers/visual/visual_server_raster.cpp
@@ -148,7 +148,7 @@ String VisualServerRaster::shader_get_fragment_code(RID p_shader) const{
 
 String VisualServerRaster::shader_get_light_code(RID p_shader) const{
 
-	return rasterizer->shader_get_fragment_code(p_shader);
+	return rasterizer->shader_get_light_code(p_shader);
 }
 
 void VisualServerRaster::shader_get_param_list(RID p_shader, List<PropertyInfo> *p_param_list) const {
@@ -1038,6 +1038,7 @@ RID VisualServerRaster::baked_light_create() {
 	baked_light->data.octree_lattice_size=0;
 	baked_light->data.octree_lattice_divide=0;
 	baked_light->data.octree_steps=1;
+	baked_light->data.lightmap_multiplier=1.0;
 
 	return baked_light_owner.make_rid( baked_light );
 
@@ -1063,6 +1064,26 @@ VisualServer::BakedLightMode VisualServerRaster::baked_light_get_mode(RID p_bake
 
 }
 
+void VisualServerRaster::baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier) {
+
+	VS_CHANGED;
+	BakedLight *baked_light = baked_light_owner.get(p_baked_light);
+	ERR_FAIL_COND(!baked_light);
+
+	baked_light->data.lightmap_multiplier=p_multiplier;
+
+}
+
+float VisualServerRaster::baked_light_get_lightmap_multiplier(RID p_baked_light) const{
+
+	const BakedLight *baked_light = baked_light_owner.get(p_baked_light);
+	ERR_FAIL_COND_V(!baked_light,0);
+
+	return baked_light->data.lightmap_multiplier;
+
+}
+
+
 void VisualServerRaster::baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree){
 
 	VS_CHANGED;
@@ -2568,7 +2589,7 @@ void VisualServerRaster::instance_geometry_set_baked_light_texture_index(RID p_i
 	Instance *instance = instance_owner.get( p_instance );
 	ERR_FAIL_COND( !instance );
 
-	instance->lightmap_texture_index=p_tex_id;
+	instance->data.baked_lightmap_id=p_tex_id;
 
 
 }
@@ -2577,7 +2598,7 @@ int VisualServerRaster::instance_geometry_get_baked_light_texture_index(RID p_in
 	const Instance *instance = instance_owner.get( p_instance );
 	ERR_FAIL_COND_V( !instance,0 );
 
-	return instance->lightmap_texture_index;
+	return instance->data.baked_lightmap_id;
 
 }
 
diff --git a/servers/visual/visual_server_raster.h b/servers/visual/visual_server_raster.h
index 0368780bfb..2620225cc2 100644
--- a/servers/visual/visual_server_raster.h
+++ b/servers/visual/visual_server_raster.h
@@ -157,7 +157,7 @@ class VisualServerRaster : public VisualServer {
 		float draw_range_begin;
 		float draw_range_end;
 		float extra_margin;
-		int lightmap_texture_index;
+
 
 
 		Rasterizer::InstanceData data;
@@ -267,6 +267,7 @@ class VisualServerRaster : public VisualServer {
 			data.billboard_y=false;
 			data.baked_light=NULL;
 			data.baked_light_octree_xform=NULL;
+			data.baked_lightmap_id=-1;
 			version=1;
 			room_info=NULL;
 			room=NULL;
@@ -278,7 +279,7 @@ class VisualServerRaster : public VisualServer {
 			draw_range_end=0;
 			extra_margin=0;
 			visible_in_all_rooms=false;
-			lightmap_texture_index=-1;
+
 			baked_light=NULL;
 			baked_light_info=NULL;
 			BLE=NULL;
@@ -942,6 +943,9 @@ public:
 	virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree);
 	virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const;
 
+	virtual void baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier);
+	virtual float baked_light_get_lightmap_multiplier(RID p_baked_light) const;
+
 	virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id);
 	virtual void baked_light_clear_lightmaps(RID p_baked_light);
 
diff --git a/servers/visual/visual_server_wrap_mt.h b/servers/visual/visual_server_wrap_mt.h
index d577ca0c59..f1ba4c453b 100644
--- a/servers/visual/visual_server_wrap_mt.h
+++ b/servers/visual/visual_server_wrap_mt.h
@@ -909,6 +909,8 @@ public:
 	FUNC2(baked_light_set_octree,RID,DVector<uint8_t>);
 	FUNC1RC(DVector<uint8_t>,baked_light_get_octree,RID);
 
+	FUNC2(baked_light_set_lightmap_multiplier,RID,float);
+	FUNC1RC(float,baked_light_get_lightmap_multiplier,RID);
 
 	FUNC3(baked_light_add_lightmap,RID,RID,int);
 	FUNC1(baked_light_clear_lightmaps,RID);
diff --git a/servers/visual_server.h b/servers/visual_server.h
index 9cad173903..7d7b10bed2 100644
--- a/servers/visual_server.h
+++ b/servers/visual_server.h
@@ -585,6 +585,9 @@ public:
 	virtual void baked_light_set_octree(RID p_baked_light,const DVector<uint8_t> p_octree)=0;
 	virtual DVector<uint8_t> baked_light_get_octree(RID p_baked_light) const=0;
 
+	virtual void baked_light_set_lightmap_multiplier(RID p_baked_light,float p_multiplier)=0;
+	virtual float baked_light_get_lightmap_multiplier(RID p_baked_light) const=0;
+
 	virtual void baked_light_add_lightmap(RID p_baked_light,const RID p_texture,int p_id)=0;
 	virtual void baked_light_clear_lightmaps(RID p_baked_light)=0;