-added kinematic body

-added kinematic body demos

42 files changed, 1156 insertions, 544 deletions

diff --git a/core/script_language.h b/core/script_language.h
index 102d7c8436..9731273610 100644
--- a/core/script_language.h
+++ b/core/script_language.h
@@ -31,6 +31,7 @@
 
 #include "resource.h"
 #include "map.h"
+#include "pair.h"
 /**
 	@author Juan Linietsky <reduzio@gmail.com>
 */
@@ -157,6 +158,7 @@ public:
 
 	virtual void get_recognized_extensions(List<String> *p_extensions) const=0;
 	virtual void get_public_functions(List<MethodInfo> *p_functions) const=0;
+	virtual void get_public_constants(List<Pair<String,Variant> > *p_constants) const=0;
 
 	virtual void frame();
 
diff --git a/demos/2d/kinematic_char/colworld.gd b/demos/2d/kinematic_char/colworld.gd
new file mode 100644
index 0000000000..efd1dab805
--- /dev/null
+++ b/demos/2d/kinematic_char/colworld.gd
@@ -0,0 +1,17 @@
+
+extends Node2D
+
+# member variables here, example:
+# var a=2
+# var b="textvar"
+
+func _ready():
+	# Initalization here
+	pass
+
+
+
+
+func _on_princess_body_enter( body ):
+	#the name of this editor-generated callback is unfortunate
+	get_node("youwin").show()
diff --git a/demos/2d/kinematic_char/colworld.scn b/demos/2d/kinematic_char/colworld.scn
new file mode 100644
index 0000000000..b1285b4811
--- /dev/null
+++ b/demos/2d/kinematic_char/colworld.scn
diff --git a/demos/2d/kinematic_char/engine.cfg b/demos/2d/kinematic_char/engine.cfg
new file mode 100644
index 0000000000..0132442c18
--- /dev/null
+++ b/demos/2d/kinematic_char/engine.cfg
@@ -0,0 +1,13 @@
+[application]
+
+name="Kinematic Collision"
+main_scene="res://colworld.scn"
+icon="res://icon.png"
+
+[input]
+
+move_up=[key(Up)]
+move_left=[key(Left)]
+move_right=[key(Right)]
+move_bottom=[key(Down)]
+jump=[key(Space)]
diff --git a/demos/2d/kinematic_char/icon.png b/demos/2d/kinematic_char/icon.png
new file mode 100644
index 0000000000..bdca104c1f
--- /dev/null
+++ b/demos/2d/kinematic_char/icon.png
diff --git a/demos/2d/kinematic_char/obstacle.png b/demos/2d/kinematic_char/obstacle.png
new file mode 100644
index 0000000000..3ade3c3a52
--- /dev/null
+++ b/demos/2d/kinematic_char/obstacle.png
diff --git a/demos/2d/kinematic_char/player.gd b/demos/2d/kinematic_char/player.gd
new file mode 100644
index 0000000000..b35bbfa693
--- /dev/null
+++ b/demos/2d/kinematic_char/player.gd
@@ -0,0 +1,116 @@
+
+extends KinematicBody2D
+
+# This is a simple collision demo showing how
+# the kinematic cotroller works.
+# move() will allow to move the node, and will
+# always move it to a non-colliding spot, 
+# as long as it starts from a non-colliding spot too.
+
+
+#pixels / second
+const GRAVITY = 500.0
+
+# Angle in degrees towards either side that the player can 
+# consider "floor".
+const FLOOR_ANGLE_TOLERANCE = 40
+const WALK_FORCE = 600
+const WALK_MAX_SPEED = 200
+const STOP_FORCE = 1300
+const JUMP_SPEED = 200
+const JUMP_MAX_AIRBORNE_TIME=0.2
+
+var velocity = Vector2()
+var on_air_time=100
+var jumping=false
+
+var prev_jump_pressed=false
+
+func _fixed_process(delta):
+
+	#create forces
+	var force = Vector2(0,GRAVITY)
+
+	var stop = velocity.x!=0.0
+	
+	var walk_left = Input.is_action_pressed("move_left")
+	var walk_right = Input.is_action_pressed("move_right")
+	var jump = Input.is_action_pressed("jump")
+
+	var stop=true
+	
+	if (walk_left):
+		if (velocity.x<=0 and velocity.x > -WALK_MAX_SPEED):
+			force.x-=WALK_FORCE			
+			stop=false
+		
+	elif (walk_right):
+		if (velocity.x>=0 and velocity.x < WALK_MAX_SPEED):
+			force.x+=WALK_FORCE
+			stop=false
+	
+	if (stop):
+		var vsign = sign(velocity.x)
+		var vlen = abs(velocity.x)
+		
+		vlen -= STOP_FORCE * delta
+		if (vlen<0):
+			vlen=0
+			
+		velocity.x=vlen*vsign
+		
+
+		
+	#integrate forces to velocity
+	velocity += force * delta
+	
+	#integrate velocity into motion and move
+	var motion = velocity * delta
+
+	#move and consume motion
+#
+	motion = move(motion)
+
+
+	var floor_velocity=Vector2()
+
+	if (is_colliding()):
+		#ran against something, is it the floor? get normal
+		var n = get_collision_normal()
+
+		if ( rad2deg(acos(n.dot( Vector2(0,-1)))) < FLOOR_ANGLE_TOLERANCE ):
+			#if angle to the "up" vectors is < angle tolerance
+			#char is on floor
+			on_air_time=0
+			floor_velocity=get_collider_velocity()
+			#velocity.y=0 
+			
+		# But we were moving and our motion was interrupted, 
+		# so try to complete the motion by "sliding"
+		# by the normal
+		motion = n.slide(motion)
+		velocity = n.slide(velocity)
+		
+		#then move again
+		move(motion)
+
+	if (floor_velocity!=Vector2()):
+		#if floor moves, move with floor
+		move(floor_velocity*delta)
+
+	if (jumping and velocity.y>0):
+		jumping=false
+		
+	if (on_air_time<JUMP_MAX_AIRBORNE_TIME and jump and not prev_jump_pressed and not jumping):	
+		velocity.y=-JUMP_SPEED	
+		jumping=true
+		
+	on_air_time+=delta
+	prev_jump_pressed=jump	
+
+func _ready():
+	# Initalization here
+	set_fixed_process(true)
+	pass
+
+
diff --git a/demos/2d/kinematic_char/player.png b/demos/2d/kinematic_char/player.png
new file mode 100644
index 0000000000..0e7d843899
--- /dev/null
+++ b/demos/2d/kinematic_char/player.png
diff --git a/demos/2d/kinematic_char/player.scn b/demos/2d/kinematic_char/player.scn
new file mode 100644
index 0000000000..126b332184
--- /dev/null
+++ b/demos/2d/kinematic_char/player.scn
diff --git a/demos/2d/kinematic_char/princess.png b/demos/2d/kinematic_char/princess.png
new file mode 100644
index 0000000000..9605c9c831
--- /dev/null
+++ b/demos/2d/kinematic_char/princess.png
diff --git a/demos/2d/kinematic_col/colworld.scn b/demos/2d/kinematic_col/colworld.scn
new file mode 100644
index 0000000000..064ff12075
--- /dev/null
+++ b/demos/2d/kinematic_col/colworld.scn
diff --git a/demos/2d/kinematic_col/engine.cfg b/demos/2d/kinematic_col/engine.cfg
new file mode 100644
index 0000000000..654288a9bd
--- /dev/null
+++ b/demos/2d/kinematic_col/engine.cfg
@@ -0,0 +1,12 @@
+[application]
+
+name="Kinematic Collision"
+main_scene="res://colworld.scn"
+icon="res://icon.png"
+
+[input]
+
+move_up=[key(Up)]
+move_left=[key(Left)]
+move_right=[key(Right)]
+move_bottom=[key(Down)]
diff --git a/demos/2d/kinematic_col/icon.png b/demos/2d/kinematic_col/icon.png
new file mode 100644
index 0000000000..2774de6110
--- /dev/null
+++ b/demos/2d/kinematic_col/icon.png
diff --git a/demos/2d/kinematic_col/obstacle.png b/demos/2d/kinematic_col/obstacle.png
new file mode 100644
index 0000000000..693f115a98
--- /dev/null
+++ b/demos/2d/kinematic_col/obstacle.png
diff --git a/demos/2d/kinematic_col/player.gd b/demos/2d/kinematic_col/player.gd
new file mode 100644
index 0000000000..36784a9d9f
--- /dev/null
+++ b/demos/2d/kinematic_col/player.gd
@@ -0,0 +1,36 @@
+
+extends KinematicBody2D
+
+# This is a simple collision demo showing how
+# the kinematic cotroller works.
+# move() will allow to move the node, and will
+# always move it to a non-colliding spot, 
+# as long as it starts from a non-colliding spot too.
+
+
+#pixels / second
+const MOTION_SPEED=160
+
+func _fixed_process(delta):
+
+	var motion = Vector2()
+	
+	if (Input.is_action_pressed("move_up")):
+		motion+=Vector2(0,-1)
+	if (Input.is_action_pressed("move_bottom")):
+		motion+=Vector2(0,1)
+	if (Input.is_action_pressed("move_left")):
+		motion+=Vector2(-1,0)
+	if (Input.is_action_pressed("move_right")):
+		motion+=Vector2(1,0)
+	
+	motion = motion.normalized() * MOTION_SPEED * delta
+	move(motion)
+	
+
+func _ready():
+	# Initalization here
+	set_fixed_process(true)
+	pass
+
+
diff --git a/demos/2d/kinematic_col/player.png b/demos/2d/kinematic_col/player.png
new file mode 100644
index 0000000000..0e7d843899
--- /dev/null
+++ b/demos/2d/kinematic_col/player.png
diff --git a/demos/2d/kinematic_col/player.scn b/demos/2d/kinematic_col/player.scn
new file mode 100644
index 0000000000..e558bffe8e
--- /dev/null
+++ b/demos/2d/kinematic_col/player.scn
diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 166ee4daa8..afdca4a3be 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -744,192 +744,149 @@ bool KinematicBody2D::_ignores_mode(Physics2DServer::BodyMode p_mode) const {
 	return true;
 }
 
-bool KinematicBody2D::is_trapped() const {
-
-	ERR_FAIL_COND_V(!is_inside_scene(),false);
-
-	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
-	ERR_FAIL_COND_V(!dss,false);
-
-	const int max_shapes=32;
-	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
-
-	Set<RID> exclude;
-	exclude.insert(get_rid());
-
-
-	for(int i=0;i<get_shape_count();i++) {
-
-
-		int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),sr,max_shapes,exclude);
-
-		for(int j=0;j<res;j++) {
-
-			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
-			if (!_ignores_mode(bm)) {
-				return true; //it's indeed trapped
-			}
-
-		}
-
-	}
-
-	return false;
+Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 
-}
-void KinematicBody2D::untrap() {
+	//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..
 
-	//this is reaaaaaaaaally wild, will probably only work for simple cases
-	ERR_FAIL_COND(!is_inside_scene());
 
+	colliding=false;
+	ERR_FAIL_COND_V(!is_inside_scene(),Vector2());
 	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
-	ERR_FAIL_COND(!dss);
-
+	ERR_FAIL_COND_V(!dss,Vector2());
 	const int max_shapes=32;
-	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
-	const int max_contacts=8;
-	Vector2 pairs[max_contacts*2];
+	Vector2 sr[max_shapes*2];
+	int res_shapes;
 
 	Set<RID> exclude;
 	exclude.insert(get_rid());
 
-	Vector2 untrap_vec;
-
-	for(int i=0;i<get_shape_count();i++) {
 
-		Matrix32 shape_xform = get_global_transform() * get_shape_transform(i);
-		int res = dss->intersect_shape(get_shape(i)->get_rid(), shape_xform,Vector2(),sr,max_shapes,exclude);
+	//recover first
+	int recover_attempts=4;
 
-		for(int j=0;j<res;j++) {
+	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;
 
-			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
-			if (_ignores_mode(bm)) {
-				exclude.insert(sr[j].rid);
-			} else {
+//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
 
-				int rc;
-				bool c = Physics2DServer::get_singleton()->body_collide_shape(sr[j].rid,sr[j].shape,get_shape(i)->get_rid(),shape_xform,Vector2(),pairs,max_contacts,rc);
+	//print_line("margin: "+rtos(margin));
+	do {
 
-				if (c) {
-
-					for(int k=0;k<rc;k++) {
+		//fill exclude list..
+		for(int i=0;i<get_shape_count();i++) {
 
-						untrap_vec+=pairs[k*2+0]-pairs[k*2+1];
-					}
-				}
 
-			}
+			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,0,mask))
+				collided=true;
 
 		}
 
-	}
-
-	untrap_vec += untrap_vec.normalized()*margin;
+		if (!collided)
+			break;
 
+		Vector2 recover_motion;
 
-	Matrix32 gt = get_global_transform();
-	gt.elements[2]+=untrap_vec;
-	set_global_transform(gt);
+		for(int i=0;i<res_shapes;i++) {
 
-}
+			Vector2 a = sr[i*2+0];
+			Vector2 b = sr[i*2+1];
 
-Vector2 KinematicBody2D::move(const Vector2& p_motion) {
+			float d = a.distance_to(b);
 
-	colliding=false;
-	ERR_FAIL_COND_V(!is_inside_scene(),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;
-	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+			//if (d<margin)
+			///	continue;
+			recover_motion+=(b-a)*0.2;
+		}
 
-	float best_travel = 1e20;
-	Physics2DDirectSpaceState::MotionCastCollision mcc_final;
-	Set<RID> exclude;
-	exclude.insert(get_rid());
+		if (recover_motion==Vector2()) {
+			collided=false;
+			break;
+		}
 
-	print_line("pos: "+get_global_pos());
-	print_line("mlen: "+p_motion);
+		Matrix32 gt = get_global_transform();
+		gt.elements[2]+=recover_motion;
+		set_global_transform(gt);
 
-	if (!collide_static || ! collide_rigid || !collide_character || !collide_kinematic) {
-		//fill exclude list..
-		for(int i=0;i<get_shape_count();i++) {
+		recover_attempts--;
 
+	} while (recover_attempts);
 
-			int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),p_motion,sr,max_shapes,exclude);
 
-			for(int j=0;j<res;j++) {
-
-				Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
-				if (_ignores_mode(bm)) {
-					exclude.insert(sr[j].rid);
-				} else {
-				//	print_line("DANGER???");
-				}
-			}
-		}
-	}
+	//move second
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
 
 	for(int i=0;i<get_shape_count();i++) {
 
-		Physics2DDirectSpaceState::MotionCastCollision mcc;
 
-		bool res = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, mcc,exclude,0);
-		if (res==false)
+		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,0,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 (mcc.travel<=0) {
-			//uh it's trapped
-			colliding=false;
-			return p_motion;
 		}
-		if (mcc.travel < best_travel) {
+		if (lsafe < safe) {
 
-			mcc_final=mcc;
-			best_travel=mcc.travel;
+			safe=lsafe;
+			unsafe=lunsafe;
+			best_shape=i;
 		}
 	}
 
-	float motion;
-	Vector2 motion_ret;
-	Vector2 push;
-	if (best_travel>1) {
+
+	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
+
+	if (safe>=1) {
 		//not collided
 		colliding=false;
-		motion=p_motion.length(); //no stopped
 	} else {
 
-		colliding=true;
-		collision=mcc_final.point;
-		normal=mcc_final.normal;
-		collider=mcc_final.collider_id;
-		Vector2 mnormal=p_motion.normalized();
-
-		float sine = Math::abs(mnormal.dot(normal));
-		float retreat=0;
-		motion = p_motion.length()*mcc_final.travel;
-
-		if (sine==0) {
-			//something odd going on, do not allow motion?
-
-			retreat=motion;
-
+		//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,0,mask);
+		if (!c2) {
+			//should not happen, but floating point precision is so weird..
+			colliding=false;
 		} else {
 
-			retreat = margin/sine;
-			if (retreat>motion)
-				retreat=motion;
+			//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;
 		}
 
-		motion_ret=p_motion.normalized() * ( p_motion.length() - motion);
-		motion-=retreat;
-
-
 	}
 
+	Vector2 motion=p_motion*safe;
 	Matrix32 gt = get_global_transform();
-	gt.elements[2]+=p_motion.normalized()*motion;
+	gt.elements[2]+=motion;
 	set_global_transform(gt);
 
-	return motion_ret;
+	return p_motion-motion;
 
 }
 
@@ -938,18 +895,31 @@ Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
 	return move(p_position-get_global_pos());
 }
 
-bool KinematicBody2D::can_move_to(const Vector2& p_position) {
+bool KinematicBody2D::can_move_to(const Vector2& p_position, bool p_discrete) {
 
 	ERR_FAIL_COND_V(!is_inside_scene(),false);
 	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
 	ERR_FAIL_COND_V(!dss,false);
 
-	const int max_shapes=32;
-	Physics2DDirectSpaceState::ShapeResult sr[max_shapes];
+	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;
 
 	Vector2 motion = p_position-get_global_pos();
+	Matrix32 xform=get_global_transform();
+
+	if (p_discrete) {
+
+		xform.elements[2]+=motion;
+		motion=Vector2();
+	}
 
-	Physics2DDirectSpaceState::MotionCastCollision mcc_final;
 	Set<RID> exclude;
 	exclude.insert(get_rid());
 
@@ -957,19 +927,9 @@ bool KinematicBody2D::can_move_to(const Vector2& p_position) {
 	for(int i=0;i<get_shape_count();i++) {
 
 
-		int res = dss->intersect_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),motion,sr,max_shapes,exclude);
-
-		for(int j=0;j<res;j++) {
-
-			Physics2DServer::BodyMode bm = Physics2DServer::get_singleton()->body_get_mode(sr[j].rid);
-			if (_ignores_mode(bm)) {
-				exclude.insert(sr[j].rid);
-				continue;
-			}
-
-			return false; //omg collided
-
-		}
+		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),motion,0,NULL,0,exclude,0,mask);
+		if (col)
+			return false;
 	}
 
 	return true;
@@ -993,6 +953,12 @@ Vector2 KinematicBody2D::get_collision_normal() const {
 	return normal;
 
 }
+
+Vector2 KinematicBody2D::get_collider_velocity() const {
+
+	return collider_vel;
+}
+
 ObjectID KinematicBody2D::get_collider() const {
 
 	ERR_FAIL_COND_V(!colliding,0);
@@ -1051,9 +1017,6 @@ float KinematicBody2D::get_collision_margin() const{
 void KinematicBody2D::_bind_methods() {
 
 
-	ObjectTypeDB::bind_method(_MD("is_trapped"),&KinematicBody2D::is_trapped);
-	ObjectTypeDB::bind_method(_MD("untrap"),&KinematicBody2D::untrap);
-
 	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody2D::move);
 	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
 
@@ -1063,6 +1026,7 @@ void KinematicBody2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody2D::get_collision_pos);
 	ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody2D::get_collision_normal);
+	ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&KinematicBody2D::get_collider_velocity);
 	ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody2D::get_collider);
 
 
@@ -1085,7 +1049,7 @@ void KinematicBody2D::_bind_methods() {
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies"));
 	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies"));
-	ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.01,256,0.01"),_SCS("set_collision_margin"),_SCS("get_collision_margin"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_margin"));
 
 
 }
@@ -1100,7 +1064,7 @@ KinematicBody2D::KinematicBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_KI
 	colliding=false;
 	collider=0;
 
-	margin=1;
+	margin=0.01;
 }
 KinematicBody2D::~KinematicBody2D()  {
 
diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h
index 6596c0ce04..e7b65b1ef3 100644
--- a/scene/2d/physics_body_2d.h
+++ b/scene/2d/physics_body_2d.h
@@ -243,6 +243,7 @@ class KinematicBody2D : public PhysicsBody2D {
 	bool colliding;
 	Vector2 collision;
 	Vector2 normal;
+	Vector2 collider_vel;
 	ObjectID collider;
 
 
@@ -254,16 +255,14 @@ protected:
 	static void _bind_methods();
 public:
 
-	bool is_trapped() const;
-	void untrap();
-
 	Vector2 move(const Vector2& p_motion);
 	Vector2 move_to(const Vector2& p_position);
 
-	bool can_move_to(const Vector2& p_position);
+	bool can_move_to(const Vector2& p_position,bool p_discrete=false);
 	bool is_colliding() const;
 	Vector2 get_collision_pos() const;
 	Vector2 get_collision_normal() const;
+	Vector2 get_collider_velocity() const;
 	ObjectID get_collider() const;
 
 	void set_collide_with_static_bodies(bool p_enable);
diff --git a/scene/main/node.cpp b/scene/main/node.cpp
index 1f54040de4..5ac09e837f 100644
--- a/scene/main/node.cpp
+++ b/scene/main/node.cpp
@@ -462,7 +462,7 @@ void Node::_set_name_nocheck(const StringName& p_name) {
 
 void Node::set_name(const String& p_name) {
 	
-	String name=p_name.replace(":","").replace("/","");
+	String name=p_name.replace(":","").replace("/","").replace("@","");
 
 	ERR_FAIL_COND(name=="");
 	data.name=name;
@@ -479,45 +479,97 @@ void Node::set_name(const String& p_name) {
 	}
 }
 
+static bool node_hrcr=false;
+static SafeRefCount node_hrcr_count;
+
+void Node::init_node_hrcr() {
+	node_hrcr_count.init(1);
+}
+
+void Node::set_human_readable_collision_renaming(bool p_enabled) {
+
+	node_hrcr=p_enabled;
+}
+
+
 void Node::_validate_child_name(Node *p_child) {
 
 	/* Make sure the name is unique */
-	String basename = p_child->data.name;
 
-	if (basename=="") {
-		
-		basename = p_child->get_type();
-	}
-		
-	int val=1;
-	
-	for(;;) {
-		
-		String attempted = val > 1 ? (basename + " " +itos(val) ) : basename;
+	if (node_hrcr) {
 
-		bool found=false;
-		
-		for (int i=0;i<data.children.size();i++) {
-			
-			if (data.children[i]==p_child)
+		//this approach to autoset node names is human readable but very slow
+		//it's turned on while running in the editor
+
+		String basename = p_child->data.name;
+
+		if (basename=="") {
+
+			basename = p_child->get_type();
+		}
+
+		int val=1;
+
+		for(;;) {
+
+			String attempted = val > 1 ? (basename + " " +itos(val) ) : basename;
+
+			bool found=false;
+
+			for (int i=0;i<data.children.size();i++) {
+
+				if (data.children[i]==p_child)
+					continue;
+				if (data.children[i]->get_name() == attempted) {
+					found=true;
+					break;
+				}
+
+			}
+
+			if (found) {
+
+				val++;
 				continue;
-			if (data.children[i]->get_name() == attempted) {
-				found=true;
-				break;
 			}
-			
+
+			p_child->data.name=attempted;
+			break;
 		}
-		
-		if (found) {
-			
-			val++;
-			continue;
+	} else {
+
+		//this approach to autoset node names is fast but not as readable
+		//it's the default and reserves the '@' character for unique names.
+
+		bool unique=true;
+
+		if (p_child->data.name==StringName() || p_child->data.name.operator String()[0]=='@') {
+			//new unique name must be assigned
+			unique=false;
+		} else {
+			//check if exists
+			Node **childs=data.children.ptr();
+			int cc = data.children.size();
+
+			for(int i=0;i<cc;i++) {
+				if (childs[i]->data.name==p_child->data.name) {
+					unique=false;
+					break;
+				}
+			}
+		}
+
+		if (!unique) {
+
+			node_hrcr_count.ref();
+#ifdef DEBUG_ENABLED
+			String name = "@"+String(p_child->get_type_name())+itos(node_hrcr_count.get());
+#else
+			String name = "@"+itos(node_hrcr_count.get());
+#endif
+			p_child->data.name=name;
 		}
-		
-		p_child->data.name=attempted;
-		break;
 	}
-	
 }
 
 void Node::_add_child_nocheck(Node* p_child,const StringName& p_name) {
@@ -541,6 +593,7 @@ void Node::_add_child_nocheck(Node* p_child,const StringName& p_name) {
 
 }
 
+
 void Node::add_child(Node *p_child) {
 
 	ERR_FAIL_NULL(p_child);
diff --git a/scene/main/node.h b/scene/main/node.h
index f38a782181..ec03fb19e8 100644
--- a/scene/main/node.h
+++ b/scene/main/node.h
@@ -261,6 +261,9 @@ public:
 
 	void queue_delete();
 
+	static void set_human_readable_collision_renaming(bool p_enabled);
+	static void init_node_hrcr();
+
 	/* CANVAS */
 
 	Node();
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index ae67023d5b..007ecb88b7 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -223,6 +223,7 @@ void register_scene_types() {
 
 	OS::get_singleton()->yield(); //may take time to init
 
+	Node::init_node_hrcr();
 
 #ifdef OLD_SCENE_FORMAT_ENABLED
 	ObjectTypeDB::register_type<SceneIO>();
@@ -452,7 +453,7 @@ void register_scene_types() {
 	ObjectTypeDB::register_virtual_type<PhysicsBody2D>();
 	ObjectTypeDB::register_type<StaticBody2D>();
 	ObjectTypeDB::register_type<RigidBody2D>();
-	//ObjectTypeDB::register_type<KinematicBody2D>();
+	ObjectTypeDB::register_type<KinematicBody2D>();
 	ObjectTypeDB::register_type<Area2D>();
 	ObjectTypeDB::register_type<CollisionShape2D>();
 	ObjectTypeDB::register_type<CollisionPolygon2D>();
diff --git a/script/gdscript/gd_editor.cpp b/script/gdscript/gd_editor.cpp
index 4bb5d3206c..c10cadf83f 100644
--- a/script/gdscript/gd_editor.cpp
+++ b/script/gdscript/gd_editor.cpp
@@ -282,6 +282,14 @@ void GDScriptLanguage::get_public_functions(List<MethodInfo> *p_functions) const
 	}
 }
 
+void GDScriptLanguage::get_public_constants(List<Pair<String,Variant> > *p_constants) const {
+
+	Pair<String,Variant> pi;
+	pi.first="PI";
+	pi.second=Math_PI;
+	p_constants->push_back(pi);
+}
+
 String GDScriptLanguage::make_function(const String& p_class,const String& p_name,const StringArray& p_args) const {
 
 	String s="func "+p_name+"(";
diff --git a/script/gdscript/gd_script.h b/script/gdscript/gd_script.h
index bb9beaaf56..70dec4e8ee 100644
--- a/script/gdscript/gd_script.h
+++ b/script/gdscript/gd_script.h
@@ -440,6 +440,8 @@ public:
 	virtual void frame();
 
 	virtual void get_public_functions(List<MethodInfo> *p_functions) const;
+	virtual void get_public_constants(List<Pair<String,Variant> > *p_constants) const;
+
 	/* LOADER FUNCTIONS */
 
 	virtual void get_recognized_extensions(List<String> *p_extensions) const;
diff --git a/script/multiscript/multi_script.h b/script/multiscript/multi_script.h
index a67cedc56b..87d4b4e4c8 100644
--- a/script/multiscript/multi_script.h
+++ b/script/multiscript/multi_script.h
@@ -148,6 +148,7 @@ public:
 
 	virtual void get_recognized_extensions(List<String> *p_extensions) const {}
 	virtual void get_public_functions(List<MethodInfo> *p_functions) const {}
+	virtual void get_public_constants(List<Pair<String,Variant> > *p_constants) const {}
 
 	MultiScriptLanguage() { singleton=this; }
 	virtual ~MultiScriptLanguage() {};
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 14cae3dbb0..ffe47e0267 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -62,6 +62,7 @@ class Body2DSW : public CollisionObject2DSW {
 	Vector2 applied_force;
 	real_t applied_torque;
 
+
 	SelfList<Body2DSW> active_list;
 	SelfList<Body2DSW> inertia_update_list;
 	SelfList<Body2DSW> direct_state_query_list;
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 129c9ecb9c..0f08f63937 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -434,8 +434,9 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const
 		if (E->key()->pass==pass)
 			continue;
 
-		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+		if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
 			continue;
+		}
 
 		if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
 			continue;
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 3a5c8c3ade..e07dca472b 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -218,4 +218,5 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
 	type=p_type;
 	space=NULL;
 	instance_id=0;
+	user_mask=0;
 }
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 74457cfa0a..8138cfcc69 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -65,6 +65,7 @@ private:
 	Space2DSW *space;
 	Matrix32 transform;
 	Matrix32 inv_transform;
+	uint32_t user_mask;
 	bool _static;
 
 	void _update_shapes();
@@ -117,6 +118,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 remove_shape(Shape2DSW *p_shape);
 	void remove_shape(int p_index);
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index fdbbebefcf..7d85183645 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -321,7 +321,7 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po
 
 
 
-template<class ShapeA, class ShapeB,bool castA=false,bool castB=false>
+template<class ShapeA, class ShapeB,bool castA=false,bool castB=false, bool withMargin=false>
 class SeparatorAxisTest2D {
 
 	const ShapeA *shape_A;
@@ -334,6 +334,8 @@ class SeparatorAxisTest2D {
 	int best_axis_index;
 	Vector2 motion_A;
 	Vector2 motion_B;
+	real_t margin_A;
+	real_t margin_B;
 	_CollectorCallback2D *callback;
 
 public:
@@ -397,6 +399,13 @@ public:
 		else
 			shape_B->project_range(axis,*transform_B,min_B,max_B);
 
+		if (withMargin) {
+			min_A-=margin_A;
+			max_A+=margin_A;
+			min_B-=margin_B;
+			max_B+=margin_B;
+		}
+
 		min_B -= ( max_A - min_A ) * 0.5;
 		max_B += ( max_A - min_A ) * 0.5;
 
@@ -468,6 +477,14 @@ public:
 			}
 		}
 
+		if (withMargin) {
+
+			for(int i=0;i<support_count_A;i++) {
+				supports_A[i]+=-best_axis*margin_A;
+			}
+
+		}
+
 
 
 		Vector2 supports_B[max_supports];
@@ -480,6 +497,15 @@ public:
 				supports_B[i] = transform_B->xform(supports_B[i]);
 			}
 		}
+
+		if (withMargin) {
+
+			for(int i=0;i<support_count_B;i++) {
+				supports_B[i]+=best_axis*margin_B;
+			}
+
+		}
+
 /*
 
 
@@ -517,7 +543,10 @@ public:
 
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2()) {
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2(),real_t p_margin_A=0,real_t p_margin_B=0) {
+
+		margin_A=p_margin_A;
+		margin_B=p_margin_B;
 		best_depth=1e15;
 		shape_A=p_shape_A;
 		shape_B=p_shape_B;
@@ -548,16 +577,16 @@ public:
 		(castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) )
 
 
-typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&);
+typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,float,float);
 
 
-template<bool castA, bool castB>
-static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -570,24 +599,39 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr
 	if (!separator.test_cast())
 		return;
 
+
 	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 	if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b)))
 		return;
 
+	if (withMargin) {
+		//points grow to circles
+
+
+		if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_a())) )
+			return;
+		if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_b())) )
+			return;
+		if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_a())) )
+			return;
+		if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) )
+			return;
+	}
+
 	separator.generate_contacts();
 
 }
 
-template<bool castA, bool castB>
-static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -612,13 +656,13 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_tra
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -635,17 +679,55 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_
 	if (!separator.test_axis(p_transform_b.elements[1].normalized()))
 		return;
 
+	if (withMargin) {
+
+		Matrix32 inv = p_transform_b.affine_inverse();
+
+		Vector2 a = p_transform_a.xform(segment_A->get_a());
+		Vector2 b = p_transform_a.xform(segment_A->get_b());
+
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a)))
+			return;
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b)))
+			return;
+
+		if (castA) {
+
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a+p_motion_a)))
+				return;
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b+p_motion_a)))
+				return;
+		}
+
+		if (castB) {
+
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b)))
+				return;
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b)))
+				return;
+		}
+
+		if (castA && castB) {
+
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b+p_motion_a)))
+				return;
+			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b+p_motion_a)))
+				return;
+		}
+
+	}
+
 	separator.generate_contacts();
 
 }
 
-template<bool castA, bool castB>
-static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -671,13 +753,13 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -692,6 +774,16 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3
 
 		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
+
+		if (withMargin) {
+
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) )))
+				return;
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) )))
+				return;
+
+		}
+
 	}
 
 	separator.generate_contacts();
@@ -701,14 +793,14 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3
 
 /////////
 
-template<bool castA, bool castB>
-static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -724,14 +816,14 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_tran
 
 }
 
-template<bool castA, bool castB>
-static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -741,7 +833,7 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
 
 	const Vector2 &sphere=p_transform_a.elements[2];
 	const Vector2 *axis=&p_transform_b.elements[0];
-	const Vector2& half_extents = rectangle_B->get_half_extents();
+//	const Vector2& half_extents = rectangle_B->get_half_extents();
 
 	if (!separator.test_axis(axis[0].normalized()))
 		return;
@@ -749,75 +841,45 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
 	if (!separator.test_axis(axis[1].normalized()))
 		return;
 
+	Matrix32 binv = p_transform_b.affine_inverse();
 	{
-		Vector2 local_v = p_transform_b.affine_inverse().xform(p_transform_a.get_origin());
-
-		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
 
-
-		if (!separator.test_axis((p_transform_b.xform(he)-sphere).normalized()))
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) )
 			return;
 	}
 
 	if (castA) {
 
 		Vector2 sphereofs = sphere + p_motion_a;
-		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
-
-		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
-
-
-		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
 			return;
 	}
 
 	if (castB) {
 
 		Vector2 sphereofs = sphere - p_motion_b;
-		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
-
-		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
-
-
-		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
 			return;
 	}
 
 	if (castA && castB) {
 
 		Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
-		Vector2 local_v = p_transform_b.affine_inverse().xform(sphereofs);
-
-		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
-
-
-		if (!separator.test_axis((p_transform_b.xform(he)-sphereofs).normalized()))
+		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
 			return;
 	}
 
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -840,14 +902,14 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_tra
 
 }
 
-template<bool castA, bool castB>
-static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -872,14 +934,14 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32
 
 /////////
 
-template<bool castA, bool castB>
-static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -901,17 +963,56 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32&
 	if (!separator.test_axis(p_transform_b.elements[1].normalized()))
 		return;
 
+	if (withMargin) {
+
+		Matrix32 invA=p_transform_a.affine_inverse();
+		Matrix32 invB=p_transform_b.affine_inverse();
+
+		if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,p_transform_b,invB) ) )
+			return;
+
+		if (castA || castB) {
+
+			Matrix32 aofs = p_transform_a;
+			aofs.elements[2]+=p_motion_a;
+
+			Matrix32 bofs = p_transform_b;
+			bofs.elements[2]+=p_motion_b;
+
+			Matrix32 aofsinv = aofs.affine_inverse();
+			Matrix32 bofsinv = bofs.affine_inverse();
+
+			if (castA) {
+
+				if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,p_transform_b,invB) ) )
+					return;
+			}
+
+			if (castB) {
+
+				if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,bofs,bofsinv) ) )
+					return;
+			}
+
+			if (castA && castB) {
+
+				if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) )
+					return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -940,15 +1041,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 		{
 			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
 
-			const Vector2& half_extents = rectangle_A->get_half_extents();
-			Vector2 local_v = boxinv.xform(capsule_endpoint);
-
-			Vector2 he(
-				(local_v.x<0) ? -half_extents.x : half_extents.x,
-				(local_v.y<0) ? -half_extents.y : half_extents.y
-			);
-
-			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
 				return;
 		}
 
@@ -957,16 +1050,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
 			capsule_endpoint-=p_motion_a;
 
-
-			const Vector2& half_extents = rectangle_A->get_half_extents();
-			Vector2 local_v = boxinv.xform(capsule_endpoint);
-
-			Vector2 he(
-				(local_v.x<0) ? -half_extents.x : half_extents.x,
-				(local_v.y<0) ? -half_extents.y : half_extents.y
-			);
-
-			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
 				return;
 		}
 
@@ -974,16 +1058,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
 			capsule_endpoint+=p_motion_b;
 
-
-			const Vector2& half_extents = rectangle_A->get_half_extents();
-			Vector2 local_v = boxinv.xform(capsule_endpoint);
-
-			Vector2 he(
-				(local_v.x<0) ? -half_extents.x : half_extents.x,
-				(local_v.y<0) ? -half_extents.y : half_extents.y
-			);
-
-			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
 				return;
 		}
 
@@ -993,15 +1068,7 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 			capsule_endpoint+=p_motion_b;
 
 
-			const Vector2& half_extents = rectangle_A->get_half_extents();
-			Vector2 local_v = boxinv.xform(capsule_endpoint);
-
-			Vector2 he(
-				(local_v.x<0) ? -half_extents.x : half_extents.x,
-				(local_v.y<0) ? -half_extents.y : half_extents.y
-			);
-
-			if (!separator.test_axis(p_transform_a.xform(he-capsule_endpoint).normalized()))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
 				return;
 		}
 
@@ -1011,13 +1078,13 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB>
-static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1033,10 +1100,36 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
 		return;
 
 	//convex faces
+	Matrix32 boxinv;
+	if (withMargin) {
+		boxinv=p_transform_a.affine_inverse();
+	}
 	for(int i=0;i<convex_B->get_point_count();i++) {
 
 		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
+
+		if (withMargin) {
+			//all points vs all points need to be tested if margin exist
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i)))))
+				return;
+			if (castA) {
+
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a)))
+					return;
+			}
+			if (castB) {
+
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b)))
+					return;
+			}
+			if (castA && castB) {
+
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a)))
+					return;
+			}
+
+		}
 	}
 
 	separator.generate_contacts();
@@ -1046,14 +1139,14 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
 
 /////////
 
-template<bool castA, bool castB>
-static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
 	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1089,14 +1182,14 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 
 }
 
-template<bool castA, bool castB>
-static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
 
-	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1135,14 +1228,14 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
 /////////
 
 
-template<bool castA, bool castB>
-static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b) {
+template<bool castA, bool castB,bool withMargin>
+static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
 
 
 	const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
 	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
 
-	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b);
+	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1161,6 +1254,19 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const
 
 		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
 			return;
+
+	}
+
+	if (withMargin) {
+
+		for(int i=0;i<convex_A->get_point_count();i++) {
+			for(int j=0;j<convex_B->get_point_count();j++) {
+
+				if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)) , p_transform_b.xform(convex_B->get_point(j))))
+					return;
+			}
+		}
+
 	}
 
 	separator.generate_contacts();
@@ -1170,7 +1276,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const
 
 ////////
 
-bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis) {
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
 
 	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
 
@@ -1186,121 +1292,238 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
 
 
 	static const CollisionFunc collision_table[5][5]={
-		{_collision_segment_segment<false,false>,
-		 _collision_segment_circle<false,false>,
-		 _collision_segment_rectangle<false,false>,
-		 _collision_segment_capsule<false,false>,
-		 _collision_segment_convex_polygon<false,false>},
+		{_collision_segment_segment<false,false,false>,
+		 _collision_segment_circle<false,false,false>,
+		 _collision_segment_rectangle<false,false,false>,
+		 _collision_segment_capsule<false,false,false>,
+		 _collision_segment_convex_polygon<false,false,false>},
 		{0,
-		 _collision_circle_circle<false,false>,
-		 _collision_circle_rectangle<false,false>,
-		 _collision_circle_capsule<false,false>,
-		 _collision_circle_convex_polygon<false,false>},
+		 _collision_circle_circle<false,false,false>,
+		 _collision_circle_rectangle<false,false,false>,
+		 _collision_circle_capsule<false,false,false>,
+		 _collision_circle_convex_polygon<false,false,false>},
 		{0,
 		 0,
-		 _collision_rectangle_rectangle<false,false>,
-		 _collision_rectangle_capsule<false,false>,
-		 _collision_rectangle_convex_polygon<false,false>},
+		 _collision_rectangle_rectangle<false,false,false>,
+		 _collision_rectangle_capsule<false,false,false>,
+		 _collision_rectangle_convex_polygon<false,false,false>},
 		{0,
 		 0,
 		 0,
-		 _collision_capsule_capsule<false,false>,
-		 _collision_capsule_convex_polygon<false,false>},
+		 _collision_capsule_capsule<false,false,false>,
+		 _collision_capsule_convex_polygon<false,false,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<false,false>}
+		 _collision_convex_polygon_convex_polygon<false,false,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castA[5][5]={
-		{_collision_segment_segment<true,false>,
-		 _collision_segment_circle<true,false>,
-		 _collision_segment_rectangle<true,false>,
-		 _collision_segment_capsule<true,false>,
-		 _collision_segment_convex_polygon<true,false>},
+		{_collision_segment_segment<true,false,false>,
+		 _collision_segment_circle<true,false,false>,
+		 _collision_segment_rectangle<true,false,false>,
+		 _collision_segment_capsule<true,false,false>,
+		 _collision_segment_convex_polygon<true,false,false>},
 		{0,
-		 _collision_circle_circle<true,false>,
-		 _collision_circle_rectangle<true,false>,
-		 _collision_circle_capsule<true,false>,
-		 _collision_circle_convex_polygon<true,false>},
+		 _collision_circle_circle<true,false,false>,
+		 _collision_circle_rectangle<true,false,false>,
+		 _collision_circle_capsule<true,false,false>,
+		 _collision_circle_convex_polygon<true,false,false>},
 		{0,
 		 0,
-		 _collision_rectangle_rectangle<true,false>,
-		 _collision_rectangle_capsule<true,false>,
-		 _collision_rectangle_convex_polygon<true,false>},
+		 _collision_rectangle_rectangle<true,false,false>,
+		 _collision_rectangle_capsule<true,false,false>,
+		 _collision_rectangle_convex_polygon<true,false,false>},
 		{0,
 		 0,
 		 0,
-		 _collision_capsule_capsule<true,false>,
-		 _collision_capsule_convex_polygon<true,false>},
+		 _collision_capsule_capsule<true,false,false>,
+		 _collision_capsule_convex_polygon<true,false,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<true,false>}
+		 _collision_convex_polygon_convex_polygon<true,false,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castB[5][5]={
-		{_collision_segment_segment<false,true>,
-		 _collision_segment_circle<false,true>,
-		 _collision_segment_rectangle<false,true>,
-		 _collision_segment_capsule<false,true>,
-		 _collision_segment_convex_polygon<false,true>},
+		{_collision_segment_segment<false,true,false>,
+		 _collision_segment_circle<false,true,false>,
+		 _collision_segment_rectangle<false,true,false>,
+		 _collision_segment_capsule<false,true,false>,
+		 _collision_segment_convex_polygon<false,true,false>},
 		{0,
-		 _collision_circle_circle<false,true>,
-		 _collision_circle_rectangle<false,true>,
-		 _collision_circle_capsule<false,true>,
-		 _collision_circle_convex_polygon<false,true>},
+		 _collision_circle_circle<false,true,false>,
+		 _collision_circle_rectangle<false,true,false>,
+		 _collision_circle_capsule<false,true,false>,
+		 _collision_circle_convex_polygon<false,true,false>},
 		{0,
 		 0,
-		 _collision_rectangle_rectangle<false,true>,
-		 _collision_rectangle_capsule<false,true>,
-		 _collision_rectangle_convex_polygon<false,true>},
+		 _collision_rectangle_rectangle<false,true,false>,
+		 _collision_rectangle_capsule<false,true,false>,
+		 _collision_rectangle_convex_polygon<false,true,false>},
 		{0,
 		 0,
 		 0,
-		 _collision_capsule_capsule<false,true>,
-		 _collision_capsule_convex_polygon<false,true>},
+		 _collision_capsule_capsule<false,true,false>,
+		 _collision_capsule_convex_polygon<false,true,false>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<false,true>}
+		 _collision_convex_polygon_convex_polygon<false,true,false>}
 
 	};
 
 	static const CollisionFunc collision_table_castA_castB[5][5]={
-		{_collision_segment_segment<true,true>,
-		 _collision_segment_circle<true,true>,
-		 _collision_segment_rectangle<true,true>,
-		 _collision_segment_capsule<true,true>,
-		 _collision_segment_convex_polygon<true,true>},
+		{_collision_segment_segment<true,true,false>,
+		 _collision_segment_circle<true,true,false>,
+		 _collision_segment_rectangle<true,true,false>,
+		 _collision_segment_capsule<true,true,false>,
+		 _collision_segment_convex_polygon<true,true,false>},
+		{0,
+		 _collision_circle_circle<true,true,false>,
+		 _collision_circle_rectangle<true,true,false>,
+		 _collision_circle_capsule<true,true,false>,
+		 _collision_circle_convex_polygon<true,true,false>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<true,true,false>,
+		 _collision_rectangle_capsule<true,true,false>,
+		 _collision_rectangle_convex_polygon<true,true,false>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<true,true,false>,
+		 _collision_capsule_convex_polygon<true,true,false>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<true,true,false>}
+
+	};
+
+	static const CollisionFunc collision_table_margin[5][5]={
+		{_collision_segment_segment<false,false,true>,
+		 _collision_segment_circle<false,false,true>,
+		 _collision_segment_rectangle<false,false,true>,
+		 _collision_segment_capsule<false,false,true>,
+		 _collision_segment_convex_polygon<false,false,true>},
+		{0,
+		 _collision_circle_circle<false,false,true>,
+		 _collision_circle_rectangle<false,false,true>,
+		 _collision_circle_capsule<false,false,true>,
+		 _collision_circle_convex_polygon<false,false,true>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<false,false,true>,
+		 _collision_rectangle_capsule<false,false,true>,
+		 _collision_rectangle_convex_polygon<false,false,true>},
 		{0,
-		 _collision_circle_circle<true,true>,
-		 _collision_circle_rectangle<true,true>,
-		 _collision_circle_capsule<true,true>,
-		 _collision_circle_convex_polygon<true,true>},
+		 0,
+		 0,
+		 _collision_capsule_capsule<false,false,true>,
+		 _collision_capsule_convex_polygon<false,false,true>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<false,false,true>}
+
+	};
+
+	static const CollisionFunc collision_table_castA_margin[5][5]={
+		{_collision_segment_segment<true,false,true>,
+		 _collision_segment_circle<true,false,true>,
+		 _collision_segment_rectangle<true,false,true>,
+		 _collision_segment_capsule<true,false,true>,
+		 _collision_segment_convex_polygon<true,false,true>},
+		{0,
+		 _collision_circle_circle<true,false,true>,
+		 _collision_circle_rectangle<true,false,true>,
+		 _collision_circle_capsule<true,false,true>,
+		 _collision_circle_convex_polygon<true,false,true>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<true,false,true>,
+		 _collision_rectangle_capsule<true,false,true>,
+		 _collision_rectangle_convex_polygon<true,false,true>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<true,false,true>,
+		 _collision_capsule_convex_polygon<true,false,true>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<true,false,true>}
+
+	};
+
+	static const CollisionFunc collision_table_castB_margin[5][5]={
+		{_collision_segment_segment<false,true,true>,
+		 _collision_segment_circle<false,true,true>,
+		 _collision_segment_rectangle<false,true,true>,
+		 _collision_segment_capsule<false,true,true>,
+		 _collision_segment_convex_polygon<false,true,true>},
+		{0,
+		 _collision_circle_circle<false,true,true>,
+		 _collision_circle_rectangle<false,true,true>,
+		 _collision_circle_capsule<false,true,true>,
+		 _collision_circle_convex_polygon<false,true,true>},
+		{0,
+		 0,
+		 _collision_rectangle_rectangle<false,true,true>,
+		 _collision_rectangle_capsule<false,true,true>,
+		 _collision_rectangle_convex_polygon<false,true,true>},
+		{0,
+		 0,
+		 0,
+		 _collision_capsule_capsule<false,true,true>,
+		 _collision_capsule_convex_polygon<false,true,true>},
+		{0,
+		 0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<false,true,true>}
+
+	};
+
+	static const CollisionFunc collision_table_castA_castB_margin[5][5]={
+		{_collision_segment_segment<true,true,true>,
+		 _collision_segment_circle<true,true,true>,
+		 _collision_segment_rectangle<true,true,true>,
+		 _collision_segment_capsule<true,true,true>,
+		 _collision_segment_convex_polygon<true,true,true>},
+		{0,
+		 _collision_circle_circle<true,true,true>,
+		 _collision_circle_rectangle<true,true,true>,
+		 _collision_circle_capsule<true,true,true>,
+		 _collision_circle_convex_polygon<true,true,true>},
 		{0,
 		 0,
-		 _collision_rectangle_rectangle<true,true>,
-		 _collision_rectangle_capsule<true,true>,
-		 _collision_rectangle_convex_polygon<true,true>},
+		 _collision_rectangle_rectangle<true,true,true>,
+		 _collision_rectangle_capsule<true,true,true>,
+		 _collision_rectangle_convex_polygon<true,true,true>},
 		{0,
 		 0,
 		 0,
-		 _collision_capsule_capsule<true,true>,
-		 _collision_capsule_convex_polygon<true,true>},
+		 _collision_capsule_capsule<true,true,true>,
+		 _collision_capsule_convex_polygon<true,true,true>},
 		{0,
 		 0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon<true,true>}
+		 _collision_convex_polygon_convex_polygon<true,true,true>}
 
 	};
 
+
 	_CollectorCallback2D callback;
 	callback.callback=p_result_callback;
 	callback.swap=p_swap;
@@ -1314,32 +1537,49 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_
 	const Matrix32 *transform_B=&p_transform_B;
 	const Vector2 *motion_A=&p_motion_A;
 	const Vector2 *motion_B=&p_motion_B;
+	real_t margin_A=p_margin_A,margin_B=p_margin_B;
 
 	if (type_A > type_B) {
 		SWAP(A,B);
 		SWAP(transform_A,transform_B);
 		SWAP(type_A,type_B);
 		SWAP(motion_A,motion_B);
+		SWAP(margin_A,margin_B);
 		callback.swap = !callback.swap;
 	}
 
 
 	CollisionFunc collision_func;
-	if (*motion_A==Vector2() && *motion_B==Vector2()) {
-		collision_func = collision_table[type_A-2][type_B-2];
-	} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
-		collision_func = collision_table_castA[type_A-2][type_B-2];
-	} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
-		collision_func = collision_table_castB[type_A-2][type_B-2];
+
+	if (p_margin_A || p_margin_B) {
+		if (*motion_A==Vector2() && *motion_B==Vector2()) {
+			collision_func = collision_table_margin[type_A-2][type_B-2];
+		} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
+			collision_func = collision_table_castA_margin[type_A-2][type_B-2];
+		} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
+			collision_func = collision_table_castB_margin[type_A-2][type_B-2];
+		} else {
+			collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2];
+		}
 	} else {
-		collision_func = collision_table_castA_castB[type_A-2][type_B-2];
+
+		if (*motion_A==Vector2() && *motion_B==Vector2()) {
+			collision_func = collision_table[type_A-2][type_B-2];
+		} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
+			collision_func = collision_table_castA[type_A-2][type_B-2];
+		} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
+			collision_func = collision_table_castB[type_A-2][type_B-2];
+		} else {
+			collision_func = collision_table_castA_castB[type_A-2][type_B-2];
+		}
+
 	}
 
 
 
 	ERR_FAIL_COND_V(!collision_func,false);
 
-	collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B);
+	collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B);
 
 	return callback.collided;
 
diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h
index 95468a18b8..be5a3dc79f 100644
--- a/servers/physics_2d/collision_solver_2d_sat.h
+++ b/servers/physics_2d/collision_solver_2d_sat.h
@@ -32,6 +32,6 @@
 #include "collision_solver_2d_sw.h"
 
 
-bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL);
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
 
 #endif // COLLISION_SOLVER_2D_SAT_H
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index 5d43510aea..4c7e68d643 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -150,6 +150,8 @@ struct _ConcaveCollisionInfo2D {
 	const Matrix32 *transform_B;
 	Vector2 motion_A;
 	Vector2 motion_B;
+	real_t margin_A;
+	real_t margin_B;
 	CollisionSolver2DSW::CallbackResult result_callback;
 	void *userdata;
 	bool swap_result;
@@ -169,7 +171,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
 	if (!cinfo.result_callback && cinfo.collided)
 		return; //already collided and no contacts requested, don't test anymore
 
-	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis );
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B );
 	if (!collided)
 		return;
 
@@ -179,7 +181,7 @@ void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
 
 }
 
-bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
+bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
 
 
 	const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
@@ -195,6 +197,8 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
 	cinfo.collided=false;
 	cinfo.collisions=0;
 	cinfo.sep_axis=sep_axis;
+	cinfo.margin_A=p_margin_A;
+	cinfo.margin_B=p_margin_B;
 
 	cinfo.aabb_tests=0;
 
@@ -227,7 +231,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix3
 }
 
 
-bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis) {
+bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
 
 
 
@@ -236,12 +240,14 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
 	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
 	bool concave_A=p_shape_A->is_concave();
 	bool concave_B=p_shape_B->is_concave();
+	real_t margin_A=p_margin_A,margin_B=p_margin_B;
 
 	bool swap = false;
 
 	if (type_A>type_B) {
 		SWAP(type_A,type_B);
 		SWAP(concave_A,concave_B);
+		SWAP(margin_A,margin_B);
 		swap=true;
 	}
 
@@ -292,16 +298,16 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
 			return false;
 
 		if (!swap)
-			return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis);
+			return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
 		else
-			return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis);
+			return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B);
 
 
 
 	} else {
 
 
-		return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis);
+		return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
 	}
 
 
diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h
index 11b5701f46..07141b0d09 100644
--- a/servers/physics_2d/collision_solver_2d_sw.h
+++ b/servers/physics_2d/collision_solver_2d_sw.h
@@ -37,14 +37,14 @@ public:
 private:
 	static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
 	static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
-	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
+	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
 	static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
 
 
 
 public:
 
-	static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL);
+	static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
 
 
 };
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 0fbd461f46..fd1ea579f0 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -132,8 +132,12 @@ real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
 
 void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
 
+
 	CollCbkData *cbk=(CollCbkData *)p_userdata;
 
+	if (cbk->max==0)
+		return;
+
 	if (cbk->amount == cbk->max) {
 		//find least deep
 		float min_depth=1e20;
@@ -159,6 +163,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p
 
 		cbk->ptr[cbk->amount*2+0]=p_point_A;
 		cbk->ptr[cbk->amount*2+1]=p_point_B;
+		cbk->amount++;
 	}
 }
 
@@ -648,19 +653,20 @@ uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const {
 };
 
 
-void Physics2DServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
+void Physics2DServerSW::body_set_user_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);
 
 };
 
-uint32_t Physics2DServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
+uint32_t Physics2DServerSW::body_get_user_mask(RID p_body, uint32_t p_flags) const {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,0);
 
-	return 0;
+	return body->get_user_mask();
 };
 
 void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index ef00eae7e4..e50bb0ab96 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -58,6 +58,12 @@ friend class Physics2DDirectSpaceStateSW;
 	mutable RID_Owner<Body2DSW> body_owner;
 	mutable RID_Owner<Joint2DSW> joint_owner;
 
+
+
+
+//	void _clear_query(Query2DSW *p_query);
+public:
+
 	struct CollCbkData {
 
 		int max;
@@ -68,9 +74,6 @@ friend class Physics2DDirectSpaceStateSW;
 	static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
 
 
-//	void _clear_query(Query2DSW *p_query);
-public:
-
 	virtual RID shape_create(ShapeType p_shape);
 	virtual void shape_set_data(RID p_shape, const Variant& p_data);
 	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
@@ -158,8 +161,8 @@ public:
 	virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
 	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
 
-	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
-	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) 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_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index ba5f60cb32..d3fcf1fab2 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -354,6 +354,51 @@ public:
 		r_max = distance + length;
 	}
 
+
+
+	_FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const Vector2& p_circle) const {
+
+		Vector2 local_v = p_xform_inv.xform(p_circle);
+
+		Vector2 he(
+			(local_v.x<0) ? -half_extents.x : half_extents.x,
+			(local_v.y<0) ? -half_extents.y : half_extents.y
+		);
+
+		return (p_xform.xform(he)-p_circle).normalized();
+	}
+
+	_FORCE_INLINE_ Vector2 get_box_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const RectangleShape2DSW *p_B,const Matrix32& p_B_xform, const Matrix32& p_B_xform_inv) const {
+
+		Vector2 a,b;
+
+		{
+			Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
+
+			Vector2 he(
+				(local_v.x<0) ? -half_extents.x : half_extents.x,
+				(local_v.y<0) ? -half_extents.y : half_extents.y
+			);
+
+			a=p_xform.xform(he);
+
+		}
+		{
+			Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
+
+			Vector2 he(
+				(local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x,
+				(local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y
+			);
+
+			b=p_B_xform.xform(he);
+
+		}
+
+		return (a-b).normalized();
+	}
+
+
 	DEFAULT_PROJECT_RANGE_CAST
 
 };
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 2c714f5065..d1aec92984 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -31,7 +31,22 @@
 #include "physics_2d_server_sw.h"
 
 
-bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+_FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_user_mask, uint32_t p_type_mask) {
+
+	if (p_user_mask && !(p_object->get_user_mask()&p_user_mask))
+		return false;
+
+	if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA && !(p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA))
+		return false;
+
+	Body2DSW *body = static_cast<Body2DSW*>(p_object);
+
+	return (1<<body->get_mode())&p_type_mask;
+
+}
+
+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
+
 
 
 	ERR_FAIL_COND_V(space->locked,false);
@@ -55,8 +70,8 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
 
 	for(int i=0;i<amount;i++) {
 
-		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
-			continue; //ignore area
+		if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+			continue;
 
 		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
 			continue;
@@ -120,7 +135,7 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
 }
 
 
-int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
 
 	if (p_result_max<=0)
 		return 0;
@@ -129,6 +144,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
 	ERR_FAIL_COND_V(!shape,0);
 
 	Rect2 aabb = p_xform.xform(shape->get_aabb());
+	aabb=aabb.grow(p_margin);
 
 	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
 
@@ -137,11 +153,8 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
 
 	for(int i=0;i<amount;i++) {
 
-		if (cc>=p_result_max)
-			break;
-
-		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
-			continue; //ignore area
+		if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+			continue;
 
 		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
 			continue;
@@ -150,7 +163,7 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
 		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
 		int shape_idx=space->intersection_query_subindex_results[i];
 
-		if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL))
+		if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin))
 			continue;
 
 		r_results[cc].collider_id=col_obj->get_instance_id();
@@ -168,193 +181,235 @@ int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matri
 }
 
 
-struct MotionCallbackRayCastData {
 
-	Vector2 best_contact;
-	Vector2 best_normal;
-	float best_len;
-	Matrix32 b_xform_inv;
-	Matrix32 b_xform;
-	Vector2 motion;
-	Shape2DSW * shape_B;
+bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
 
-};
 
-static void _motion_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
 
+	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,false);
+
+	Rect2 aabb = p_xform.xform(shape->get_aabb());
+	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+	aabb=aabb.grow(p_margin);
 
-	MotionCallbackRayCastData *rd=(MotionCallbackRayCastData*)p_userdata;
+	//if (p_motion!=Vector2())
+	//	print_line(p_motion);
 
-	Vector2 contact_normal = (p_point_B-p_point_A).normalized();
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
 
-	Vector2 from=p_point_A-(rd->motion*1.01);
-	Vector2 p,n;
+	float best_safe=1;
+	float best_unsafe=1;
+
+	for(int i=0;i<amount;i++) {
 
-	if (contact_normal.dot(rd->motion.normalized())<CMP_EPSILON) {
-		//safe to assume it was a perpendicular collision
-		n=contact_normal;
-		p=p_point_B;
-	} else {
-		//entered in a different angle
-		Vector2 to = p_point_A+rd->motion; //avoid precission issues
 
+		if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+			continue;
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue; //ignore excluded
 
-		bool res = rd->shape_B->intersect_segment(rd->b_xform_inv.xform(from),rd->b_xform_inv.xform(to),p,n);
 
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
 
-		if (!res) {
-			print_line("lolwut failed");
-			return;
+
+		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(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+			continue;
 		}
 
-		p = rd->b_xform.xform(p);
 
-		n = rd->b_xform_inv.basis_xform_inv(n).normalized();
-	}
+		//test initial overlap
+		if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+
+			return false;
+		}
+
+
+		//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(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin);
+
+			if (collided) {
+
+				hi=ofs;
+			} else {
+
+				low=ofs;
+			}
+		}
 
-	float len = p.distance_to(from);
+		if (low<best_safe) {
+			best_safe=low;
+			best_unsafe=hi;
+		}
 
-	if (len<rd->best_len) {
-		rd->best_contact=p;
-		rd->best_normal=n;
-		rd->best_len=len;
 	}
+
+	p_closest_safe=best_safe;
+	p_closest_unsafe=best_unsafe;
+
+	return true;
+
+
 }
 
-bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
+
+
+	if (p_result_max<=0)
+		return 0;
 
 	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
 	ERR_FAIL_COND_V(!shape,0);
 
-	Rect2 aabb = p_xform.xform(shape->get_aabb());
+	Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
 	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+	aabb=aabb.grow(p_margin);
 
 	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
 
 	bool collided=false;
-	r_result.travel=1;
-
-	MotionCallbackRayCastData best_normal;
-	best_normal.best_len=1e20;
-	for(int i=0;i<amount;i++) {
-
+	int cc=0;
+	r_result_count=0;
+
+	Physics2DServerSW::CollCbkData cbk;
+	cbk.max=p_result_max;
+	cbk.amount=0;
+	cbk.ptr=r_results;
+	CollisionSolver2DSW::CallbackResult cbkres=NULL;
+
+	Physics2DServerSW::CollCbkData *cbkptr=NULL;
+	if (p_result_max>0) {
+		cbkptr=&cbk;
+		cbkres=Physics2DServerSW::_shape_col_cbk;
+	}
 
-		if (space->intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
-			continue; //ignore area
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
-			continue; //ignore excluded
+	for(int i=0;i<amount;i++) {
 
+		if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+			continue;
 
 		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
 		int shape_idx=space->intersection_query_subindex_results[i];
 
+		if (p_exclude.has( col_obj->get_self() ))
+			continue;
 
-		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(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
 
-			continue;
+		if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
+			collided=true;
 		}
 
+	}
 
-		//test initial overlap
-		if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL)) {
-
-			r_result.collider_id=col_obj->get_instance_id();
-			r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
-			r_result.shape=shape_idx;
-			r_result.rid=col_obj->get_self();
-			r_result.travel=0;
-			r_result.point=Vector2();
-			r_result.normal=Vector2();
-			return true;
-		}
+	r_result_count=cbk.amount;
 
-#if 0
-		Vector2 mnormal=p_motion.normalized();
-		Matrix32 col_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
-		ShapeSW *col_shape = col_obj->get_shape(shape_idx);
+	return collided;
+}
 
-		real_t min,max;
-		col_shape->project_rangev(mnormal,col_shape_xform,min,max);
-		real_t width = max-min;
 
-		int a;
-		Vector2 s[2];
-		col_shape->get_supports(col_shape_xform.basis_xform(mnormal).normalized(),s,a);
-		Vector2 from = col_shape_xform.xform(s[0]);
-		Vector2 to = from + p_motion;
+struct _RestCallbackData {
 
-		Matrix32 from_inv = col_shape_xform.affine_inverse();
+	const CollisionObject2DSW *object;
+	const CollisionObject2DSW *best_object;
+	int shape;
+	int best_shape;
+	Vector2 best_contact;
+	Vector2 best_normal;
+	float best_len;
+};
 
-		Vector2 local_from = from_inv.xform(from-mnormal*width*0.1); //start from a little inside the bounding box
-		Vector2 local_to = from_inv.xform(to);
+static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
 
-		Vector2 rpos,rnorm;
-		if (!col_shape->intersect_segment(local_from,local_to,rpos,rnorm))
-			return false;
 
-		//ray hit something
+	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
 
+	Vector2 contact_rel = p_point_B - p_point_A;
+	float len = contact_rel.length();
+	if (len <= rd->best_len)
+		return;
 
-		Vector2 hitpos = p_xform_B.xform(rpos);
-#endif
+	rd->best_len=len;
+	rd->best_contact=p_point_B;
+	rd->best_normal=contact_rel/len;
+	rd->best_object=rd->object;
+	rd->best_shape=rd->shape;
 
-		//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;
+bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_user_mask,uint32_t p_object_type_mask) {
 
-			Vector2 sep=mnormal; //important optimization for this to work fast enough
-			bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep);
 
-			if (collided) {
+	Shape2DSW *shape = static_cast<Physics2DServerSW*>(Physics2DServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
 
-				hi=ofs;
-			} else {
+	Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
+	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
+	aabb=aabb.grow(p_margin);
 
-				low=ofs;
-			}
-		}
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	_RestCallbackData rcd;
+	rcd.best_len=0;
+	rcd.best_object=NULL;
+	rcd.best_shape=0;
 
+	for(int i=0;i<amount;i++) {
 
-		best_normal.shape_B=col_obj->get_shape(shape_idx);
-		best_normal.motion=p_motion*hi;
-		best_normal.b_xform=col_obj_xform;
-		best_normal.b_xform_inv=col_obj_xform.affine_inverse();
 
-		bool sc = CollisionSolver2DSW::solve(shape,p_xform,p_motion*hi,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_motion_cbk_result,&best_normal);
-		print_line("CLD: "+itos(sc));
+		if (!_match_object_type_query(space->intersection_query_results[i],p_user_mask,p_object_type_mask))
+			continue;
 
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
 
-		if (collided && low>=r_result.travel)
+		if (p_exclude.has( col_obj->get_self() ))
 			continue;
 
-		collided=true;
-		r_result.travel=low;
+		rcd.object=col_obj;
+		rcd.shape=shape_idx;
+		bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,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;
 
-		r_result.collider_id=col_obj->get_instance_id();
-		r_result.collider=r_result.collider_id!=0 ? ObjectDB::get_instance(col_obj->get_instance_id()) : NULL;
-		r_result.shape=shape_idx;
-		r_result.rid=col_obj->get_self();
 
 	}
 
-	if (collided) {
-		ERR_FAIL_COND_V(best_normal.best_normal==Vector2(),false);
-		r_result.normal=best_normal.best_normal;
-		r_result.point=best_normal.best_contact;
-	}
+	if (rcd.best_len==0)
+		return false;
 
-	return collided;
+	r_info->collider_id=rcd.best_object->get_instance_id();
+	r_info->shape=rcd.best_shape;
+	r_info->normal=rcd.best_normal;
+	r_info->point=rcd.best_contact;
+	r_info->rid=rcd.best_object->get_self();
+	if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) {
 
+		const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
+		Vector2 rel_vec = r_info->point-body->get_transform().get_origin();
+		r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
+	} else {
+		r_info->linear_velocity=Vector2();
+	}
+
+	return true;
 }
 
 
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 978e88479d..9d3dfae9b5 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -46,9 +46,11 @@ public:
 
 	Space2DSW *space;
 
-	bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
-	int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
-	bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 
 	Physics2DDirectSpaceStateSW();
 };
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
index cbf5cffce6..0851ad59ef 100644
--- a/servers/physics_2d_server.cpp
+++ b/servers/physics_2d_server.cpp
@@ -145,7 +145,7 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
 
 	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
 
-	int rc = intersect_shape(p_shape,p_xform,Vector2(),res,p_result_max,exclude,p_user_mask);
+	int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask);
 
 	if (rc==0)
 		return Variant();
@@ -163,15 +163,13 @@ Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Ma
 Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
 
 
+#if 0
 	Set<RID> exclude;
 	for(int i=0;i<p_exclude.size();i++)
 		exclude.insert(p_exclude[i]);
 
 
-
-	MotionCastCollision mcc;
-
-	bool result = cast_motion(p_shape,p_xform,p_motion,mcc,exclude,p_user_mask);
+	bool result = cast_motion(p_shape,p_xform,p_motion,0,mcc,exclude,p_user_mask);
 
 	if (!result)
 		return Variant();
@@ -185,7 +183,8 @@ Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix
 	d["shape"]=mcc.shape;
 
 	return d;
-
+#endif
+	return Variant();
 
 }
 
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index 5242ec0e2a..172fa1699b 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -98,6 +98,16 @@ protected:
 
 public:
 
+	enum ObjectTypeMask {
+		TYPE_MASK_STATIC_BODY=1<<0,
+		TYPE_MASK_KINEMATIC_BODY=1<<1,
+		TYPE_MASK_RIGID_BODY=1<<2,
+		TYPE_MASK_CHARACTER_BODY=1<<3,
+		TYPE_MASK_AREA=1<<4,
+		TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
+
+	};
+
 	struct RayResult {
 
 		Vector2 position;
@@ -108,7 +118,7 @@ public:
 		int shape;
 	};
 
-	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
 	struct ShapeResult {
 
@@ -119,25 +129,26 @@ public:
 
 	};
 
-	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
+	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+
 
 
+	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
-	struct MotionCastCollision {
+	virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+
+	struct ShapeRestInfo {
 
-		float travel; //0 to 1, if 0 then it's blocked
 		Vector2 point;
 		Vector2 normal;
 		RID rid;
 		ObjectID collider_id;
-		Object *collider;
 		int shape;
+		Vector2 linear_velocity; //velocity at contact point
 
 	};
 
-	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion, MotionCastCollision &r_result, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=0;
-
-
+	virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
 
 	Physics2DDirectSpaceState();
@@ -327,8 +338,8 @@ public:
 	virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0;
 	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0;
 
-	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
-	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+	virtual void body_set_user_mask(RID p_body, uint32_t p_mask)=0;
+	virtual uint32_t body_get_user_mask(RID p_body, uint32_t p_mask) const=0;
 
 	// common body variables
 	enum BodyParameter {
diff --git a/tools/doc/doc_data.cpp b/tools/doc/doc_data.cpp
index 5167b9a0b0..35f1140644 100644
--- a/tools/doc/doc_data.cpp
+++ b/tools/doc/doc_data.cpp
@@ -530,6 +530,17 @@ void DocData::generate(bool p_basic_types) {
 
 			}
 
+			List<Pair<String,Variant> > cinfo;
+			lang->get_public_constants(&cinfo);
+
+
+			for(List<Pair<String,Variant> >::Element *E=cinfo.front();E;E=E->next()) {
+
+				ConstantDoc cd;
+				cd.name=E->get().first;
+				cd.value=E->get().second;
+				c.constants.push_back(cd);
+			}
 		}
 	}
 
diff --git a/tools/editor/editor_node.cpp b/tools/editor/editor_node.cpp
index ed932396db..33b2e72d62 100644
--- a/tools/editor/editor_node.cpp
+++ b/tools/editor/editor_node.cpp
@@ -4148,6 +4148,7 @@ EditorNode::EditorNode() {
 
 
 	EditorSettings::get_singleton()->enable_plugins();
+	Node::set_human_readable_collision_renaming(true);
 
 //	Ref<ImageTexture> it = gui_base->get_icon("logo","Icons");
 //	OS::get_singleton()->set_icon( it->get_data() );