From f33d9dab5bf05356f9a2c882537e83390a710e0b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Sun, 22 Nov 2015 14:14:07 +0100
Subject: [PATCH] Fix can_move_to and rename it for more clarity

Fixes #2416.
The KinematicBody::can_move_to function was likely designed for two behaviours:
 - discrete: check if the body can "teleport" to the destination
 - continuous: check if the direct path to the destination is valid
The continuous behaviour was however not implemented, and the discrete behaviour was broken too due to a wrong call to intersect_shape.

The discrete behaviour has thus been fixed and the function renamed to can_teleport_to for more clarity.
---
 scene/3d/physics_body.cpp    | 15 ++++-----------
 scene/3d/physics_body.h      |  2 +-
 servers/physics/space_sw.cpp |  6 ++++--
 3 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index d61859a3d06..de50484a1e2 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1073,7 +1073,7 @@ Vector3 KinematicBody::move_to(const Vector3& p_position) {
 	return move(p_position-get_global_transform().origin);
 }
 
-bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
+bool KinematicBody::can_teleport_to(const Vector3& p_position) {
 
 	ERR_FAIL_COND_V(!is_inside_tree(),false);
 	PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
@@ -1089,25 +1089,18 @@ bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
 	if (collide_character)
 		mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
 
-	Vector3 motion = p_position-get_global_transform().origin;
 	Transform xform=get_global_transform();
-
-	if (true || p_discrete) {
-
-		xform.origin+=motion;
-		motion=Vector3();
-	}
+	xform.origin=p_position;
 
 	Set<RID> exclude;
 	exclude.insert(get_rid());
 
-	//fill exclude list..
 	for(int i=0;i<get_shape_count();i++) {
 
 		if (is_shape_set_as_trigger(i))
 			continue;
 
-		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask);
+		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,1,exclude,get_layer_mask(),mask);
 		if (col)
 			return false;
 	}
@@ -1205,7 +1198,7 @@ void KinematicBody::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody::move);
 	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody::move_to);
 
-	ObjectTypeDB::bind_method(_MD("can_move_to","position"),&KinematicBody::can_move_to);
+	ObjectTypeDB::bind_method(_MD("can_teleport_to","position"),&KinematicBody::can_teleport_to);
 
 	ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody::is_colliding);
 
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 66490ba9258..0e63b771187 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -304,7 +304,7 @@ public:
 	Vector3 move(const Vector3& p_motion);
 	Vector3 move_to(const Vector3& p_position);
 
-	bool can_move_to(const Vector3& p_position,bool p_discrete=false);
+	bool can_teleport_to(const Vector3& p_position);
 	bool is_colliding() const;
 	Vector3 get_collision_pos() const;
 	Vector3 get_collision_normal() const;
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index ba1c737530f..4b59f2d0ba3 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -175,6 +175,10 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
 		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL,p_margin,0))
 			continue;
 
+		cc++;
+
+		if (!r_results)
+			continue;
 		r_results[cc].collider_id=col_obj->get_instance_id();
 		if (r_results[cc].collider_id!=0)
 			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
@@ -183,8 +187,6 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
 		r_results[cc].rid=col_obj->get_self();
 		r_results[cc].shape=shape_idx;
 
-		cc++;
-
 	}
 
 	return cc;