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.

22 files changed, 1037 insertions, 212 deletions

diff --git a/servers/audio/audio_filter_sw.cpp b/servers/audio/audio_filter_sw.cpp
index 1210312ac5..4bf1cebf12 100644
--- a/servers/audio/audio_filter_sw.cpp
+++ b/servers/audio/audio_filter_sw.cpp
@@ -242,28 +242,49 @@ AudioFilterSW::Processor::Processor() {
 	set_filter(NULL);
 }
 
-void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter) {
+void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter, bool p_clear_history) {
 
-	ha1 = ha2 = hb1 = hb2 = 0;
+	if (p_clear_history) {
+		ha1 = ha2 = hb1 = hb2 = 0;
+	}
 	filter = p_filter;
 }
 
-void AudioFilterSW::Processor::update_coeffs() {
+void AudioFilterSW::Processor::update_coeffs(int p_interp_buffer_len) {
 
 	if (!filter)
 		return;
 
-	filter->prepare_coefficients(&coeffs);
+	if (p_interp_buffer_len) { //interpolate
+		Coeffs old_coeffs = coeffs;
+		filter->prepare_coefficients(&coeffs);
+		incr_coeffs.a1 = (coeffs.a1 - old_coeffs.a1) / p_interp_buffer_len;
+		incr_coeffs.a2 = (coeffs.a2 - old_coeffs.a2) / p_interp_buffer_len;
+		incr_coeffs.b0 = (coeffs.b0 - old_coeffs.b0) / p_interp_buffer_len;
+		incr_coeffs.b1 = (coeffs.b1 - old_coeffs.b1) / p_interp_buffer_len;
+		incr_coeffs.b2 = (coeffs.b2 - old_coeffs.b2) / p_interp_buffer_len;
+		coeffs = old_coeffs;
+	} else {
+		filter->prepare_coefficients(&coeffs);
+	}
 }
 
-void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride) {
+void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride, bool p_interpolate) {
 
 	if (!filter)
 		return;
 
-	for (int i = 0; i < p_amount; i++) {
+	if (p_interpolate) {
+		for (int i = 0; i < p_amount; i++) {
+
+			process_one_interp(*p_samples);
+			p_samples += p_stride;
+		}
+	} else {
+		for (int i = 0; i < p_amount; i++) {
 
-		process_one(*p_samples);
-		p_samples += p_stride;
+			process_one(*p_samples);
+			p_samples += p_stride;
+		}
 	}
 }
diff --git a/servers/audio/audio_filter_sw.h b/servers/audio/audio_filter_sw.h
index e1dd5e5c0e..f5a07c4c8f 100644
--- a/servers/audio/audio_filter_sw.h
+++ b/servers/audio/audio_filter_sw.h
@@ -60,11 +60,14 @@ public:
 		AudioFilterSW *filter;
 		Coeffs coeffs;
 		float ha1, ha2, hb1, hb2; //history
+		Coeffs incr_coeffs;
+
 	public:
-		void set_filter(AudioFilterSW *p_filter);
-		void process(float *p_samples, int p_amount, int p_stride = 1);
-		void update_coeffs();
+		void set_filter(AudioFilterSW *p_filter, bool p_clear_history = true);
+		void process(float *p_samples, int p_amount, int p_stride = 1, bool p_interpolate = false);
+		void update_coeffs(int p_interp_buffer_len = 0);
 		_ALWAYS_INLINE_ void process_one(float &p_sample);
+		_ALWAYS_INLINE_ void process_one_interp(float &p_sample);
 
 		Processor();
 	};
@@ -104,4 +107,20 @@ void AudioFilterSW::Processor::process_one(float &p_val) {
 	ha1 = p_val;
 }
 
+void AudioFilterSW::Processor::process_one_interp(float &p_val) {
+
+	float pre = p_val;
+	p_val = (p_val * coeffs.b0 + hb1 * coeffs.b1 + hb2 * coeffs.b2 + ha1 * coeffs.a1 + ha2 * coeffs.a2);
+	ha2 = ha1;
+	hb2 = hb1;
+	hb1 = pre;
+	ha1 = p_val;
+
+	coeffs.b0 += incr_coeffs.b0;
+	coeffs.b1 += incr_coeffs.b1;
+	coeffs.b2 += incr_coeffs.b2;
+	coeffs.a1 += incr_coeffs.a1;
+	coeffs.a2 += incr_coeffs.a2;
+}
+
 #endif // AUDIO_FILTER_SW_H
diff --git a/servers/audio/audio_stream.cpp b/servers/audio/audio_stream.cpp
index 14a091e27c..aa498cccad 100644
--- a/servers/audio/audio_stream.cpp
+++ b/servers/audio/audio_stream.cpp
@@ -81,3 +81,134 @@ void AudioStreamPlaybackResampled::mix(AudioFrame *p_buffer, float p_rate_scale,
 		}
 	}
 }
+////////////////////////////////
+
+void AudioStreamRandomPitch::set_audio_stream(const Ref<AudioStream> &p_audio_stream) {
+
+	audio_stream = p_audio_stream;
+	if (audio_stream.is_valid()) {
+		for (Set<AudioStreamPlaybackRandomPitch *>::Element *E = playbacks.front(); E; E = E->next()) {
+			E->get()->playback = audio_stream->instance_playback();
+		}
+	}
+}
+
+Ref<AudioStream> AudioStreamRandomPitch::get_audio_stream() const {
+
+	return audio_stream;
+}
+
+void AudioStreamRandomPitch::set_random_pitch(float p_pitch) {
+
+	if (p_pitch < 1)
+		p_pitch = 1;
+	random_pitch = p_pitch;
+}
+
+float AudioStreamRandomPitch::get_random_pitch() const {
+	return random_pitch;
+}
+
+Ref<AudioStreamPlayback> AudioStreamRandomPitch::instance_playback() {
+	Ref<AudioStreamPlaybackRandomPitch> playback;
+	playback.instance();
+	if (audio_stream.is_valid())
+		playback->playback = audio_stream->instance_playback();
+
+	playbacks.insert(playback.ptr());
+	playback->random_pitch = Ref<AudioStreamRandomPitch>((AudioStreamRandomPitch *)this);
+	return playback;
+}
+
+String AudioStreamRandomPitch::get_stream_name() const {
+
+	if (audio_stream.is_valid()) {
+		return "Random: " + audio_stream->get_name();
+	}
+	return "RandomPitch";
+}
+
+void AudioStreamRandomPitch::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_audio_stream", "stream"), &AudioStreamRandomPitch::set_audio_stream);
+	ClassDB::bind_method(D_METHOD("get_audio_stream"), &AudioStreamRandomPitch::get_audio_stream);
+
+	ClassDB::bind_method(D_METHOD("set_random_pitch", "scale"), &AudioStreamRandomPitch::set_random_pitch);
+	ClassDB::bind_method(D_METHOD("get_random_pitch"), &AudioStreamRandomPitch::get_random_pitch);
+
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "audio_stream", PROPERTY_HINT_RESOURCE_TYPE, "AudioStream"), "set_audio_stream", "get_audio_stream");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "random_pitch", PROPERTY_HINT_RANGE, "1,16,0.01"), "set_random_pitch", "get_random_pitch");
+}
+
+AudioStreamRandomPitch::AudioStreamRandomPitch() {
+	random_pitch = 1.1;
+}
+
+void AudioStreamPlaybackRandomPitch::start(float p_from_pos) {
+	playing = playback;
+	float range_from = 1.0 / random_pitch->random_pitch;
+	float range_to = random_pitch->random_pitch;
+
+	pitch_scale = range_from + Math::randf() * (range_to - range_from);
+
+	if (playing.is_valid()) {
+		playing->start(p_from_pos);
+	}
+}
+
+void AudioStreamPlaybackRandomPitch::stop() {
+	if (playing.is_valid()) {
+		playing->stop();
+		;
+	}
+}
+bool AudioStreamPlaybackRandomPitch::is_playing() const {
+	if (playing.is_valid()) {
+		return playing->is_playing();
+	}
+
+	return false;
+}
+
+int AudioStreamPlaybackRandomPitch::get_loop_count() const {
+	if (playing.is_valid()) {
+		return playing->get_loop_count();
+	}
+
+	return 0;
+}
+
+float AudioStreamPlaybackRandomPitch::get_pos() const {
+	if (playing.is_valid()) {
+		return playing->get_pos();
+	}
+
+	return 0;
+}
+void AudioStreamPlaybackRandomPitch::seek_pos(float p_time) {
+	if (playing.is_valid()) {
+		playing->seek_pos(p_time);
+	}
+}
+
+void AudioStreamPlaybackRandomPitch::mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames) {
+	if (playing.is_valid()) {
+		playing->mix(p_bufer, p_rate_scale * pitch_scale, p_frames);
+	} else {
+		for (int i = 0; i < p_frames; i++) {
+			p_bufer[i] = AudioFrame(0, 0);
+		}
+	}
+}
+
+float AudioStreamPlaybackRandomPitch::get_length() const {
+	if (playing.is_valid()) {
+		return playing->get_length();
+	}
+
+	return 0;
+}
+
+AudioStreamPlaybackRandomPitch::~AudioStreamPlaybackRandomPitch() {
+	random_pitch->playbacks.erase(this);
+}
diff --git a/servers/audio/audio_stream.h b/servers/audio/audio_stream.h
index 1cf3cd294d..a35826be21 100644
--- a/servers/audio/audio_stream.h
+++ b/servers/audio/audio_stream.h
@@ -31,6 +31,7 @@
 #define AUDIO_STREAM_H
 
 #include "resource.h"
+#include "servers/audio/audio_filter_sw.h"
 #include "servers/audio_server.h"
 
 class AudioStreamPlayback : public Reference {
@@ -88,4 +89,58 @@ public:
 	virtual String get_stream_name() const = 0;
 };
 
+class AudioStreamPlaybackRandomPitch;
+
+class AudioStreamRandomPitch : public AudioStream {
+
+	GDCLASS(AudioStreamRandomPitch, AudioStream)
+	friend class AudioStreamPlaybackRandomPitch;
+
+	Set<AudioStreamPlaybackRandomPitch *> playbacks;
+	Ref<AudioStream> audio_stream;
+	float random_pitch;
+
+protected:
+	static void _bind_methods();
+
+public:
+	void set_audio_stream(const Ref<AudioStream> &audio_stream);
+	Ref<AudioStream> get_audio_stream() const;
+
+	void set_random_pitch(float p_pitch);
+	float get_random_pitch() const;
+
+	virtual Ref<AudioStreamPlayback> instance_playback();
+	virtual String get_stream_name() const;
+
+	AudioStreamRandomPitch();
+};
+
+class AudioStreamPlaybackRandomPitch : public AudioStreamPlayback {
+
+	GDCLASS(AudioStreamPlaybackRandomPitch, AudioStreamPlayback)
+	friend class AudioStreamRandomPitch;
+
+	Ref<AudioStreamRandomPitch> random_pitch;
+	Ref<AudioStreamPlayback> playback;
+	Ref<AudioStreamPlayback> playing;
+	float pitch_scale;
+
+public:
+	virtual void start(float p_from_pos = 0.0);
+	virtual void stop();
+	virtual bool is_playing() const;
+
+	virtual int get_loop_count() const; //times it looped
+
+	virtual float get_pos() const;
+	virtual void seek_pos(float p_time);
+
+	virtual void mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames);
+
+	virtual float get_length() const; //if supported, otherwise return 0
+
+	~AudioStreamPlaybackRandomPitch();
+};
+
 #endif // AUDIO_STREAM_H
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();
 };
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index bcc58b3aac..c407a17bc6 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -809,162 +809,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	}
 
 	return collided;
-
-#if 0
-	//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..
-
-
-	colliding=false;
-	ERR_FAIL_COND_V(!is_inside_tree(),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;
-	Vector2 sr[max_shapes*2];
-	int res_shapes;
-
-	Set<RID> exclude;
-	exclude.insert(get_rid());
-
-
-	//recover first
-	int recover_attempts=4;
-
-	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;
-
-	//print_line("motion: "+p_motion+" margin: "+rtos(margin));
-
-	//print_line("margin: "+rtos(margin));
-	do {
-
-		//motion recover
-		for(int i=0;i<get_shape_count();i++) {
-
-			if (is_shape_set_as_trigger(i))
-				continue;
-			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask))
-				collided=true;
-
-		}
-
-		if (!collided)
-			break;
-
-		Vector2 recover_motion;
-
-		for(int i=0;i<res_shapes;i++) {
-
-			Vector2 a = sr[i*2+0];
-			Vector2 b = sr[i*2+1];
-
-			real_t d = a.distance_to(b);
-
-			/*
-			if (d<margin)
-				continue;
-			*/
-			recover_motion+=(b-a)*0.4;
-		}
-
-		if (recover_motion==Vector2()) {
-			collided=false;
-			break;
-		}
-
-		Matrix32 gt = get_global_transform();
-		gt.elements[2]+=recover_motion;
-		set_global_transform(gt);
-
-		recover_attempts--;
-
-	} while (recover_attempts);
-
-
-	//move second
-	real_t safe = 1.0;
-	real_t unsafe = 1.0;
-	int best_shape=-1;
-
-	for(int i=0;i<get_shape_count();i++) {
-
-		if (is_shape_set_as_trigger(i))
-			continue;
-
-		real_t 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,get_layer_mask(),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 (lsafe < safe) {
-
-			safe=lsafe;
-			unsafe=lunsafe;
-			best_shape=i;
-		}
-	}
-
-
-	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
-
-	if (safe>=1) {
-		//not collided
-		colliding=false;
-	} else {
-
-		//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,get_layer_mask(),mask);
-		if (!c2) {
-			//should not happen, but floating point precision is so weird..
-
-			colliding=false;
-		} else {
-
-
-			//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;
-			collider_shape=rest_info.shape;
-			collider_metadata=rest_info.metadata;
-		}
-
-	}
-
-	Vector2 motion=p_motion*safe;
-	Matrix32 gt = get_global_transform();
-	gt.elements[2]+=motion;
-	set_global_transform(gt);
-
-	return p_motion-motion;
-
-#endif
-	return false;
 }
 
 void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 21c65a74d0..0f07fca637 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -156,26 +156,28 @@ protected:
 	static void _bind_methods();
 
 public:
-	struct RayResult {
+	struct ShapeResult {
 
-		Vector3 position;
-		Vector3 normal;
 		RID rid;
 		ObjectID collider_id;
 		Object *collider;
 		int shape;
 	};
 
-	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) = 0;
+	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) = 0;
 
-	struct ShapeResult {
+	struct RayResult {
 
+		Vector3 position;
+		Vector3 normal;
 		RID rid;
 		ObjectID collider_id;
 		Object *collider;
 		int shape;
 	};
 
+	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) = 0;
+
 	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float 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) = 0;
 
 	struct ShapeRestInfo {
@@ -194,6 +196,8 @@ public:
 
 	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float 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) = 0;
 
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const = 0;
+
 	PhysicsDirectSpaceState();
 };
 
@@ -322,6 +326,8 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0;
 	virtual void area_clear_shapes(RID p_area) = 0;
 
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) = 0;
+
 	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0;
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0;
 
@@ -370,12 +376,11 @@ public:
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0;
 	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0;
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0;
-
 	virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0;
 	virtual void body_clear_shapes(RID p_body) = 0;
 
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) = 0;
+
 	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0;
 	virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0;
 
@@ -458,6 +463,23 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;
 	virtual bool body_is_ray_pickable(RID p_body) const = 0;
 
+	struct MotionResult {
+
+		Vector3 motion;
+		Vector3 remainder;
+
+		Vector3 collision_point;
+		Vector3 collision_normal;
+		Vector3 collider_velocity;
+		int collision_local_shape;
+		ObjectID collider_id;
+		RID collider;
+		int collider_shape;
+		Variant collider_metadata;
+	};
+
+	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) = 0;
+
 	/* JOINT API */
 
 	enum JointType {
diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp
index 79b994dd27..e97a6baeba 100644
--- a/servers/register_server_types.cpp
+++ b/servers/register_server_types.cpp
@@ -84,6 +84,7 @@ void register_server_types() {
 
 	ClassDB::register_virtual_class<AudioStream>();
 	ClassDB::register_virtual_class<AudioStreamPlayback>();
+	ClassDB::register_class<AudioStreamRandomPitch>();
 	ClassDB::register_virtual_class<AudioEffect>();
 	ClassDB::register_class<AudioBusLayout>();
 
diff --git a/servers/visual/visual_server_scene.cpp b/servers/visual/visual_server_scene.cpp
index fb1c66d0b9..5009d76d43 100644
--- a/servers/visual/visual_server_scene.cpp
+++ b/servers/visual/visual_server_scene.cpp
@@ -1482,6 +1482,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 
 				// a pre pass will need to be needed to determine the actual z-near to be used
 
+				Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2));
+
 				for (int j = 0; j < cull_count; j++) {
 
 					float min, max;
@@ -1494,6 +1496,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 					}
 
 					instance->transformed_aabb.project_range_in_plane(Plane(z_vec, 0), min, max);
+					instance->depth = near_plane.distance_to(instance->transform.origin);
+					instance->depth_layer = 0;
 					if (max > z_max)
 						z_max = max;
 				}
@@ -1539,6 +1543,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 						planes[4] = p_instance->transform.xform(Plane(Vector3(0, -1, z).normalized(), radius));
 
 						int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
+						Plane near_plane(p_instance->transform.origin, p_instance->transform.basis.get_axis(2) * z);
 
 						for (int j = 0; j < cull_count; j++) {
 
@@ -1547,6 +1552,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 								cull_count--;
 								SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 								j--;
+							} else {
+								instance->depth = near_plane.distance_to(instance->transform.origin);
+								instance->depth_layer = 0;
 							}
 						}
 
@@ -1587,6 +1595,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 
 						int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
 
+						Plane near_plane(xform.origin, -xform.basis.get_axis(2));
 						for (int j = 0; j < cull_count; j++) {
 
 							Instance *instance = instance_shadow_cull_result[j];
@@ -1594,6 +1603,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 								cull_count--;
 								SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 								j--;
+							} else {
+								instance->depth = near_plane.distance_to(instance->transform.origin);
+								instance->depth_layer = 0;
 							}
 						}
 
@@ -1619,6 +1631,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 			Vector<Plane> planes = cm.get_projection_planes(p_instance->transform);
 			int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
 
+			Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2));
 			for (int j = 0; j < cull_count; j++) {
 
 				Instance *instance = instance_shadow_cull_result[j];
@@ -1626,6 +1639,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 					cull_count--;
 					SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 					j--;
+				} else {
+					instance->depth = near_plane.distance_to(instance->transform.origin);
+					instance->depth_layer = 0;
 				}
 			}
 
@@ -3344,6 +3360,8 @@ void VisualServerScene::_update_dirty_instance(Instance *p_instance) {
 					for (int i = 0; i < dp; i++) {
 
 						RID mesh = VSG::storage->particles_get_draw_pass_mesh(p_instance->base, i);
+						if (!mesh.is_valid())
+							continue;
 
 						int sc = VSG::storage->mesh_get_surface_count(mesh);
 						for (int j = 0; j < sc; j++) {