Lots of work on Audio & Physics engine:

-Added new 3D stream player node
-Added ability for Area to capture sound from streams
-Added small features in physics to be able to properly guess distance to areas for sound
-Fixed 3D CollisionObject so shapes are added the same as in 2D, directly from children
-Fixed KinematicBody API to make it the same as 2D.

14 files changed, 751 insertions, 37 deletions

diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index 8ec001709d..5c418c473f 100644
--- a/servers/physics/area_pair_sw.cpp
+++ b/servers/physics/area_pair_sw.cpp
@@ -32,12 +32,13 @@
 
 bool AreaPairSW::setup(real_t p_step) {
 
-	if (!area->test_collision_mask(body)) {
-		colliding = false;
-		return false;
-	}
+	bool result = false;
 
-	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this);
+	if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
+		result = false;
+	} else if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this)) {
+		result = true;
+	}
 
 	if (result != colliding) {
 
@@ -95,14 +96,13 @@ AreaPairSW::~AreaPairSW() {
 
 bool Area2PairSW::setup(real_t p_step) {
 
-	if (!area_a->test_collision_mask(area_b)) {
-		colliding = false;
-		return false;
+	bool result = false;
+	if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
+		result = false;
+	} else if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this)) {
+		result = true;
 	}
 
-	//bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
-	bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this);
-
 	if (result != colliding) {
 
 		if (result) {
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index d740d3c384..9ada1fbc50 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -214,6 +214,11 @@ bool BodyPairSW::setup(real_t p_step) {
 		return false;
 	}
 
+	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
+		collided = false;
+		return false;
+	}
+
 	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
 
 	validate_contacts();
@@ -313,12 +318,6 @@ bool BodyPairSW::setup(real_t p_step) {
 			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
 		}
 
-		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
-			c.active = false;
-			collided = false;
-			continue;
-		}
-
 		c.active = true;
 
 		// Precompute normal mass, tangent mass, and bias.
diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp
index 77d8538574..679b9a31fc 100644
--- a/servers/physics/broad_phase_basic.cpp
+++ b/servers/physics/broad_phase_basic.cpp
@@ -105,6 +105,26 @@ int BroadPhaseBasic::get_subindex(ID p_id) const {
 	return E->get().subindex;
 }
 
+int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
+
+	int rc = 0;
+
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
+
+		const Rect3 aabb = E->get().aabb;
+		if (aabb.has_point(p_point)) {
+
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
+			rc++;
+			if (rc >= p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
 int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
 	int rc = 0;
diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h
index a285204a32..8dabf72f11 100644
--- a/servers/physics/broad_phase_basic.h
+++ b/servers/physics/broad_phase_basic.h
@@ -91,6 +91,7 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp
index d18da1b238..2439fbeae9 100644
--- a/servers/physics/broad_phase_octree.cpp
+++ b/servers/physics/broad_phase_octree.cpp
@@ -66,6 +66,11 @@ int BroadPhaseOctree::get_subindex(ID p_id) const {
 	return octree.get_subindex(p_id);
 }
 
+int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
+
+	return octree.cull_point(p_point, p_results, p_max_results, p_result_indices);
+}
+
 int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
 	return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h
index 086fb0a1a3..88d72a2bd8 100644
--- a/servers/physics/broad_phase_octree.h
+++ b/servers/physics/broad_phase_octree.h
@@ -56,6 +56,7 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h
index 8fe901c8ef..5564cf5077 100644
--- a/servers/physics/broad_phase_sw.h
+++ b/servers/physics/broad_phase_sw.h
@@ -57,6 +57,7 @@ public:
 	virtual bool is_static(ID p_id) const = 0;
 	virtual int get_subindex(ID p_id) const = 0;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index 15082a0551..a56253e33d 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -64,9 +64,9 @@ private:
 		Rect3 aabb_cache; //for rayqueries
 		real_t area_cache;
 		ShapeSW *shape;
-		bool trigger;
+		bool disabled;
 
-		Shape() { trigger = false; }
+		Shape() { disabled = false; }
 	};
 
 	Vector<Shape> shapes;
@@ -131,8 +131,8 @@ public:
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
-	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx, bool p_enable) { shapes[p_idx].trigger = p_enable; }
-	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
+	_FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes[p_idx].disabled = p_enable; }
+	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { return shapes[p_idx].disabled; }
 
 	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 733bd5b63b..101bd4b185 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -330,6 +330,14 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) {
 		area->remove_shape(0);
 }
 
+void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
+	area->set_shape_as_disabled(p_shape_idx, p_disabled);
+}
+
 void PhysicsServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) {
 
 	if (space_owner.owns(p_area)) {
@@ -551,21 +559,12 @@ RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
 	return shape->get_self();
 }
 
-void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) {
+void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
-	body->set_shape_as_trigger(p_shape_idx, p_enable);
-}
-
-bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const {
-
-	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false);
-
-	return body->is_shape_set_as_trigger(p_shape_idx);
+	body->set_shape_as_disabled(p_shape_idx, p_disabled);
 }
 
 Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
@@ -875,6 +874,16 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const {
 	return body->is_ray_pickable();
 }
 
+bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
+}
+
 /* JOINT API */
 
 RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
@@ -1530,8 +1539,9 @@ void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_
 	}
 }
 
+PhysicsServerSW *PhysicsServerSW::singleton = NULL;
 PhysicsServerSW::PhysicsServerSW() {
-
+	singleton = this;
 	BroadPhaseSW::create_func = BroadPhaseOctree::_create;
 	island_count = 0;
 	active_objects = 0;
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index a0a1bcf963..591fe4af46 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -63,6 +63,8 @@ class PhysicsServerSW : public PhysicsServer {
 
 	//void _clear_query(QuerySW *p_query);
 public:
+	static PhysicsServerSW *singleton;
+
 	struct CollCbkData {
 
 		int max;
@@ -117,6 +119,8 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx);
 	virtual void area_clear_shapes(RID p_area);
 
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled);
+
 	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID);
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
 
@@ -156,8 +160,7 @@ public:
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
 	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable);
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
 
 	virtual void body_remove_shape(RID p_body, int p_shape_idx);
 	virtual void body_clear_shapes(RID p_body);
@@ -214,6 +217,8 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool body_is_ray_pickable(RID p_body) const;
 
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+
 	/* JOINT API */
 
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index a5cea8aff7..b4004c8c94 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -117,6 +117,20 @@ bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en
 	return inters;
 }
 
+bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return plane.distance_to(p_point) < 0;
+}
+
+Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	if (plane.is_point_over(p_point)) {
+		return plane.project(p_point);
+	} else {
+		return p_point;
+	}
+}
+
 Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3(); //wtf
@@ -184,6 +198,21 @@ bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end,
 	return false; //simply not possible
 }
 
+bool RayShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //simply not possible
+}
+
+Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 s[2] = {
+		Vector3(0, 0, 0),
+		Vector3(0, 0, length)
+	};
+
+	return Geometry::get_closest_point_to_segment(p_point, s);
+}
+
 Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3();
@@ -245,6 +274,20 @@ bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e
 	return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
 }
 
+bool SphereShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return p_point.length() < radius;
+}
+
+Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 p = p_point;
+	float l = p.length();
+	if (l < radius)
+		return p_point;
+	return (p / l) * radius;
+}
+
 Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	real_t s = 0.4 * p_mass * radius * radius;
@@ -390,6 +433,62 @@ bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end,
 	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
 }
 
+bool BoxShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z);
+}
+
+Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	int outside = 0;
+	Vector3 min_point;
+
+	for (int i = 0; i < 3; i++) {
+
+		if (Math::abs(p_point[i]) > half_extents[i]) {
+			outside++;
+			if (outside == 1) {
+				//use plane if only one side matches
+				Vector3 n;
+				n[i] = SGN(p_point[i]);
+
+				Plane p(n, half_extents[i]);
+				min_point = p.project(p_point);
+			}
+		}
+	}
+
+	if (!outside)
+		return p_point; //it's inside, don't do anything else
+
+	if (outside == 1) //if only above one plane, this plane clearly wins
+		return min_point;
+
+	//check segments
+	float min_distance = 1e20;
+	Vector3 closest_vertex = half_extents * p_point.sign();
+	Vector3 s[2] = {
+		closest_vertex,
+		closest_vertex
+	};
+
+	for (int i = 0; i < 3; i++) {
+
+		s[1] = closest_vertex;
+		s[1][i] = -s[1][i]; //edge
+
+		Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s);
+
+		float d = p_point.distance_to(closest_edge);
+		if (d < min_distance) {
+			min_point = closest_edge;
+			min_distance = d;
+		}
+	}
+
+	return min_point;
+}
+
 Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	real_t lx = half_extents.x;
@@ -542,6 +641,32 @@ bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_
 	return collision;
 }
 
+bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	if (Math::abs(p_point.z) < height * 0.5) {
+		return Vector3(p_point.x, p_point.y, 0).length() < radius;
+	} else {
+		Vector3 p = p_point;
+		p.z = Math::abs(p.z) - height * 0.5;
+		return p.length() < radius;
+	}
+}
+
+Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 s[2] = {
+		Vector3(0, 0, -height * 0.5),
+		Vector3(0, 0, height * 0.5),
+	};
+
+	Vector3 p = Geometry::get_closest_point_to_segment(p_point, s);
+
+	if (p.distance_to(p_point) < radius)
+		return p_point;
+
+	return p + (p_point - p).normalized() * radius;
+}
+
 Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	// use crappy AABB approximation
@@ -738,6 +863,81 @@ bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vecto
 	return col;
 }
 
+bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+
+	for (int i = 0; i < fc; i++) {
+
+		if (faces[i].plane.distance_to(p_point) >= 0)
+			return false;
+	}
+
+	return true;
+}
+
+Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+
+	bool all_inside = true;
+	for (int i = 0; i < fc; i++) {
+
+		if (!faces[i].plane.is_point_over(p_point))
+			continue;
+
+		all_inside = false;
+		bool is_inside = true;
+		int ic = faces[i].indices.size();
+		const int *indices = faces[i].indices.ptr();
+
+		for (int j = 0; j < ic; j++) {
+
+			Vector3 a = vertices[indices[j]];
+			Vector3 b = vertices[indices[(j + 1) % ic]];
+			Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
+			if (Plane(a, n).is_point_over(p_point)) {
+				is_inside = false;
+				break;
+			}
+		}
+
+		if (is_inside) {
+			return faces[i].plane.project(p_point);
+		}
+	}
+
+	if (all_inside) {
+		return p_point;
+	}
+
+	float min_distance = 1e20;
+	Vector3 min_point;
+
+	//check edges
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int ec = mesh.edges.size();
+	for (int i = 0; i < ec; i++) {
+
+		Vector3 s[2] = {
+			vertices[edges[i].a],
+			vertices[edges[i].b]
+		};
+
+		Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s);
+		float d = closest.distance_to(p_point);
+		if (d < min_distance) {
+			min_distance = d;
+			min_point = closest;
+		}
+	}
+
+	return min_point;
+}
+
 Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	// use crappy AABB approximation
@@ -880,6 +1080,16 @@ bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end
 	return c;
 }
 
+bool FaceShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //face is flat
+}
+
+Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
+}
+
 Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
@@ -1046,6 +1256,16 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vect
 	}
 }
 
+bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //face is flat
+}
+
+Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Vector3();
+}
+
 void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
 
 	const BVH *bvh = &p_params->bvh[p_idx];
@@ -1471,6 +1691,15 @@ bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 	return false;
 }
 
+bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const {
+	return false;
+}
+
+Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Vector3();
+}
+
 void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const {
 }
 
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
index 808ff0a3a1..aa1975b655 100644
--- a/servers/physics/shape_sw.h
+++ b/servers/physics/shape_sw.h
@@ -87,8 +87,9 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0;
-
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0;
+	virtual bool intersect_point(const Vector3 &p_point) const = 0;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0;
 
 	virtual void set_data(const Variant &p_data) = 0;
@@ -134,7 +135,8 @@ public:
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
-
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
 	virtual void set_data(const Variant &p_data);
@@ -159,6 +161,8 @@ public:
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -185,6 +189,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -209,6 +215,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -237,6 +245,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -261,6 +271,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -338,6 +350,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
@@ -372,7 +386,9 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
 
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
@@ -397,6 +413,8 @@ struct FaceShapeSW : public ShapeSW {
 	Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -436,6 +454,8 @@ struct MotionShapeSW : public ShapeSW {
 	}
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; }
+	virtual bool intersect_point(const Vector3 &p_point) const { return false; }
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; }
 
 	Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); }
 
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 2bf98cecfa..2d71fd6061 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -45,6 +45,50 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object,
 	return (1 << body->get_mode()) & p_type_mask;
 }
 
+int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+
+	ERR_FAIL_COND_V(space->locked, false);
+	int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int cc = 0;
+
+	//Transform ai = p_xform.affine_inverse();
+
+	for (int i = 0; i < amount; i++) {
+
+		if (cc >= p_result_max)
+			break;
+
+		if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
+			continue;
+
+		//area can't be picked by ray (default)
+
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
+			continue;
+
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		inv_xform.affine_invert();
+
+		if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point)))
+			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);
+		else
+			r_results[cc].collider = NULL;
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+
+		cc++;
+	}
+
+	return cc;
+}
+
 bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) {
 
 	ERR_FAIL_COND_V(space->locked, false);
@@ -428,6 +472,48 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
 	return true;
 }
 
+Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
+
+	CollisionObjectSW *obj = NULL;
+	obj = PhysicsServerSW::singleton->area_owner.getornull(p_object);
+	if (!obj) {
+		obj = PhysicsServerSW::singleton->body_owner.getornull(p_object);
+	}
+	ERR_FAIL_COND_V(!obj, Vector3());
+
+	ERR_FAIL_COND_V(obj->get_space() != space, Vector3());
+
+	float min_distance = 1e20;
+	Vector3 min_point;
+
+	bool shapes_found = false;
+
+	for (int i = 0; i < obj->get_shape_count(); i++) {
+
+		if (obj->is_shape_set_as_disabled(i))
+			continue;
+
+		Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i);
+		ShapeSW *shape = obj->get_shape(i);
+
+		Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point));
+		point = shape_xform.xform(point);
+
+		float dist = point.distance_to(p_point);
+		if (dist < min_distance) {
+			min_distance = dist;
+			min_point = point;
+		}
+		shapes_found = true;
+	}
+
+	if (!shapes_found) {
+		return obj->get_transform().origin; //no shapes found, use distance to origin.
+	} else {
+		return min_point;
+	}
+}
+
 PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
 	space = NULL;
@@ -435,6 +521,337 @@ PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) {
+
+	int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
+
+	for (int i = 0; i < amount; i++) {
+
+		bool keep = true;
+
+		if (intersection_query_results[i] == p_body)
+			keep = false;
+		else if (intersection_query_results[i]->get_type() == CollisionObjectSW::TYPE_AREA)
+			keep = false;
+		else if ((static_cast<BodySW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0)
+			keep = false;
+		else if (static_cast<BodySW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
+			keep = false;
+		else if (static_cast<BodySW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i]))
+			keep = false;
+
+		if (!keep) {
+
+			if (i < amount - 1) {
+				SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
+				SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
+			}
+
+			amount--;
+			i--;
+		}
+	}
+
+	return amount;
+}
+
+bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) {
+
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	if (r_result) {
+		r_result->collider_id = 0;
+		r_result->collider_shape = 0;
+	}
+	Rect3 body_aabb;
+
+	for (int i = 0; i < p_body->get_shape_count(); i++) {
+
+		if (i == 0)
+			body_aabb = p_body->get_shape_aabb(i);
+		else
+			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
+	}
+
+	// Undo the currently transform the physics server is aware of and apply the provided one
+	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
+	body_aabb = body_aabb.grow(p_margin);
+
+	Transform body_transform = p_from;
+
+	{
+		//STEP 1, FREE BODY IF STUCK
+
+		const int max_results = 32;
+		int recover_attempts = 4;
+		Vector3 sr[max_results * 2];
+
+		do {
+
+			PhysicsServerSW::CollCbkData cbk;
+			cbk.max = max_results;
+			cbk.amount = 0;
+			cbk.ptr = sr;
+
+			CollisionSolverSW::CallbackResult cbkres = NULL;
+
+			PhysicsServerSW::CollCbkData *cbkptr = NULL;
+			cbkptr = &cbk;
+			cbkres = PhysicsServerSW::_shape_col_cbk;
+
+			bool collided = false;
+
+			int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+			for (int j = 0; j < p_body->get_shape_count(); j++) {
+				if (p_body->is_shape_set_as_disabled(j))
+					continue;
+
+				Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
+				ShapeSW *body_shape = p_body->get_shape(j);
+				for (int i = 0; i < amount; i++) {
+
+					const CollisionObjectSW *col_obj = intersection_query_results[i];
+					int shape_idx = intersection_query_subindex_results[i];
+
+					if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
+						collided = cbk.amount > 0;
+					}
+				}
+			}
+
+			if (!collided) {
+				break;
+			}
+
+			Vector3 recover_motion;
+
+			for (int i = 0; i < cbk.amount; i++) {
+
+				Vector3 a = sr[i * 2 + 0];
+				Vector3 b = sr[i * 2 + 1];
+
+#if 0
+				Vector3 rel = b-a;
+				real_t d = rel.length();
+				if (d==0)
+					continue;
+
+				Vector3 n = rel/d;
+				real_t traveled = n.dot(recover_motion);
+				a+=n*traveled;
+
+				real_t d = a.distance_to(b);
+				if (d<margin)
+					continue;
+#endif
+				recover_motion += (b - a) * 0.4;
+			}
+
+			if (recover_motion == Vector3()) {
+				collided = false;
+				break;
+			}
+
+			body_transform.origin += recover_motion;
+			body_aabb.position += recover_motion;
+
+			recover_attempts--;
+
+		} while (recover_attempts);
+	}
+
+	real_t safe = 1.0;
+	real_t unsafe = 1.0;
+	int best_shape = -1;
+
+	{
+		// STEP 2 ATTEMPT MOTION
+
+		Rect3 motion_aabb = body_aabb;
+		motion_aabb.position += p_motion;
+		motion_aabb = motion_aabb.merge(body_aabb);
+
+		int amount = _cull_aabb_for_body(p_body, motion_aabb);
+
+		for (int j = 0; j < p_body->get_shape_count(); j++) {
+
+			if (p_body->is_shape_set_as_disabled(j))
+				continue;
+
+			Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
+			ShapeSW *body_shape = p_body->get_shape(j);
+
+			Transform body_shape_xform_inv = body_shape_xform.affine_inverse();
+			MotionShapeSW mshape;
+			mshape.shape = body_shape;
+			mshape.motion = body_shape_xform_inv.basis.xform(p_motion);
+
+			bool stuck = false;
+
+			real_t best_safe = 1;
+			real_t best_unsafe = 1;
+
+			for (int i = 0; i < amount; i++) {
+
+				const CollisionObjectSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
+
+				//test initial overlap, does it collide if going all the way?
+				Vector3 point_A, point_B;
+				Vector3 sep_axis = p_motion.normalized();
+
+				Transform 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 (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+					//print_line("failed motion cast (no collision)");
+					continue;
+				}
+				sep_axis = p_motion.normalized();
+
+				if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+					//print_line("failed motion cast (no collision)");
+					stuck = true;
+					break;
+				}
+
+				//just do kinematic solving
+				real_t low = 0;
+				real_t hi = 1;
+				Vector3 mnormal = p_motion.normalized();
+
+				for (int i = 0; i < 8; i++) { //steps should be customizable..
+
+					real_t ofs = (low + hi) * 0.5;
+
+					Vector3 sep = mnormal; //important optimization for this to work fast enough
+
+					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
+
+					Vector3 lA, lB;
+
+					bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
+
+					if (collided) {
+
+						//print_line(itos(i)+": "+rtos(ofs));
+						hi = ofs;
+					} else {
+
+						point_A = lA;
+						point_B = lB;
+						low = ofs;
+					}
+				}
+
+				if (low < best_safe) {
+					best_safe = low;
+					best_unsafe = hi;
+				}
+			}
+
+			if (stuck) {
+
+				safe = 0;
+				unsafe = 0;
+				best_shape = j; //sadly it's the best
+				break;
+			}
+			if (best_safe == 1.0) {
+				continue;
+			}
+			if (best_safe < safe) {
+
+				safe = best_safe;
+				unsafe = best_unsafe;
+				best_shape = j;
+			}
+		}
+	}
+
+	bool collided = false;
+	if (safe >= 1) {
+		//not collided
+		collided = false;
+		if (r_result) {
+
+			r_result->motion = p_motion;
+			r_result->remainder = Vector3();
+			r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+		}
+
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Transform ugt = body_transform;
+		ugt.origin += p_motion * unsafe;
+
+		_RestCallbackData rcd;
+		rcd.best_len = 0;
+		rcd.best_object = NULL;
+		rcd.best_shape = 0;
+
+		Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
+		ShapeSW *body_shape = p_body->get_shape(best_shape);
+
+		body_aabb.position += p_motion * unsafe;
+
+		int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+		for (int i = 0; i < amount; i++) {
+
+			const CollisionObjectSW *col_obj = intersection_query_results[i];
+			int shape_idx = intersection_query_subindex_results[i];
+
+			rcd.object = col_obj;
+			rcd.shape = shape_idx;
+			bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin);
+			if (!sc)
+				continue;
+		}
+
+		if (rcd.best_len != 0) {
+
+			if (r_result) {
+				r_result->collider = rcd.best_object->get_self();
+				r_result->collider_id = rcd.best_object->get_instance_id();
+				r_result->collider_shape = rcd.best_shape;
+				r_result->collision_local_shape = best_shape;
+				r_result->collision_normal = rcd.best_normal;
+				r_result->collision_point = rcd.best_contact;
+				//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
+
+				const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
+				//Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin();
+				//				r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+				r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
+
+				r_result->motion = safe * p_motion;
+				r_result->remainder = p_motion - safe * p_motion;
+				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+			}
+
+			collided = true;
+		} else {
+			if (r_result) {
+
+				r_result->motion = p_motion;
+				r_result->remainder = Vector3();
+				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+			}
+
+			collided = false;
+		}
+	}
+
+	return collided;
+}
+
 void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
 
 	CollisionObjectSW::Type type_A = A->get_type();
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h
index b0e54f647c..6ef12dbeae 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -47,11 +47,13 @@ class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
 public:
 	SpaceSW *space;
 
+	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false);
 	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL);
 	virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const;
 
 	PhysicsDirectSpaceStateSW();
 };
@@ -120,6 +122,8 @@ private:
 
 	friend class PhysicsDirectSpaceStateSW;
 
+	int _cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb);
+
 public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
@@ -192,6 +196,8 @@ public:
 	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
 	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
 
+	bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result);
+
 	SpaceSW();
 	~SpaceSW();
 };