1 files changed, 106 insertions, 13 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 26c4ea385f..0c5c353766 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -436,7 +436,7 @@ bool RigidBody2D::_test_motion(const Vector2& p_motion,float p_margin,const Ref<
 	Physics2DServer::MotionResult *r=NULL;
 	if (p_result.is_valid())
 		r=p_result->get_result_ptr();
-	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,p_margin,r);
+	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),get_global_transform(),p_motion,p_margin,r);
 
 }
 
@@ -1057,8 +1057,10 @@ Vector2 KinematicBody2D::get_travel() const {
 Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 
 #if 1
+
+	Matrix32 gt = get_global_transform();
 	Physics2DServer::MotionResult result;
-	colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result);
+	colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),gt,p_motion,margin,&result);
 
 	collider_metadata=result.collider_metadata;
 	collider_shape=result.collider_shape;
@@ -1067,10 +1069,12 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	normal=result.collision_normal;
 	collider=result.collider_id;
 
-	Matrix32 gt = get_global_transform();
+
 	gt.elements[2]+=result.motion;
 	set_global_transform(gt);
 	travel=result.motion;
+
+
 	return result.remainder;
 
 #else
@@ -1081,7 +1085,6 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	//this took about a week to get right..
 	//but is it right? who knows at this point..
 
-
 	colliding=false;
 	ERR_FAIL_COND_V(!is_inside_tree(),Vector2());
 	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
@@ -1099,17 +1102,15 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 
 	bool collided=false;
 	uint32_t mask=0;
-	if (collide_static)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
-	if (collide_kinematic)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
-	if (collide_rigid)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
-	if (collide_character)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
 
-//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
-
 	//print_line("margin: "+rtos(margin));
 	do {
 
@@ -1145,6 +1146,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 			break;
 		}
 
+
+
 		Matrix32 gt = get_global_transform();
 		gt.elements[2]+=recover_motion;
 		set_global_transform(gt);
@@ -1191,6 +1194,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	if (safe>=1) {
 		//not collided
 		colliding=false;
+
+
 	} else {
 
 		//it collided, let's get the rest info in unsafe advance
@@ -1226,16 +1231,94 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 #endif
 }
 
+
+
+Vector2 KinematicBody2D::move_and_slide(const Vector2& p_linear_velocity,const Vector2& p_floor_direction,float p_slope_stop_min_velocity,int p_max_bounces) {
+
+	Vector2 motion = (move_and_slide_floor_velocity+p_linear_velocity)*get_fixed_process_delta_time();
+	Vector2 lv = p_linear_velocity;
+
+	move_and_slide_on_floor=false;
+	move_and_slide_on_ceiling=false;
+	move_and_slide_on_wall=false;
+	move_and_slide_colliders.clear();
+	move_and_slide_floor_velocity=Vector2();
+
+	while(p_max_bounces) {
+
+		motion=move(motion);
+
+		if (is_colliding()) {
+
+
+			if (p_floor_direction==Vector2()) {
+				//all is a wall
+				move_and_slide_on_wall=true;
+			} else {
+				if ( get_collision_normal().dot(p_floor_direction) > Math::cos(Math::deg2rad(45))) { //floor
+
+
+					move_and_slide_on_floor=true;
+					move_and_slide_floor_velocity=get_collider_velocity();
+
+					if (get_travel().length()<1 && ABS((lv.x-move_and_slide_floor_velocity.x))<p_slope_stop_min_velocity) {
+						revert_motion();
+						return Vector2();
+					}
+				} else if ( get_collision_normal().dot(p_floor_direction) < Math::cos(Math::deg2rad(45))) { //ceiling
+					move_and_slide_on_ceiling=true;
+				} else {
+					move_and_slide_on_wall=true;
+				}
+
+			}
+
+			motion=get_collision_normal().slide(motion);
+			lv=get_collision_normal().slide(lv);
+			Variant collider = _get_collider();
+			if (collider.get_type()!=Variant::NIL) {
+				move_and_slide_colliders.push_back(collider);
+			}
+
+		} else {
+			break;
+		}
+
+		p_max_bounces--;
+		if (motion==Vector2())
+			break;
+	}
+
+	return lv;
+}
+
+bool KinematicBody2D::is_move_and_slide_on_floor() const {
+
+	return move_and_slide_on_floor;
+}
+bool KinematicBody2D::is_move_and_slide_on_wall() const{
+
+	return move_and_slide_on_wall;
+}
+bool KinematicBody2D::is_move_and_slide_on_ceiling() const{
+
+	return move_and_slide_on_ceiling;
+}
+Array KinematicBody2D::get_move_and_slide_colliders() const{
+
+	return move_and_slide_colliders;
+}
+
 Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
 
 	return move(p_position-get_global_pos());
 }
 
-bool KinematicBody2D::test_move(const Vector2& p_motion) {
+bool KinematicBody2D::test_move(const Matrix32& p_from,const Vector2& p_motion) {
 
 	ERR_FAIL_COND_V(!is_inside_tree(),false);
 
-	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin);
+	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_from,p_motion,margin);
 
 
 }
@@ -1300,8 +1383,9 @@ void KinematicBody2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody2D::move);
 	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
+	ObjectTypeDB::bind_method(_MD("move_and_slide","linear_velocity","floor_normal","slope_stop_min_velocity","max_bounces"),&KinematicBody2D::move_and_slide,DEFVAL(Vector2(0,0)),DEFVAL(5),DEFVAL(4));
 
-	ObjectTypeDB::bind_method(_MD("test_move","rel_vec"),&KinematicBody2D::test_move);
+	ObjectTypeDB::bind_method(_MD("test_move","from","rel_vec"),&KinematicBody2D::test_move);
 	ObjectTypeDB::bind_method(_MD("get_travel"),&KinematicBody2D::get_travel);
 	ObjectTypeDB::bind_method(_MD("revert_motion"),&KinematicBody2D::revert_motion);
 
@@ -1313,6 +1397,10 @@ void KinematicBody2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody2D::_get_collider);
 	ObjectTypeDB::bind_method(_MD("get_collider_shape"),&KinematicBody2D::get_collider_shape);
 	ObjectTypeDB::bind_method(_MD("get_collider_metadata:Variant"),&KinematicBody2D::get_collider_metadata);
+	ObjectTypeDB::bind_method(_MD("get_move_and_slide_colliders"),&KinematicBody2D::get_move_and_slide_colliders);
+	ObjectTypeDB::bind_method(_MD("is_move_and_slide_on_floor"),&KinematicBody2D::is_move_and_slide_on_floor);
+	ObjectTypeDB::bind_method(_MD("is_move_and_slide_on_ceiling"),&KinematicBody2D::is_move_and_slide_on_ceiling);
+	ObjectTypeDB::bind_method(_MD("is_move_and_slide_on_wall"),&KinematicBody2D::is_move_and_slide_on_wall);
 
 	ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody2D::set_collision_margin);
 	ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody2D::get_collision_margin);
@@ -1330,6 +1418,11 @@ KinematicBody2D::KinematicBody2D() : PhysicsBody2D(Physics2DServer::BODY_MODE_KI
 	collider_shape=0;
 
 	margin=0.08;
+
+	move_and_slide_on_floor=false;
+	move_and_slide_on_ceiling=false;
+	move_and_slide_on_wall=false;
+
 }
 KinematicBody2D::~KinematicBody2D()  {