Merge branch 'master' of github.com:okamstudio/godot

34 files changed, 1220 insertions, 79 deletions

diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 65f3b80dd3..1a41cda482 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -120,6 +120,7 @@ void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant&
 		case Physics2DServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
 		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
 		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale=p_value; ; break;
 		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
 		case Physics2DServer::AREA_PARAM_LINEAR_DAMP: linear_damp=p_value; ; break;
 		case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: angular_damp=p_value; ; break;
@@ -136,6 +137,7 @@ Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const {
 		case Physics2DServer::AREA_PARAM_GRAVITY: return gravity;
 		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
 		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case Physics2DServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale;
 		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
 		case Physics2DServer::AREA_PARAM_LINEAR_DAMP: return linear_damp;
 		case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp;
@@ -248,6 +250,7 @@ Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this),
 	gravity=9.80665;
 	gravity_vector=Vector2(0,-1);
 	gravity_is_point=false;
+	gravity_distance_scale=0;
 	point_attenuation=1;
 
 	angular_damp=1.0;
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index 26b7b2516c..6d99764c68 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/servers/physics_2d/area_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -46,6 +46,7 @@ class Area2DSW : public CollisionObject2DSW{
 	float gravity;
 	Vector2 gravity_vector;
 	bool gravity_is_point;
+	float gravity_distance_scale;
 	float point_attenuation;
 	float linear_damp;
 	float angular_damp;
@@ -139,6 +140,9 @@ public:
 	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
 	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
 
+	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale=scale; }
+	_FORCE_INLINE_ float get_gravity_distance_scale() const { return gravity_distance_scale; }
+
 	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
 	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
 
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index ed2e34c972..3b1705bd56 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -32,7 +32,7 @@
 
 bool AreaPair2DSW::setup(float p_step) {
 
-	bool result = CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this);
+	bool result = area->test_collision_mask(body) &&  CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this);
 
 	if (result!=colliding) {
 
@@ -102,7 +102,7 @@ AreaPair2DSW::~AreaPair2DSW() {
 
 bool Area2Pair2DSW::setup(float p_step) {
 
-	bool result = CollisionSolver2DSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
+	bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
 
 	if (result!=colliding) {
 
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
index 575490b109..59113c9162 100644
--- a/servers/physics_2d/area_pair_2d_sw.h
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 06d466ace8..38835c9a82 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -275,10 +275,13 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va
 				Matrix32 t = p_variant;
 				t.orthonormalize();
 				new_transform=get_transform(); //used as old to compute motion
+				if (t==new_transform)
+					break;
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
 
 			}
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_LINEAR_VELOCITY: {
@@ -286,12 +289,14 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va
 			//if (mode==Physics2DServer::BODY_MODE_STATIC)
 			//	break;
 			linear_velocity=p_variant;
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
 			//if (mode!=Physics2DServer::BODY_MODE_RIGID)
 			//	break;
 			angular_velocity=p_variant;
+			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_SLEEPING: {
@@ -378,14 +383,16 @@ void Body2DSW::set_space(Space2DSW *p_space){
 void Body2DSW::_compute_area_gravity(const Area2DSW *p_area) {
 
 	if (p_area->is_gravity_point()) {
-
-		gravity = (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
-
+		if(p_area->get_gravity_distance_scale() > 0) {
+			Vector2 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) );
+		} else {
+			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
+		}
 	} else {
-		gravity = p_area->get_gravity_vector() * p_area->get_gravity();
+		gravity += p_area->get_gravity_vector() * p_area->get_gravity();
 	}
 
-	gravity*=gravity_scale;
 }
 
 void Body2DSW::integrate_forces(real_t p_step) {
@@ -393,32 +400,39 @@ void Body2DSW::integrate_forces(real_t p_step) {
 	if (mode==Physics2DServer::BODY_MODE_STATIC)
 		return;
 
-	Area2DSW *current_area = get_space()->get_default_area();
-	ERR_FAIL_COND(!current_area);
+	Area2DSW *def_area = get_space()->get_default_area();
+	Area2DSW *damp_area = def_area;
+	ERR_FAIL_COND(!def_area);
 
-	int prio = current_area->get_priority();
 	int ac = areas.size();
+	bool replace = false;
+	gravity=Vector2(0,0);
 	if (ac) {
+		areas.sort();
 		const AreaCMP *aa = &areas[0];
-		for(int i=0;i<ac;i++) {
-			if (aa[i].area->get_priority() > prio) {
-				current_area=aa[i].area;
-				prio=current_area->get_priority();
+		damp_area = aa[ac-1].area;
+		for(int i=ac-1;i>=0;i--) {
+			_compute_area_gravity(aa[i].area);
+			if (aa[i].area->get_space_override_mode() == Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE) {
+				replace = true;
+				break;
 			}
 		}
 	}
-
-	_compute_area_gravity(current_area);
+	if( !replace ) {
+		_compute_area_gravity(def_area);
+	}
+	gravity*=gravity_scale;
 
 	if (angular_damp>=0)
 		area_angular_damp=angular_damp;
 	else
-		area_angular_damp=current_area->get_angular_damp();
+		area_angular_damp=damp_area->get_angular_damp();
 
 	if (linear_damp>=0)
 		area_linear_damp=linear_damp;
 	else
-		area_linear_damp=current_area->get_linear_damp();
+		area_linear_damp=damp_area->get_linear_damp();
 
 	Vector2 motion;
 	bool do_motion=false;
@@ -482,7 +496,8 @@ void Body2DSW::integrate_forces(real_t p_step) {
 		_update_shapes_with_motion(motion);
 	}
 
-	current_area=NULL; // clear the area, so it is set in the next frame
+	damp_area=NULL; // clear the area, so it is set in the next frame
+	def_area=NULL; // clear the area, so it is set in the next frame
 	contact_count=0;	
 
 }
@@ -647,6 +662,7 @@ Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inerti
 	area_linear_damp=0;
 	contact_count=0;
 	gravity_scale=1.0;
+	using_one_way_cache=false;
 	one_way_collision_max_depth=0.1;
 
 	still_time=0;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 3b87be2737..c86bb51f1d 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -81,6 +81,7 @@ class Body2DSW : public CollisionObject2DSW {
 	bool active;
 	bool can_sleep;
 	bool first_time_kinematic;
+	bool using_one_way_cache;
 	void _update_inertia();
 	virtual void _shapes_changed();
 	Matrix32 new_transform;
@@ -91,13 +92,14 @@ class Body2DSW : public CollisionObject2DSW {
 	struct AreaCMP {
 
 		Area2DSW *area;
-		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
+		_FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();}
+		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();}
 		_FORCE_INLINE_ AreaCMP() {}
 		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area;}
 	};
 
 
-	VSet<AreaCMP> areas;
+	Vector<AreaCMP> areas;
 
 	struct Contact {
 
@@ -140,7 +142,7 @@ public:
 	void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
 
 
-	_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.insert(AreaCMP(p_area)); }
+	_FORCE_INLINE_ void add_area(Area2DSW *p_area) { areas.ordered_insert(AreaCMP(p_area)); }
 	_FORCE_INLINE_ void remove_area(Area2DSW *p_area) { areas.erase(AreaCMP(p_area)); }
 
 	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
@@ -200,6 +202,15 @@ public:
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
+	_FORCE_INLINE_ void wakeup() {
+		if ((!get_space()) || mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+			return;
+		set_active(true);
+	}
+
+
+
+
 	void set_param(Physics2DServer::BodyParameter p_param, float);
 	float get_param(Physics2DServer::BodyParameter p_param) const;
 
@@ -219,12 +230,17 @@ public:
 	_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; }
 	_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
 
-	void set_one_way_collision_direction(const Vector2& p_dir) { one_way_collision_direction=p_dir; }
+	void set_one_way_collision_direction(const Vector2& p_dir) {
+		one_way_collision_direction=p_dir;
+		using_one_way_cache=one_way_collision_direction!=Vector2();
+	}
 	Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; }
 
 	void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth=p_depth; }
 	float get_one_way_collision_max_depth() const { return one_way_collision_max_depth; }
 
+	_FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; }
+
 	void set_space(Space2DSW *p_space);
 
 	void update_inertias();
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index c4d6abe5ac..6bfed134e6 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -234,7 +234,7 @@ bool BodyPair2DSW::setup(float p_step) {
 
 
 	//cannot collide
-	if ((A->get_layer_mask()&B->get_layer_mask())==0 || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
+	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
 		collided=false;
 		return false;
 	}
@@ -265,6 +265,8 @@ bool BodyPair2DSW::setup(float p_step) {
 	} 
 	//faster to set than to check..
 
+	//bool prev_collided=collided;
+
 	collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis);
 	if (!collided) {
 
@@ -280,9 +282,68 @@ bool BodyPair2DSW::setup(float p_step) {
 				collided=true;
 		}
 
-		if (!collided)
+		if (!collided) {
+			oneway_disabled=false;
 			return false;
+		}
+
+	}
+
+	if (oneway_disabled)
+		return false;
+
+	//if (!prev_collided) {
+	{
+
+		if (A->is_using_one_way_collision()) {
+			Vector2 direction = A->get_one_way_collision_direction();
+			bool valid=false;
+			for(int i=0;i<contact_count;i++) {
+				Contact& c = contacts[i];
+
+				if (c.normal.dot(direction)<0)
+					continue;
+				if (B->get_linear_velocity().dot(direction)<0)
+					continue;
+
+				if (!c.reused) {
+					continue;
+				}
+
+				valid=true;
+			}
+
+			if (!valid) {
+				collided=false;
+				oneway_disabled=true;
+				return false;
+			}
+		}
+
+		if (B->is_using_one_way_collision()) {
+			Vector2 direction = B->get_one_way_collision_direction();
+			bool valid=false;
+			for(int i=0;i<contact_count;i++) {
+
+				Contact& c = contacts[i];
 
+				if (c.normal.dot(direction)<0)
+					continue;
+				if (A->get_linear_velocity().dot(direction)<0)
+					continue;
+
+				if (!c.reused) {
+					continue;
+				}
+
+				valid=true;
+			}
+			if (!valid) {
+				collided=false;
+				oneway_disabled=true;
+				return false;
+			}
+		}
 	}
 
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
@@ -472,6 +533,7 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_sha
 	B->add_constraint(this,1);
 	contact_count=0;
 	collided=false;
+	oneway_disabled=false;
 
 }
 
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
index 15d7e62d3a..a7fa287be4 100644
--- a/servers/physics_2d/body_pair_2d_sw.h
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -76,6 +76,7 @@ class BodyPair2DSW : public Constraint2DSW {
 	Contact contacts[MAX_CONTACTS];
 	int contact_count;
 	bool collided;
+	bool oneway_disabled;
 	int cc;
 
 
diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp
index 9641a986e8..e61b4735b9 100644
--- a/servers/physics_2d/broad_phase_2d_basic.cpp
+++ b/servers/physics_2d/broad_phase_2d_basic.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h
index ce15752251..cdee77ffd7 100644
--- a/servers/physics_2d/broad_phase_2d_basic.h
+++ b/servers/physics_2d/broad_phase_2d_basic.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 0f08f63937..4651d485c2 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -467,9 +467,10 @@ int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_t
 	delta.x=cell_size/delta.x;
 	delta.y=cell_size/delta.y;
 
-	Point2i pos = p_from.floor() / cell_size;
-	Point2i end = p_to.floor() / cell_size;
-	Point2i step = Vector2( SGN(dir.x), SGN(dir.y));
+	Point2i pos = (p_from/cell_size).floor();
+	Point2i end = (p_to/cell_size).floor();
+
+	Point2i step = Vector2( SGN(dir.x), SGN(dir.y) );
 
 	Vector2 max;
 
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
index d530b35d5d..a2eecf7bbf 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.h
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp
index 7ded6ed01e..fd8e7254b6 100644
--- a/servers/physics_2d/broad_phase_2d_sw.cpp
+++ b/servers/physics_2d/broad_phase_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h
index 510f7db112..283d6941e1 100644
--- a/servers/physics_2d/broad_phase_2d_sw.h
+++ b/servers/physics_2d/broad_phase_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index eefc598b39..7c8e223c57 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -226,7 +226,7 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
 	type=p_type;
 	space=NULL;
 	instance_id=0;
-	user_mask=0;
+	collision_mask=1;
 	layer_mask=1;
 	pickable=true;
 }
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 0c91237876..f3432060b9 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -65,7 +65,7 @@ private:
 	Space2DSW *space;
 	Matrix32 transform;
 	Matrix32 inv_transform;
-	uint32_t user_mask;
+	uint32_t collision_mask;
 	uint32_t layer_mask;
 	bool _static;
 
@@ -117,8 +117,8 @@ public:
 	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
 
 
-	void set_user_mask(uint32_t p_mask) {user_mask=p_mask;}
-	_FORCE_INLINE_ uint32_t get_user_mask() const { return user_mask; }
+	void set_collision_mask(uint32_t p_mask) {collision_mask=p_mask;}
+	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
 	void set_layer_mask(uint32_t p_mask) {layer_mask=p_mask;}
 	_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
@@ -133,6 +133,11 @@ public:
 	void set_pickable(bool p_pickable) { pickable=p_pickable; }
 	_FORCE_INLINE_ bool is_pickable() const { return pickable; }
 
+	_FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW* p_other) const {
+
+		return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask;
+	}
+
 	virtual ~CollisionObject2DSW() {}
 
 };
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index 9ed594e0eb..2525c6e942 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h
index be5a3dc79f..554f756738 100644
--- a/servers/physics_2d/collision_solver_2d_sat.h
+++ b/servers/physics_2d/collision_solver_2d_sat.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index 4c7e68d643..134ad04222 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h
index 07141b0d09..0cdd54f8b6 100644
--- a/servers/physics_2d/collision_solver_2d_sw.h
+++ b/servers/physics_2d/collision_solver_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/constraint_2d_sw.cpp b/servers/physics_2d/constraint_2d_sw.cpp
index e97b5d794e..24d56ce9fe 100644
--- a/servers/physics_2d/constraint_2d_sw.cpp
+++ b/servers/physics_2d/constraint_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
index 7abe49f5b4..a9145c382f 100644
--- a/servers/physics_2d/constraint_2d_sw.h
+++ b/servers/physics_2d/constraint_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index fea58b6e8d..b4c149e7e0 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index 0a9bf34250..2093be88c9 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 883acd0200..b446f4928a 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -30,7 +30,7 @@
 #include "broad_phase_2d_basic.h"
 #include "broad_phase_2d_hash_grid.h"
 #include "collision_solver_2d_sw.h"
-
+#include "globals.h"
 RID Physics2DServerSW::shape_create(ShapeType p_shape) {
 
 	Shape2DSW *shape=NULL;
@@ -261,7 +261,7 @@ Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space
 
 	Space2DSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND_V(!space,NULL);
-	if (/*doing_sync ||*/ space->is_locked()) {
+	if ((using_threads && !doing_sync) || space->is_locked()) {
 
 		ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification.");
 		ERR_FAIL_V(NULL);
@@ -480,6 +480,22 @@ void Physics2DServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
 
 }
 
+void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_collision_mask(p_mask);
+}
+
+void Physics2DServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_layer_mask(p_mask);
+}
+
 
 void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
 
@@ -717,7 +733,7 @@ void Physics2DServerSW::body_set_layer_mask(RID p_body, uint32_t p_flags) {
 
 };
 
-uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body, uint32_t p_flags) const {
+uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,0);
@@ -726,20 +742,20 @@ uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body, uint32_t p_flags) co
 };
 
 
-void Physics2DServerSW::body_set_user_mask(RID p_body, uint32_t p_flags) {
+void Physics2DServerSW::body_set_collision_mask(RID p_body, uint32_t p_flags) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	body->set_user_mask(p_flags);
+	body->set_collision_mask(p_flags);
 
 };
 
-uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const {
+uint32_t Physics2DServerSW::body_get_collision_mask(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,0);
 
-	return body->get_user_mask();
+	return body->get_collision_mask();
 };
 
 void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
@@ -783,6 +799,8 @@ void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_forc
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_force(p_force);
+	body->wakeup();
+
 };
 
 Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const {
@@ -798,6 +816,7 @@ void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) {
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_torque(p_torque);
+	body->wakeup();
 };
 
 float Physics2DServerSW::body_get_applied_torque(RID p_body) const {
@@ -814,6 +833,7 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, con
 	ERR_FAIL_COND(!body);
 
 	body->apply_impulse(p_pos,p_impulse);
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) {
@@ -826,7 +846,7 @@ void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis
 	v-=axis*axis.dot(v);
 	v+=p_axis_velocity;
 	body->set_linear_velocity(v);
-
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
@@ -835,7 +855,7 @@ void Physics2DServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
 	ERR_FAIL_COND(!body);
 
 	body->add_exception(p_body_b);
-
+	body->wakeup();
 };
 
 void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
@@ -844,6 +864,7 @@ void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b
 	ERR_FAIL_COND(!body);
 
 	body->remove_exception(p_body_b);
+	body->wakeup();
 
 };
 
@@ -959,6 +980,18 @@ void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) {
 
 }
 
+bool Physics2DServerSW::body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,MotionResult *r_result) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body->get_space(),false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(),false);
+
+	return body->get_space()->test_body_motion(body,p_motion,p_margin,r_result);
+
+}
+
+
 /* JOINT API */
 
 void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
@@ -1163,7 +1196,7 @@ void Physics2DServerSW::set_active(bool p_active) {
 
 void Physics2DServerSW::init() {
 
-	doing_sync=true;
+	doing_sync=false;
 	last_step=0.001;
 	iterations=8;// 8?
 	stepper = memnew( Step2DSW );
@@ -1195,6 +1228,7 @@ void Physics2DServerSW::step(float p_step) {
 
 void Physics2DServerSW::sync() {
 
+	doing_sync=true;
 };
 
 void Physics2DServerSW::flush_queries() {
@@ -1202,7 +1236,7 @@ void Physics2DServerSW::flush_queries() {
 	if (!active)
 		return;
 
-	doing_sync=true;
+
 	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
 
 		Space2DSW *space=(Space2DSW *)E->get();
@@ -1211,6 +1245,10 @@ void Physics2DServerSW::flush_queries() {
 
 };
 
+void Physics2DServerSW::end_sync() {
+	doing_sync=false;
+}
+
 
 
 void Physics2DServerSW::finish() {
@@ -1250,6 +1288,7 @@ Physics2DServerSW::Physics2DServerSW() {
 	island_count=0;
 	active_objects=0;
 	collision_pairs=0;
+	using_threads=int(Globals::get_singleton()->get("physics_2d/thread_model"))==2;
 
 };
 
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 58fc4eeb33..6e875701b8 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -51,6 +51,8 @@ friend class Physics2DDirectSpaceStateSW;
 	int active_objects;
 	int collision_pairs;
 
+	bool using_threads;
+
 
 	Step2DSW *stepper;
 	Set<const Space2DSW*> active_spaces;
@@ -134,6 +136,8 @@ public:
 	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
 	virtual Matrix32 area_get_transform(RID p_area) const;
 	virtual void area_set_monitorable(RID p_area,bool p_monitorable);
+	virtual void area_set_collision_mask(RID p_area,uint32_t p_mask);
+	virtual void area_set_layer_mask(RID p_area,uint32_t p_mask);
 
 	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
 	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
@@ -177,10 +181,10 @@ public:
 	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
 
 	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask);
-	virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const;
+	virtual uint32_t body_get_layer_mask(RID p_body) const;
 
-	virtual void body_set_user_mask(RID p_body, uint32_t p_mask);
-	virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const;
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask);
+	virtual uint32_t body_get_collision_mask(RID p_) const;
 
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
@@ -223,6 +227,9 @@ public:
 
 	virtual void body_set_pickable(RID p_body,bool p_pickable);
 
+	virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
+
+
 	/* JOINT API */
 
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
@@ -243,8 +250,9 @@ public:
 	virtual void set_active(bool p_active);
 	virtual void init();
 	virtual void step(float p_step);
-	virtual void sync();
+	virtual void sync();	
 	virtual void flush_queries();
+	virtual void end_sync();
 	virtual void finish();
 
 	int get_process_info(ProcessInfo p_info);
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.cpp b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
new file mode 100644
index 0000000000..c5f023f162
--- /dev/null
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
@@ -0,0 +1,169 @@
+#include "physics_2d_server_wrap_mt.h"
+
+#include "os/os.h"
+
+void Physics2DServerWrapMT::thread_exit() {
+
+	exit=true;
+}
+
+void Physics2DServerWrapMT::thread_step(float p_delta) {
+
+	physics_2d_server->step(p_delta);
+	step_sem->post();
+
+}
+
+void Physics2DServerWrapMT::_thread_callback(void *_instance) {
+
+	Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT*>(_instance);
+
+
+	vsmt->thread_loop();
+}
+
+void Physics2DServerWrapMT::thread_loop() {
+
+	server_thread=Thread::get_caller_ID();
+
+	OS::get_singleton()->make_rendering_thread();
+
+	physics_2d_server->init();
+
+	exit=false;
+	step_thread_up=true;
+	while(!exit) {
+		// flush commands one by one, until exit is requested
+		command_queue.wait_and_flush_one();
+	}
+
+	command_queue.flush_all(); // flush all
+
+	physics_2d_server->finish();
+
+}
+
+
+/* EVENT QUEUING */
+
+
+void Physics2DServerWrapMT::step(float p_step) {
+
+	if (create_thread) {
+
+		command_queue.push( this, &Physics2DServerWrapMT::thread_step,p_step);
+	} else {
+
+		command_queue.flush_all(); //flush all pending from other threads
+		physics_2d_server->step(p_step);
+	}
+}
+
+void Physics2DServerWrapMT::sync() {
+
+	if (step_sem) {
+		if (first_frame)
+			first_frame=false;
+		else
+			step_sem->wait(); //must not wait if a step was not issued
+	}
+	physics_2d_server->sync();;
+}
+
+void Physics2DServerWrapMT::flush_queries(){
+
+	physics_2d_server->flush_queries();
+}
+
+void Physics2DServerWrapMT::end_sync() {
+
+	physics_2d_server->end_sync();;
+}
+
+void Physics2DServerWrapMT::init() {
+
+	if (create_thread) {
+
+		step_sem = Semaphore::create();
+		print_line("CREATING PHYSICS 2D THREAD");
+		//OS::get_singleton()->release_rendering_thread();
+		if (create_thread) {
+			thread = Thread::create( _thread_callback, this );
+			print_line("STARTING PHYISICS 2D THREAD");
+		}
+		while(!step_thread_up) {
+			OS::get_singleton()->delay_usec(1000);
+		}
+		print_line("DONE PHYSICS 2D THREAD");
+	} else {
+
+		physics_2d_server->init();
+	}
+
+}
+
+void Physics2DServerWrapMT::finish() {
+
+
+	if (thread) {
+
+		command_queue.push( this, &Physics2DServerWrapMT::thread_exit);
+		Thread::wait_to_finish( thread );
+		memdelete(thread);
+
+/*
+		shape_free_cached_ids();
+		area_free_cached_ids();
+		body_free_cached_ids();
+		pin_joint_free_cached_ids();
+		groove_joint_free_cached_ids();
+		damped_string_free_cached_ids();
+*/
+		thread=NULL;
+	} else {
+		physics_2d_server->finish();
+	}
+
+	if (step_sem)
+		memdelete(step_sem);
+
+}
+
+
+Physics2DServerWrapMT::Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread) : command_queue(p_create_thread) {
+
+	physics_2d_server=p_contained;
+	create_thread=p_create_thread;
+	thread=NULL;
+	step_sem=NULL;
+	step_pending=0;
+	step_thread_up=false;
+	alloc_mutex=Mutex::create();
+
+	shape_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	area_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	body_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	pin_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	groove_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	damped_spring_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+
+	if (!p_create_thread) {
+		server_thread=Thread::get_caller_ID();
+	} else {
+		server_thread=0;
+	}
+
+	main_thread = Thread::get_caller_ID();
+	first_frame=true;
+}
+
+
+Physics2DServerWrapMT::~Physics2DServerWrapMT() {
+
+	memdelete(physics_2d_server);
+	memdelete(alloc_mutex);
+	//finish();
+
+}
+
+
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
new file mode 100644
index 0000000000..0ddc8f16ec
--- /dev/null
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -0,0 +1,297 @@
+#ifndef PHYSICS2DSERVERWRAPMT_H
+#define PHYSICS2DSERVERWRAPMT_H
+
+
+#include "servers/physics_2d_server.h"
+#include "command_queue_mt.h"
+#include "os/thread.h"
+#include "globals.h"
+
+#ifdef DEBUG_SYNC
+#define SYNC_DEBUG print_line("sync on: "+String(__FUNCTION__));
+#else
+#define SYNC_DEBUG
+#endif
+
+
+class Physics2DServerWrapMT : public Physics2DServer {
+
+	mutable Physics2DServer *physics_2d_server;
+
+	mutable CommandQueueMT command_queue;
+
+	static void _thread_callback(void *_instance);
+	void thread_loop();
+
+	Thread::ID server_thread;
+	Thread::ID main_thread;
+	volatile bool exit;
+	Thread *thread;
+	volatile bool step_thread_up;
+	bool create_thread;
+
+	Semaphore *step_sem;
+	int step_pending;
+	void thread_step(float p_delta);
+	void thread_flush();
+
+	void thread_exit();
+
+	Mutex*alloc_mutex;
+	bool first_frame;
+
+	int shape_pool_max_size;
+	List<RID> shape_id_pool;
+	int area_pool_max_size;
+	List<RID> area_id_pool;
+	int body_pool_max_size;
+	List<RID> body_id_pool;
+	int pin_joint_pool_max_size;
+	List<RID> pin_joint_id_pool;
+	int groove_joint_pool_max_size;
+	List<RID> groove_joint_id_pool;
+	int damped_spring_joint_pool_max_size;
+	List<RID> damped_spring_joint_id_pool;
+
+
+public:
+
+#define ServerName Physics2DServer
+#define ServerNameWrapMT Physics2DServerWrapMT
+#define server_name physics_2d_server
+#include "servers/server_wrap_mt_common.h"
+
+	//FUNC1RID(shape,ShapeType); todo fix
+	FUNC1R(RID,shape_create,ShapeType);
+	FUNC2(shape_set_data,RID,const Variant& );
+	FUNC2(shape_set_custom_solver_bias,RID,real_t );
+
+	FUNC1RC(ShapeType,shape_get_type,RID );
+	FUNC1RC(Variant,shape_get_data,RID);
+	FUNC1RC(real_t,shape_get_custom_solver_bias,RID);
+
+
+	//these work well, but should be used from the main thread only
+	bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
+		return physics_2d_server->shape_collide(p_shape_A,p_xform_A,p_motion_A,p_shape_B,p_xform_B,p_motion_B,r_results,p_result_max,r_result_count);
+	}
+
+	/* SPACE API */
+
+	FUNC0R(RID,space_create);
+	FUNC2(space_set_active,RID,bool);
+	FUNC1RC(bool,space_is_active,RID);
+
+	FUNC3(space_set_param,RID,SpaceParameter,real_t);
+	FUNC2RC(real_t,space_get_param,RID,SpaceParameter);
+
+	// this function only works on fixed process, errors and returns null otherwise
+	Physics2DDirectSpaceState* space_get_direct_state(RID p_space) {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),NULL);
+		return physics_2d_server->space_get_direct_state(p_space);
+	}
+
+
+	/* AREA API */
+
+	//FUNC0RID(area);
+	FUNC0R(RID,area_create);
+
+	FUNC2(area_set_space,RID,RID);
+	FUNC1RC(RID,area_get_space,RID);
+
+	FUNC2(area_set_space_override_mode,RID,AreaSpaceOverrideMode);
+	FUNC1RC(AreaSpaceOverrideMode,area_get_space_override_mode,RID);
+
+	FUNC3(area_add_shape,RID,RID,const Matrix32&);
+	FUNC3(area_set_shape,RID,int,RID);
+	FUNC3(area_set_shape_transform,RID,int,const Matrix32&);
+
+	FUNC1RC(int,area_get_shape_count,RID);
+	FUNC2RC(RID,area_get_shape,RID,int);
+	FUNC2RC(Matrix32,area_get_shape_transform,RID,int);
+	FUNC2(area_remove_shape,RID,int);
+	FUNC1(area_clear_shapes,RID);
+
+	FUNC2(area_attach_object_instance_ID,RID,ObjectID);
+	FUNC1RC(ObjectID,area_get_object_instance_ID,RID);
+
+	FUNC3(area_set_param,RID,AreaParameter,const Variant&);
+	FUNC2(area_set_transform,RID,const Matrix32&);
+
+	FUNC2RC(Variant,area_get_param,RID,AreaParameter);
+	FUNC1RC(Matrix32,area_get_transform,RID);
+
+	FUNC2(area_set_collision_mask,RID,uint32_t);
+	FUNC2(area_set_layer_mask,RID,uint32_t);
+
+	FUNC2(area_set_monitorable,RID,bool);
+	FUNC2(area_set_pickable,RID,bool);
+
+	FUNC3(area_set_monitor_callback,RID,Object*,const StringName&);
+	FUNC3(area_set_area_monitor_callback,RID,Object*,const StringName&);
+
+
+	/* BODY API */
+
+	//FUNC2RID(body,BodyMode,bool);
+	FUNC2R(RID,body_create,BodyMode,bool)
+
+	FUNC2(body_set_space,RID,RID);
+	FUNC1RC(RID,body_get_space,RID);
+
+	FUNC2(body_set_mode,RID,BodyMode);
+	FUNC1RC(BodyMode,body_get_mode,RID);
+
+
+	FUNC3(body_add_shape,RID,RID,const Matrix32&);
+	FUNC3(body_set_shape,RID,int,RID);
+	FUNC3(body_set_shape_transform,RID,int,const Matrix32&);
+	FUNC3(body_set_shape_metadata,RID,int,const Variant&);
+
+	FUNC1RC(int,body_get_shape_count,RID);
+	FUNC2RC(Matrix32,body_get_shape_transform,RID,int);
+	FUNC2RC(Variant,body_get_shape_metadata,RID,int);
+	FUNC2RC(RID,body_get_shape,RID,int);
+
+	FUNC3(body_set_shape_as_trigger,RID,int,bool);
+	FUNC2RC(bool,body_is_shape_set_as_trigger,RID,int);
+
+	FUNC2(body_remove_shape,RID,int);
+	FUNC1(body_clear_shapes,RID);
+
+	FUNC2(body_attach_object_instance_ID,RID,uint32_t);
+	FUNC1RC(uint32_t,body_get_object_instance_ID,RID);
+
+	FUNC2(body_set_continuous_collision_detection_mode,RID,CCDMode);
+	FUNC1RC(CCDMode,body_get_continuous_collision_detection_mode,RID);
+
+	FUNC2(body_set_layer_mask,RID,uint32_t);
+	FUNC1RC(uint32_t,body_get_layer_mask,RID);
+
+	FUNC2(body_set_collision_mask,RID,uint32_t);
+	FUNC1RC(uint32_t,body_get_collision_mask,RID);
+
+
+	FUNC3(body_set_param,RID,BodyParameter,float);
+	FUNC2RC(float,body_get_param,RID,BodyParameter);
+
+
+	FUNC3(body_set_state,RID,BodyState,const Variant&);
+	FUNC2RC(Variant,body_get_state,RID,BodyState);
+
+	FUNC2(body_set_applied_force,RID,const Vector2&);
+	FUNC1RC(Vector2,body_get_applied_force,RID);
+
+	FUNC2(body_set_applied_torque,RID,float);
+	FUNC1RC(float,body_get_applied_torque,RID);
+
+	FUNC3(body_apply_impulse,RID,const Vector2&,const Vector2&);
+	FUNC2(body_set_axis_velocity,RID,const Vector2&);
+
+	FUNC2(body_add_collision_exception,RID,RID);
+	FUNC2(body_remove_collision_exception,RID,RID);
+	FUNC2S(body_get_collision_exceptions,RID,List<RID>*);
+
+	FUNC2(body_set_max_contacts_reported,RID,int);
+	FUNC1RC(int,body_get_max_contacts_reported,RID);
+
+	FUNC2(body_set_one_way_collision_direction,RID,const Vector2&);
+	FUNC1RC(Vector2,body_get_one_way_collision_direction,RID);
+
+	FUNC2(body_set_one_way_collision_max_depth,RID,float);
+	FUNC1RC(float,body_get_one_way_collision_max_depth,RID);
+
+
+	FUNC2(body_set_contacts_reported_depth_treshold,RID,float);
+	FUNC1RC(float,body_get_contacts_reported_depth_treshold,RID);
+
+	FUNC2(body_set_omit_force_integration,RID,bool);
+	FUNC1RC(bool,body_is_omitting_force_integration,RID);
+
+	FUNC4(body_set_force_integration_callback,RID ,Object *,const StringName& ,const Variant& );
+
+
+	bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) {
+		return physics_2d_server->body_collide_shape(p_body,p_body_shape,p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count);
+	}
+
+	FUNC2(body_set_pickable,RID,bool);
+
+	bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
+		return physics_2d_server->body_test_motion(p_body,p_motion,p_margin,r_result);
+	}
+
+	/* JOINT API */
+
+
+	FUNC3(joint_set_param,RID,JointParam,real_t);
+	FUNC2RC(real_t,joint_get_param,RID,JointParam);
+
+
+	///FUNC3RID(pin_joint,const Vector2&,RID,RID);
+	///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID);
+	///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID);
+
+	FUNC3R(RID,pin_joint_create,const Vector2&,RID,RID);
+	FUNC5R(RID,groove_joint_create,const Vector2&,const Vector2&,const Vector2&,RID,RID);
+	FUNC4R(RID,damped_spring_joint_create,const Vector2&,const Vector2&,RID,RID);
+
+	FUNC3(damped_string_joint_set_param,RID,DampedStringParam,real_t);
+	FUNC2RC(real_t,damped_string_joint_get_param,RID,DampedStringParam);
+
+	FUNC1RC(JointType,joint_get_type,RID);
+
+
+	/* MISC */
+
+
+	FUNC1(free,RID);
+	FUNC1(set_active,bool);
+
+	virtual void init();
+	virtual void step(float p_step);
+	virtual void sync();
+	virtual void end_sync();
+	virtual void flush_queries();
+	virtual void finish();
+
+	int get_process_info(ProcessInfo p_info) {
+		return physics_2d_server->get_process_info(p_info);
+	}
+
+	Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread);
+	~Physics2DServerWrapMT();
+
+
+	template<class T>
+	static Physics2DServer* init_server() {
+
+		int tm = GLOBAL_DEF("physics_2d/thread_model",1);
+		if (tm==0) //single unsafe
+			return memnew( T );
+		else if (tm==1) //single saef
+			return memnew( Physics2DServerWrapMT( memnew( T ), false ));
+		else //single unsafe
+			return memnew( Physics2DServerWrapMT( memnew( T ), true ));
+
+
+	}
+
+#undef ServerNameWrapMT
+#undef ServerName
+#undef server_name
+
+};
+
+#ifdef DEBUG_SYNC
+#undef DEBUG_SYNC
+#endif
+#undef SYNC_DEBUG
+
+#endif // PHYSICS2DSERVERWRAPMT_H
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 9a4b52d563..d3591ec744 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index 05ea5b21cd..6d5473aa17 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 5aaf9a7613..767ad9038d 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -555,8 +555,524 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
 
 
 
+int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) {
 
 
+	int amount = broadphase->cull_aabb(p_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
+
+	for(int i=0;i<amount;i++) {
+
+		bool keep=true;
+
+		if (intersection_query_results[i]==p_body)
+			keep=false;
+		else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
+			keep=false;
+		else if ((static_cast<Body2DSW*>(intersection_query_results[i])->test_collision_mask(p_body))==0)
+			keep=false;
+		else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
+			keep=false;
+		else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i]))
+			keep=false;
+
+		if (!keep) {
+
+			if (i<amount-1) {
+				SWAP(intersection_query_results[i],intersection_query_results[amount-1]);
+				SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]);
+
+			}
+
+			amount--;
+			i--;
+
+		}
+	}
+
+	return amount;
+}
+
+bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
+
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	Rect2 body_aabb;
+
+	for(int i=0;i<p_body->get_shape_count();i++) {
+
+		if (i==0)
+			body_aabb=p_body->get_shape_aabb(i);
+		else
+			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
+	}
+
+	body_aabb=body_aabb.grow(p_margin);
+
+
+	Matrix32 body_transform = p_body->get_transform();
+
+	{
+		//STEP 1, FREE BODY IF STUCK
+
+		const int max_results = 32;
+		int recover_attempts=4;
+		Vector2 sr[max_results*2];
+
+		do {
+
+			Physics2DServerSW::CollCbkData cbk;
+			cbk.max=max_results;
+			cbk.amount=0;
+			cbk.ptr=sr;
+
+
+			CollisionSolver2DSW::CallbackResult cbkres=NULL;
+
+			Physics2DServerSW::CollCbkData *cbkptr=NULL;
+			cbkptr=&cbk;
+			cbkres=Physics2DServerSW::_shape_col_cbk;
+
+			bool collided=false;
+
+			int amount = _cull_aabb_for_body(p_body,body_aabb);
+
+			for(int j=0;j<p_body->get_shape_count();j++) {
+				if (p_body->is_shape_set_as_trigger(j))
+					continue;
+
+				Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
+				Shape2DSW *body_shape = p_body->get_shape(j);
+				for(int i=0;i<amount;i++) {
+
+					const CollisionObject2DSW *col_obj=intersection_query_results[i];
+					int shape_idx=intersection_query_subindex_results[i];
+
+					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+
+						Vector2 cdir = body->get_one_way_collision_direction();
+						//if (cdir!=Vector2() && p_motion.dot(cdir)<0)
+						//	continue;
+
+						cbk.valid_dir=cdir;
+						cbk.valid_depth=body->get_one_way_collision_max_depth();
+					} else {
+						cbk.valid_dir=Vector2();
+						cbk.valid_depth=0;
+					}
+
+					if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
+						collided=cbk.amount>0;
+					}
+				}
+			}
+
+
+			if (!collided)
+				break;
+
+			Vector2 recover_motion;
+
+			for(int i=0;i<cbk.amount;i++) {
+
+				Vector2 a = sr[i*2+0];
+				Vector2 b = sr[i*2+1];
+
+			//	float d = a.distance_to(b);
+
+				//if (d<margin)
+				///	continue;
+				recover_motion+=(b-a)*0.4;
+			}
+
+			if (recover_motion==Vector2()) {
+				collided=false;
+				break;
+			}
+
+			body_transform.elements[2]+=recover_motion;
+			body_aabb.pos+=recover_motion;
+
+			recover_attempts--;
+
+		} while (recover_attempts);
+	}
+
+
+
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
+
+	{
+		// STEP 2 ATTEMPT MOTION
+
+		Rect2 motion_aabb=body_aabb;
+		motion_aabb.pos+=p_motion;
+		motion_aabb=motion_aabb.merge(body_aabb);
+
+		int amount = _cull_aabb_for_body(p_body,motion_aabb);
+
+		for(int j=0;j<p_body->get_shape_count();j++) {
+
+			if (p_body->is_shape_set_as_trigger(j))
+				continue;
+
+			Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
+			Shape2DSW *body_shape = p_body->get_shape(j);
+
+			bool stuck=false;
+
+			float best_safe=1;
+			float best_unsafe=1;
+
+			for(int i=0;i<amount;i++) {
+
+				const CollisionObject2DSW *col_obj=intersection_query_results[i];
+				int shape_idx=intersection_query_subindex_results[i];
+
+
+				Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+				//test initial overlap, does it collide if going all the way?
+				if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+					continue;
+				}
+
+
+				//test initial overlap
+				if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+
+					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+						//if one way collision direction ignore initial overlap
+						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+						if (body->get_one_way_collision_direction()!=Vector2()) {
+							continue;
+						}
+					}
+
+					stuck=true;
+					break;
+				}
+
+
+				//just do kinematic solving
+				float low=0;
+				float hi=1;
+				Vector2 mnormal=p_motion.normalized();
+
+				for(int i=0;i<8;i++) { //steps should be customizable..
+
+					//Matrix32 xfa = p_xform;
+					float ofs = (low+hi)*0.5;
+
+					Vector2 sep=mnormal; //important optimization for this to work fast enough
+					bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0);
+
+					if (collided) {
+
+						hi=ofs;
+					} else {
+
+						low=ofs;
+					}
+				}
+
+				if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+					const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+					if (body->get_one_way_collision_direction()!=Vector2()) {
+
+						Vector2 cd[2];
+						Physics2DServerSW::CollCbkData cbk;
+						cbk.max=1;
+						cbk.amount=0;
+						cbk.ptr=cd;
+						cbk.valid_dir=body->get_one_way_collision_direction();
+						cbk.valid_depth=body->get_one_way_collision_max_depth();
+
+						Vector2 sep=mnormal; //important optimization for this to work fast enough
+						bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0);
+						if (!collided || cbk.amount==0) {
+							continue;
+						}
+
+					}
+				}
+
+
+				if (low<best_safe) {
+					best_safe=low;
+					best_unsafe=hi;
+				}
+			}
+
+			if (stuck) {
+
+				safe=0;
+				unsafe=0;
+				best_shape=j; //sadly it's the best
+				break;
+			}
+			if (best_safe==1.0) {
+				continue;
+			}
+			if (best_safe < safe) {
+
+				safe=best_safe;
+				unsafe=best_unsafe;
+				best_shape=j;
+			}
+		}
+	}
+
+	bool collided=false;
+	if (safe>=1) {
+		//not collided
+		collided=false;
+		if (r_result) {
+
+			r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+			r_result->remainder=Vector2();
+		}
+
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Matrix32 ugt = body_transform;
+		ugt.elements[2]+=p_motion*unsafe;
+
+		_RestCallbackData2D rcd;
+		rcd.best_len=0;
+		rcd.best_object=NULL;
+		rcd.best_shape=0;
+
+		Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
+		Shape2DSW *body_shape = p_body->get_shape(best_shape);
+
+		body_aabb.pos+=p_motion*unsafe;
+
+		int amount = _cull_aabb_for_body(p_body,body_aabb);
+
+
+		for(int i=0;i<amount;i++) {
+
+
+			const CollisionObject2DSW *col_obj=intersection_query_results[i];
+			int shape_idx=intersection_query_subindex_results[i];
+
+			if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+
+				const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+				rcd.valid_dir=body->get_one_way_collision_direction();
+				rcd.valid_depth=body->get_one_way_collision_max_depth();
+			} else {
+				rcd.valid_dir=Vector2();
+				rcd.valid_depth=0;
+			}
+
+
+			rcd.object=col_obj;
+			rcd.shape=shape_idx;
+			bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
+			if (!sc)
+				continue;
+
+		}
+
+		if (rcd.best_len!=0) {
+
+			if (r_result) {
+				r_result->collider=rcd.best_object->get_self();
+				r_result->collider_id=rcd.best_object->get_instance_id();
+				r_result->collider_shape=rcd.best_shape;
+				r_result->collision_normal=rcd.best_normal;
+				r_result->collision_point=rcd.best_contact;
+				r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
+
+				const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
+				Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
+				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+
+				r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->remainder=p_motion - safe * p_motion;
+			}
+
+			collided=true;
+		} else {
+			if (r_result) {
+
+				r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->remainder=Vector2();
+			}
+
+			collided=false;
+
+		}
+	}
+
+	return collided;
+
+
+#if 0
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+
+	colliding=false;
+	ERR_FAIL_COND_V(!is_inside_tree(),Vector2());
+	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
+	ERR_FAIL_COND_V(!dss,Vector2());
+	const int max_shapes=32;
+	Vector2 sr[max_shapes*2];
+	int res_shapes;
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+
+	//recover first
+	int recover_attempts=4;
+
+	bool collided=false;
+	uint32_t mask=0;
+	if (collide_static)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
+	if (collide_kinematic)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
+	if (collide_rigid)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
+	if (collide_character)
+		mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
+
+//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
+
+	//print_line("margin: "+rtos(margin));
+	do {
+
+		//motion recover
+		for(int i=0;i<get_shape_count();i++) {
+
+			if (is_shape_set_as_trigger(i))
+				continue;
+			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask))
+				collided=true;
+
+		}
+
+		if (!collided)
+			break;
+
+		Vector2 recover_motion;
+
+		for(int i=0;i<res_shapes;i++) {
+
+			Vector2 a = sr[i*2+0];
+			Vector2 b = sr[i*2+1];
+
+			float d = a.distance_to(b);
+
+			//if (d<margin)
+			///	continue;
+			recover_motion+=(b-a)*0.4;
+		}
+
+		if (recover_motion==Vector2()) {
+			collided=false;
+			break;
+		}
+
+		Matrix32 gt = get_global_transform();
+		gt.elements[2]+=recover_motion;
+		set_global_transform(gt);
+
+		recover_attempts--;
+
+	} while (recover_attempts);
+
+
+	//move second
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
+
+	for(int i=0;i<get_shape_count();i++) {
+
+		if (is_shape_set_as_trigger(i))
+			continue;
+
+		float lsafe,lunsafe;
+		bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask);
+		//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
+		if (!valid) {
+
+			safe=0;
+			unsafe=0;
+			best_shape=i; //sadly it's the best
+			break;
+		}
+		if (lsafe==1.0) {
+			continue;
+		}
+		if (lsafe < safe) {
+
+			safe=lsafe;
+			unsafe=lunsafe;
+			best_shape=i;
+		}
+	}
+
+
+	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
+
+	if (safe>=1) {
+		//not collided
+		colliding=false;
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Matrix32 ugt = get_global_transform();
+		ugt.elements[2]+=p_motion*unsafe;
+		Physics2DDirectSpaceState::ShapeRestInfo rest_info;
+		bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask);
+		if (!c2) {
+			//should not happen, but floating point precision is so weird..
+
+			colliding=false;
+		} else {
+
+
+			//print_line("Travel: "+rtos(travel));
+			colliding=true;
+			collision=rest_info.point;
+			normal=rest_info.normal;
+			collider=rest_info.collider_id;
+			collider_vel=rest_info.linear_velocity;
+			collider_shape=rest_info.shape;
+			collider_metadata=rest_info.metadata;
+		}
+
+	}
+
+	Vector2 motion=p_motion*safe;
+	Matrix32 gt = get_global_transform();
+	gt.elements[2]+=motion;
+	set_global_transform(gt);
+
+	return p_motion-motion;
+
+#endif
+	return false;
+}
 
 
 
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 05b55fe807..abee8628fc 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
@@ -101,6 +101,8 @@ class Space2DSW {
 	int active_objects;
 	int collision_pairs;
 
+	int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb);
+
 friend class Physics2DDirectSpaceStateSW;
 
 public:
@@ -165,6 +167,8 @@ public:
 
 	int get_collision_pairs() const { return collision_pairs; }
 
+	bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
+
 	Physics2DDirectSpaceStateSW *get_direct_state();
 
 	Space2DSW();
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index e75f9300ce..bc87789937 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */
diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h
index 91ac9d8584..d5e919836c 100644
--- a/servers/physics_2d/step_2d_sw.h
+++ b/servers/physics_2d/step_2d_sw.h
@@ -5,7 +5,7 @@
 /*                           GODOT ENGINE                                */
 /*                    http://www.godotengine.org                         */
 /*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2007-2015 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       */