1 files changed, 50 insertions, 45 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index f96c7b512f..f345d8c3c9 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -58,7 +58,11 @@ Ref<KinematicCollision2D> PhysicsBody2D::_move(const Vector2 &p_distance, bool p
 	PhysicsServer2D::MotionParameters parameters(get_global_transform(), p_distance, p_margin);
 
 	PhysicsServer2D::MotionResult result;
-	if (move_and_collide(parameters, result, p_test_only)) {
+
+	bool collided = move_and_collide(parameters, result, p_test_only);
+
+	// Don't report collision when the whole motion is done.
+	if (collided && result.collision_safe_fraction < 1) {
 		// Create a new instance when the cached reference is invalid or still in use in script.
 		if (motion_cache.is_null() || motion_cache->reference_get_count() > 1) {
 			motion_cache.instantiate();
@@ -119,7 +123,7 @@ bool PhysicsBody2D::move_and_collide(const PhysicsServer2D::MotionParameters &p_
 
 	if (!p_test_only) {
 		Transform2D gt = p_parameters.from;
-		gt.elements[2] += r_result.travel;
+		gt.columns[2] += r_result.travel;
 		set_global_transform(gt);
 	}
 
@@ -277,7 +281,7 @@ void AnimatableBody2D::_update_kinematic_motion() {
 }
 
 void AnimatableBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
-	AnimatableBody2D *body = (AnimatableBody2D *)p_instance;
+	AnimatableBody2D *body = static_cast<AnimatableBody2D *>(p_instance);
 	body->_body_state_changed(p_state);
 }
 
@@ -335,17 +339,17 @@ void RigidDynamicBody2D::_body_enter_tree(ObjectID p_id) {
 	ERR_FAIL_COND(!node);
 
 	ERR_FAIL_COND(!contact_monitor);
-	Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(p_id);
+	HashMap<ObjectID, BodyState>::Iterator E = contact_monitor->body_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	ERR_FAIL_COND(E->get().in_scene);
+	ERR_FAIL_COND(E->value.in_scene);
 
 	contact_monitor->locked = true;
 
-	E->get().in_scene = true;
+	E->value.in_scene = true;
 	emit_signal(SceneStringNames::get_singleton()->body_entered, node);
 
-	for (int i = 0; i < E->get().shapes.size(); i++) {
-		emit_signal(SceneStringNames::get_singleton()->body_shape_entered, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].local_shape);
+	for (int i = 0; i < E->value.shapes.size(); i++) {
+		emit_signal(SceneStringNames::get_singleton()->body_shape_entered, E->value.rid, node, E->value.shapes[i].body_shape, E->value.shapes[i].local_shape);
 	}
 
 	contact_monitor->locked = false;
@@ -356,17 +360,17 @@ void RigidDynamicBody2D::_body_exit_tree(ObjectID p_id) {
 	Node *node = Object::cast_to<Node>(obj);
 	ERR_FAIL_COND(!node);
 	ERR_FAIL_COND(!contact_monitor);
-	Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(p_id);
+	HashMap<ObjectID, BodyState>::Iterator E = contact_monitor->body_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	ERR_FAIL_COND(!E->get().in_scene);
-	E->get().in_scene = false;
+	ERR_FAIL_COND(!E->value.in_scene);
+	E->value.in_scene = false;
 
 	contact_monitor->locked = true;
 
 	emit_signal(SceneStringNames::get_singleton()->body_exited, node);
 
-	for (int i = 0; i < E->get().shapes.size(); i++) {
-		emit_signal(SceneStringNames::get_singleton()->body_shape_exited, E->get().rid, node, E->get().shapes[i].body_shape, E->get().shapes[i].local_shape);
+	for (int i = 0; i < E->value.shapes.size(); i++) {
+		emit_signal(SceneStringNames::get_singleton()->body_shape_exited, E->value.rid, node, E->value.shapes[i].body_shape, E->value.shapes[i].local_shape);
 	}
 
 	contact_monitor->locked = false;
@@ -380,45 +384,45 @@ void RigidDynamicBody2D::_body_inout(int p_status, const RID &p_body, ObjectID p
 	Node *node = Object::cast_to<Node>(obj);
 
 	ERR_FAIL_COND(!contact_monitor);
-	Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(objid);
+	HashMap<ObjectID, BodyState>::Iterator E = contact_monitor->body_map.find(objid);
 
 	ERR_FAIL_COND(!body_in && !E);
 
 	if (body_in) {
 		if (!E) {
 			E = contact_monitor->body_map.insert(objid, BodyState());
-			E->get().rid = p_body;
-			//E->get().rc=0;
-			E->get().in_scene = node && node->is_inside_tree();
+			E->value.rid = p_body;
+			//E->value.rc=0;
+			E->value.in_scene = node && node->is_inside_tree();
 			if (node) {
 				node->connect(SceneStringNames::get_singleton()->tree_entered, callable_mp(this, &RigidDynamicBody2D::_body_enter_tree), make_binds(objid));
 				node->connect(SceneStringNames::get_singleton()->tree_exiting, callable_mp(this, &RigidDynamicBody2D::_body_exit_tree), make_binds(objid));
-				if (E->get().in_scene) {
+				if (E->value.in_scene) {
 					emit_signal(SceneStringNames::get_singleton()->body_entered, node);
 				}
 			}
 
-			//E->get().rc++;
+			//E->value.rc++;
 		}
 
 		if (node) {
-			E->get().shapes.insert(ShapePair(p_body_shape, p_local_shape));
+			E->value.shapes.insert(ShapePair(p_body_shape, p_local_shape));
 		}
 
-		if (E->get().in_scene) {
+		if (E->value.in_scene) {
 			emit_signal(SceneStringNames::get_singleton()->body_shape_entered, p_body, node, p_body_shape, p_local_shape);
 		}
 
 	} else {
-		//E->get().rc--;
+		//E->value.rc--;
 
 		if (node) {
-			E->get().shapes.erase(ShapePair(p_body_shape, p_local_shape));
+			E->value.shapes.erase(ShapePair(p_body_shape, p_local_shape));
 		}
 
-		bool in_scene = E->get().in_scene;
+		bool in_scene = E->value.in_scene;
 
-		if (E->get().shapes.is_empty()) {
+		if (E->value.shapes.is_empty()) {
 			if (node) {
 				node->disconnect(SceneStringNames::get_singleton()->tree_entered, callable_mp(this, &RigidDynamicBody2D::_body_enter_tree));
 				node->disconnect(SceneStringNames::get_singleton()->tree_exiting, callable_mp(this, &RigidDynamicBody2D::_body_exit_tree));
@@ -427,7 +431,7 @@ void RigidDynamicBody2D::_body_inout(int p_status, const RID &p_body, ObjectID p
 				}
 			}
 
-			contact_monitor->body_map.erase(E);
+			contact_monitor->body_map.remove(E);
 		}
 		if (node && in_scene) {
 			emit_signal(SceneStringNames::get_singleton()->body_shape_exited, p_body, node, p_body_shape, p_local_shape);
@@ -443,7 +447,7 @@ struct _RigidDynamicBody2DInOut {
 };
 
 void RigidDynamicBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
-	RigidDynamicBody2D *body = (RigidDynamicBody2D *)p_instance;
+	RigidDynamicBody2D *body = static_cast<RigidDynamicBody2D *>(p_instance);
 	body->_body_state_changed(p_state);
 }
 
@@ -490,7 +494,7 @@ void RigidDynamicBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state)
 			int local_shape = p_state->get_contact_local_shape(i);
 			int shape = p_state->get_contact_collider_shape(i);
 
-			Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(obj);
+			HashMap<ObjectID, BodyState>::Iterator E = contact_monitor->body_map.find(obj);
 			if (!E) {
 				toadd[toadd_count].rid = rid;
 				toadd[toadd_count].local_shape = local_shape;
@@ -501,7 +505,7 @@ void RigidDynamicBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state)
 			}
 
 			ShapePair sp(shape, local_shape);
-			int idx = E->get().shapes.find(sp);
+			int idx = E->value.shapes.find(sp);
 			if (idx == -1) {
 				toadd[toadd_count].rid = rid;
 				toadd[toadd_count].local_shape = local_shape;
@@ -511,7 +515,7 @@ void RigidDynamicBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state)
 				continue;
 			}
 
-			E->get().shapes[idx].tagged = true;
+			E->value.shapes[idx].tagged = true;
 		}
 
 		//put the ones to remove
@@ -924,7 +928,7 @@ TypedArray<String> RigidDynamicBody2D::get_configuration_warnings() const {
 
 	TypedArray<String> warnings = CollisionObject2D::get_configuration_warnings();
 
-	if (ABS(t.elements[0].length() - 1.0) > 0.05 || ABS(t.elements[1].length() - 1.0) > 0.05) {
+	if (ABS(t.columns[0].length() - 1.0) > 0.05 || ABS(t.columns[1].length() - 1.0) > 0.05) {
 		warnings.push_back(RTR("Size changes to RigidDynamicBody2D will be overridden by the physics engine when running.\nChange the size in children collision shapes instead."));
 	}
 
@@ -1106,7 +1110,7 @@ bool CharacterBody2D::move_and_slide() {
 
 	Vector2 current_platform_velocity = platform_velocity;
 	Transform2D gt = get_global_transform();
-	previous_position = gt.elements[2];
+	previous_position = gt.columns[2];
 
 	if ((on_floor || on_wall) && platform_rid.is_valid()) {
 		bool excluded = false;
@@ -1119,7 +1123,7 @@ bool CharacterBody2D::move_and_slide() {
 			//this approach makes sure there is less delay between the actual body velocity and the one we saved
 			PhysicsDirectBodyState2D *bs = PhysicsServer2D::get_singleton()->body_get_direct_state(platform_rid);
 			if (bs) {
-				Vector2 local_position = gt.elements[2] - bs->get_transform().elements[2];
+				Vector2 local_position = gt.columns[2] - bs->get_transform().columns[2];
 				current_platform_velocity = bs->get_velocity_at_local_position(local_position);
 			} else {
 				// Body is removed or destroyed, invalidate floor.
@@ -1200,7 +1204,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
 		PhysicsServer2D::MotionParameters parameters(get_global_transform(), motion, margin);
 
-		Vector2 prev_position = parameters.from.elements[2];
+		Vector2 prev_position = parameters.from.columns[2];
 
 		PhysicsServer2D::MotionResult result;
 		bool collided = move_and_collide(parameters, result, false, !sliding_enabled);
@@ -1227,7 +1231,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 			if (on_floor && floor_stop_on_slope && (velocity.normalized() + up_direction).length() < 0.01) {
 				Transform2D gt = get_global_transform();
 				if (result.travel.length() <= margin + CMP_EPSILON) {
-					gt.elements[2] -= result.travel;
+					gt.columns[2] -= result.travel;
 				}
 				set_global_transform(gt);
 				velocity = Vector2();
@@ -1249,11 +1253,11 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 					if (result.travel.length() <= margin + CMP_EPSILON) {
 						// Cancels the motion.
 						Transform2D gt = get_global_transform();
-						gt.elements[2] -= result.travel;
+						gt.columns[2] -= result.travel;
 						set_global_transform(gt);
 					}
 					// Determines if you are on the ground.
-					_snap_on_floor(true, false);
+					_snap_on_floor(true, false, true);
 					velocity = Vector2();
 					last_motion = Vector2();
 					motion = Vector2();
@@ -1308,7 +1312,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 			can_apply_constant_speed = false;
 			sliding_enabled = true;
 			Transform2D gt = get_global_transform();
-			gt.elements[2] = prev_position;
+			gt.columns[2] = prev_position;
 			set_global_transform(gt);
 
 			Vector2 motion_slide_norm = motion.slide(prev_floor_normal).normalized();
@@ -1392,8 +1396,8 @@ void CharacterBody2D::_move_and_slide_floating(double p_delta) {
 	}
 }
 
-void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up) {
-	if (on_floor || !was_on_floor || vel_dir_facing_up) {
+void CharacterBody2D::_snap_on_floor(bool p_was_on_floor, bool p_vel_dir_facing_up, bool p_wall_as_floor) {
+	if (on_floor || !p_was_on_floor || p_vel_dir_facing_up) {
 		return;
 	}
 
@@ -1405,7 +1409,8 @@ void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 
 	PhysicsServer2D::MotionResult result;
 	if (move_and_collide(parameters, result, true, false)) {
-		if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
+		if ((result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) ||
+				(p_wall_as_floor && result.get_angle(-up_direction) > floor_max_angle + FLOOR_ANGLE_THRESHOLD)) {
 			on_floor = true;
 			floor_normal = result.collision_normal;
 			_set_platform_data(result);
@@ -1420,14 +1425,14 @@ void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 				}
 			}
 
-			parameters.from.elements[2] += result.travel;
+			parameters.from.columns[2] += result.travel;
 			set_global_transform(parameters.from);
 		}
 	}
 }
 
-bool CharacterBody2D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) {
-	if (up_direction == Vector2() || on_floor || !was_on_floor || vel_dir_facing_up) {
+bool CharacterBody2D::_on_floor_if_snapped(bool p_was_on_floor, bool p_vel_dir_facing_up) {
+	if (up_direction == Vector2() || on_floor || !p_was_on_floor || p_vel_dir_facing_up) {
 		return false;
 	}
 
@@ -1516,7 +1521,7 @@ const Vector2 &CharacterBody2D::get_last_motion() const {
 }
 
 Vector2 CharacterBody2D::get_position_delta() const {
-	return get_global_transform().elements[2] - previous_position;
+	return get_global_transform().columns[2] - previous_position;
 }
 
 const Vector2 &CharacterBody2D::get_real_velocity() const {