23 files changed, 2283 insertions, 1810 deletions

diff --git a/scene/3d/area.cpp b/scene/3d/area.cpp
index 39a4e926b2..59227070b3 100644
--- a/scene/3d/area.cpp
+++ b/scene/3d/area.cpp
@@ -29,7 +29,9 @@
 /*************************************************************************/
 #include "area.h"
 #include "scene/scene_string_names.h"
+#include "servers/audio_server.h"
 #include "servers/physics_server.h"
+
 void Area::set_space_override_mode(SpaceOverride p_mode) {
 
 	space_override = p_mode;
@@ -227,7 +229,11 @@ void Area::_clear_monitoring() {
 
 			Object *obj = ObjectDB::get_instance(E->key());
 			Node *node = obj ? obj->cast_to<Node>() : NULL;
-			ERR_CONTINUE(!node);
+
+			if (!node) //node may have been deleted in previous frame or at other legiminate point
+				continue;
+			//ERR_CONTINUE(!node);
+
 			if (!E->get().in_tree)
 				continue;
 
@@ -253,7 +259,11 @@ void Area::_clear_monitoring() {
 
 			Object *obj = ObjectDB::get_instance(E->key());
 			Node *node = obj ? obj->cast_to<Node>() : NULL;
-			ERR_CONTINUE(!node);
+
+			if (!node) //node may have been deleted in previous frame or at other legiminate point
+				continue;
+			//ERR_CONTINUE(!node);
+
 			if (!E->get().in_tree)
 				continue;
 
@@ -530,6 +540,87 @@ bool Area::get_collision_layer_bit(int p_bit) const {
 	return get_collision_layer() & (1 << p_bit);
 }
 
+void Area::set_audio_bus_override(bool p_override) {
+
+	audio_bus_override = p_override;
+}
+
+bool Area::is_overriding_audio_bus() const {
+
+	return audio_bus_override;
+}
+
+void Area::set_audio_bus(const StringName &p_audio_bus) {
+
+	audio_bus = p_audio_bus;
+}
+StringName Area::get_audio_bus() const {
+
+	for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) {
+		if (AudioServer::get_singleton()->get_bus_name(i) == audio_bus) {
+			return audio_bus;
+		}
+	}
+	return "Master";
+}
+
+void Area::set_use_reverb_bus(bool p_enable) {
+
+	use_reverb_bus = p_enable;
+}
+bool Area::is_using_reverb_bus() const {
+
+	return use_reverb_bus;
+}
+
+void Area::set_reverb_bus(const StringName &p_audio_bus) {
+
+	reverb_bus = p_audio_bus;
+}
+StringName Area::get_reverb_bus() const {
+
+	for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) {
+		if (AudioServer::get_singleton()->get_bus_name(i) == reverb_bus) {
+			return reverb_bus;
+		}
+	}
+	return "Master";
+}
+
+void Area::set_reverb_amount(float p_amount) {
+
+	reverb_amount = p_amount;
+}
+float Area::get_reverb_amount() const {
+
+	return reverb_amount;
+}
+
+void Area::set_reverb_uniformity(float p_uniformity) {
+
+	reverb_uniformity = p_uniformity;
+}
+float Area::get_reverb_uniformity() const {
+
+	return reverb_uniformity;
+}
+
+void Area::_validate_property(PropertyInfo &property) const {
+
+	if (property.name == "audio_bus_name" || property.name == "reverb_bus_name") {
+
+		String options;
+		for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) {
+			if (i > 0)
+				options += ",";
+			String name = AudioServer::get_singleton()->get_bus_name(i);
+			options += name;
+		}
+
+		property.hint_string = options;
+	}
+}
+
 void Area::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("_body_enter_tree", "id"), &Area::_body_enter_tree);
@@ -589,6 +680,24 @@ void Area::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("_body_inout"), &Area::_body_inout);
 	ClassDB::bind_method(D_METHOD("_area_inout"), &Area::_area_inout);
 
+	ClassDB::bind_method(D_METHOD("set_audio_bus_override", "enable"), &Area::set_audio_bus_override);
+	ClassDB::bind_method(D_METHOD("is_overriding_audio_bus"), &Area::is_overriding_audio_bus);
+
+	ClassDB::bind_method(D_METHOD("set_audio_bus", "name"), &Area::set_audio_bus);
+	ClassDB::bind_method(D_METHOD("get_audio_bus"), &Area::get_audio_bus);
+
+	ClassDB::bind_method(D_METHOD("set_use_reverb_bus", "enable"), &Area::set_use_reverb_bus);
+	ClassDB::bind_method(D_METHOD("is_using_reverb_bus"), &Area::is_using_reverb_bus);
+
+	ClassDB::bind_method(D_METHOD("set_reverb_bus", "name"), &Area::set_reverb_bus);
+	ClassDB::bind_method(D_METHOD("get_reverb_bus"), &Area::get_reverb_bus);
+
+	ClassDB::bind_method(D_METHOD("set_reverb_amount", "amount"), &Area::set_reverb_amount);
+	ClassDB::bind_method(D_METHOD("get_reverb_amount"), &Area::get_reverb_amount);
+
+	ClassDB::bind_method(D_METHOD("set_reverb_uniformity", "amount"), &Area::set_reverb_uniformity);
+	ClassDB::bind_method(D_METHOD("get_reverb_uniformity"), &Area::get_reverb_uniformity);
+
 	ADD_SIGNAL(MethodInfo("body_shape_entered", PropertyInfo(Variant::INT, "body_id"), PropertyInfo(Variant::OBJECT, "body"), PropertyInfo(Variant::INT, "body_shape"), PropertyInfo(Variant::INT, "area_shape")));
 	ADD_SIGNAL(MethodInfo("body_shape_exited", PropertyInfo(Variant::INT, "body_id"), PropertyInfo(Variant::OBJECT, "body"), PropertyInfo(Variant::INT, "body_shape"), PropertyInfo(Variant::INT, "area_shape")));
 	ADD_SIGNAL(MethodInfo("body_entered", PropertyInfo(Variant::OBJECT, "body")));
@@ -612,6 +721,14 @@ void Area::_bind_methods() {
 	ADD_GROUP("Collision", "collision_");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_layer", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_layer", "get_collision_layer");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
+	ADD_GROUP("Audio Bus", "audio_bus_");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "audio_bus_override"), "set_audio_bus_override", "is_overriding_audio_bus");
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "audio_bus_name", PROPERTY_HINT_ENUM, ""), "set_audio_bus", "get_audio_bus");
+	ADD_GROUP("Reverb Bus", "reverb_bus_");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "reverb_bus_enable"), "set_use_reverb_bus", "is_using_reverb_bus");
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "reverb_bus_name", PROPERTY_HINT_ENUM, ""), "set_reverb_bus", "get_reverb_bus");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "reverb_bus_amount", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_reverb_amount", "get_reverb_amount");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "reverb_bus_uniformity", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_reverb_uniformity", "get_reverb_uniformity");
 }
 
 Area::Area()
@@ -632,6 +749,14 @@ Area::Area()
 	set_ray_pickable(false);
 	set_monitoring(true);
 	set_monitorable(true);
+
+	audio_bus_override = false;
+	audio_bus = "Master";
+
+	use_reverb_bus = false;
+	reverb_bus = "Master";
+	reverb_amount = 0.0;
+	reverb_uniformity = 0.0;
 }
 
 Area::~Area() {
diff --git a/scene/3d/area.h b/scene/3d/area.h
index 279a52ee69..5df308fc47 100644
--- a/scene/3d/area.h
+++ b/scene/3d/area.h
@@ -126,6 +126,16 @@ private:
 	Map<ObjectID, AreaState> area_map;
 	void _clear_monitoring();
 
+	bool audio_bus_override;
+	StringName audio_bus;
+
+	bool use_reverb_bus;
+	StringName reverb_bus;
+	float reverb_amount;
+	float reverb_uniformity;
+
+	void _validate_property(PropertyInfo &property) const;
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
@@ -179,6 +189,24 @@ public:
 	bool overlaps_area(Node *p_area) const;
 	bool overlaps_body(Node *p_body) const;
 
+	void set_audio_bus_override(bool p_override);
+	bool is_overriding_audio_bus() const;
+
+	void set_audio_bus(const StringName &p_audio_bus);
+	StringName get_audio_bus() const;
+
+	void set_use_reverb_bus(bool p_enable);
+	bool is_using_reverb_bus() const;
+
+	void set_reverb_bus(const StringName &p_audio_bus);
+	StringName get_reverb_bus() const;
+
+	void set_reverb_amount(float p_amount);
+	float get_reverb_amount() const;
+
+	void set_reverb_uniformity(float p_uniformity);
+	float get_reverb_uniformity() const;
+
 	Area();
 	~Area();
 };
diff --git a/scene/3d/audio_stream_player_3d.cpp b/scene/3d/audio_stream_player_3d.cpp
new file mode 100644
index 0000000000..87ea018425
--- /dev/null
+++ b/scene/3d/audio_stream_player_3d.cpp
@@ -0,0 +1,930 @@
+#include "audio_stream_player_3d.h"
+#include "engine.h"
+#include "scene/3d/area.h"
+#include "scene/3d/camera.h"
+#include "scene/main/viewport.h"
+void AudioStreamPlayer3D::_mix_audio() {
+
+	if (!stream_playback.is_valid()) {
+		return;
+	}
+
+	if (!active) {
+		return;
+	}
+
+	bool started = false;
+	if (setseek >= 0.0) {
+		stream_playback->start(setseek);
+		setseek = -1.0; //reset seek
+		started = true;
+	}
+
+	//get data
+	AudioFrame *buffer = mix_buffer.ptr();
+	int buffer_size = mix_buffer.size();
+
+	//mix
+	if (output_count > 0 || out_of_range_mode == OUT_OF_RANGE_MIX) {
+
+		float pitch_scale = 0.0;
+		if (output_count) {
+			//used for doppler, not realistic but good enough
+			for (int i = 0; i < output_count; i++) {
+				pitch_scale += outputs[i].pitch_scale;
+			}
+			pitch_scale /= float(output_count);
+		} else {
+			pitch_scale = 1.0;
+		}
+
+		stream_playback->mix(buffer, pitch_scale, buffer_size);
+	}
+
+	//write all outputs
+	for (int i = 0; i < output_count; i++) {
+
+		Output current = outputs[i];
+
+		//see if current output exists, to keep volume ramp
+		bool found = false;
+		for (int j = i; j < prev_output_count; j++) {
+			if (prev_outputs[j].viewport == current.viewport) {
+				if (j != i) {
+					SWAP(prev_outputs[j], prev_outputs[i]);
+				}
+				found = true;
+				break;
+			}
+		}
+
+		bool interpolate_filter = !started;
+		;
+		if (!found) {
+			//create new if was not used before
+			if (prev_output_count < MAX_OUTPUTS) {
+				prev_outputs[prev_output_count] = prev_outputs[i]; //may be owned by another viewport
+				prev_output_count++;
+			}
+			prev_outputs[i] = current;
+			interpolate_filter = false;
+		}
+
+		//mix!
+
+		int buffers = 0;
+		int first = 0;
+
+		switch (AudioServer::get_singleton()->get_speaker_mode()) {
+
+			case AudioServer::SPEAKER_MODE_STEREO: {
+				buffers = 1;
+				first = 0;
+
+			} break;
+			case AudioServer::SPEAKER_SURROUND_51: {
+				buffers = 2;
+				first = 1;
+
+			} break;
+			case AudioServer::SPEAKER_SURROUND_71: {
+
+				buffers = 3;
+				first = 1;
+
+			} break;
+		}
+
+		for (int k = 0; k < buffers; k++) {
+			AudioFrame vol_inc = (current.vol[k] - prev_outputs[i].vol[k]) / float(buffer_size);
+			AudioFrame vol = current.vol[k];
+
+			AudioFrame *target = AudioServer::get_singleton()->thread_get_channel_mix_buffer(current.bus_index, first + k);
+
+			current.filter.set_mode(AudioFilterSW::HIGHSHELF);
+			current.filter.set_sampling_rate(AudioServer::get_singleton()->get_mix_rate());
+			current.filter.set_cutoff(attenuation_filter_cutoff_hz);
+			current.filter.set_resonance(1);
+			current.filter.set_stages(1);
+			current.filter.set_gain(current.filter_gain);
+
+			if (interpolate_filter) {
+
+				current.filter_process[k * 2 + 0] = prev_outputs[i].filter_process[k * 2 + 0];
+				current.filter_process[k * 2 + 1] = prev_outputs[i].filter_process[k * 2 + 1];
+
+				current.filter_process[k * 2 + 0].set_filter(&current.filter, false);
+				current.filter_process[k * 2 + 1].set_filter(&current.filter, false);
+
+				current.filter_process[k * 2 + 0].update_coeffs(buffer_size);
+				current.filter_process[k * 2 + 1].update_coeffs(buffer_size);
+				for (int j = 0; j < buffer_size; j++) {
+
+					AudioFrame f = buffer[j] * vol;
+					current.filter_process[k * 2 + 0].process_one_interp(f.l);
+					current.filter_process[k * 2 + 1].process_one_interp(f.r);
+
+					target[j] += f;
+					vol += vol_inc;
+				}
+			} else {
+				current.filter_process[k * 2 + 0].set_filter(&current.filter);
+				current.filter_process[k * 2 + 1].set_filter(&current.filter);
+
+				current.filter_process[k * 2 + 0].update_coeffs();
+				current.filter_process[k * 2 + 1].update_coeffs();
+				for (int j = 0; j < buffer_size; j++) {
+
+					AudioFrame f = buffer[j] * vol;
+					current.filter_process[k * 2 + 0].process_one(f.l);
+					current.filter_process[k * 2 + 1].process_one(f.r);
+
+					target[j] += f;
+					vol += vol_inc;
+				}
+			}
+
+			if (current.reverb_bus_index >= 0) {
+
+				AudioFrame *rtarget = AudioServer::get_singleton()->thread_get_channel_mix_buffer(current.reverb_bus_index, first + k);
+
+				if (current.reverb_bus_index == prev_outputs[i].reverb_bus_index) {
+					AudioFrame rvol_inc = (current.reverb_vol[k] - prev_outputs[i].reverb_vol[k]) / float(buffer_size);
+					AudioFrame rvol = prev_outputs[i].reverb_vol[k];
+
+					for (int j = 0; j < buffer_size; j++) {
+
+						rtarget[j] += buffer[j] * rvol;
+						rvol += rvol_inc;
+					}
+				} else {
+
+					AudioFrame rvol = current.reverb_vol[k];
+					for (int j = 0; j < buffer_size; j++) {
+
+						rtarget[j] += buffer[j] * rvol;
+					}
+				}
+			}
+		}
+
+		prev_outputs[i] = current;
+	}
+
+	prev_output_count = output_count;
+
+	//stream is no longer active, disable this.
+	if (!stream_playback->is_playing()) {
+		active = false;
+	}
+
+	output_ready = false;
+}
+
+float AudioStreamPlayer3D::_get_attenuation_db(float p_distance) const {
+
+	float att;
+	switch (attenuation_model) {
+		case ATTENUATION_INVERSE_DISTANCE: {
+			att = Math::linear2db(1.0 / ((p_distance / unit_size) + 000001));
+		} break;
+		case ATTENUATION_INVERSE_SQUARE_DISTANCE: {
+			float d = (p_distance / unit_size);
+			d *= d;
+			att = Math::linear2db(1.0 / (d + 0.00001));
+		} break;
+		case ATTENUATION_LOGARITHMIC: {
+			att = -20 * Math::log(p_distance / unit_size + 000001);
+		} break;
+	}
+
+	att += unit_db;
+	if (att > max_db) {
+		att = max_db;
+	}
+
+	return att;
+}
+
+void _update_sound() {
+}
+
+void AudioStreamPlayer3D::_notification(int p_what) {
+
+	if (p_what == NOTIFICATION_ENTER_TREE) {
+
+		velocity_tracker->reset(get_global_transform().origin);
+		AudioServer::get_singleton()->add_callback(_mix_audios, this);
+		if (autoplay && !get_tree()->is_editor_hint()) {
+			play();
+		}
+	}
+
+	if (p_what == NOTIFICATION_EXIT_TREE) {
+
+		AudioServer::get_singleton()->remove_callback(_mix_audios, this);
+	}
+	if (p_what == NOTIFICATION_TRANSFORM_CHANGED) {
+
+		if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+			velocity_tracker->update_position(get_global_transform().origin);
+		}
+	}
+
+	if (p_what == NOTIFICATION_INTERNAL_FIXED_PROCESS) {
+
+		//update anything related to position first, if possible of course
+
+		if (!output_ready) {
+
+			Vector3 linear_velocity;
+
+			//compute linear velocity for doppler
+			if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+				linear_velocity = velocity_tracker->get_tracked_linear_velocity();
+			}
+
+			Ref<World> world = get_world();
+			ERR_FAIL_COND(world.is_null());
+
+			int new_output_count = 0;
+
+			Vector3 global_pos = get_global_transform().origin;
+
+			int bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus);
+
+			//check if any area is diverting sound into a bus
+
+			PhysicsDirectSpaceState *space_state = PhysicsServer::get_singleton()->space_get_direct_state(world->get_space());
+
+			PhysicsDirectSpaceState::ShapeResult sr[MAX_INTERSECT_AREAS];
+
+			int areas = space_state->intersect_point(global_pos, sr, MAX_INTERSECT_AREAS, Set<RID>(), area_mask, PhysicsDirectSpaceState::TYPE_MASK_AREA);
+			Area *area = NULL;
+
+			for (int i = 0; i < areas; i++) {
+				if (!sr[i].collider)
+					continue;
+
+				Area *tarea = sr[i].collider->cast_to<Area>();
+				if (!tarea)
+					continue;
+
+				if (!tarea->is_overriding_audio_bus() && !tarea->is_using_reverb_bus())
+					continue;
+
+				area = tarea;
+				break;
+			}
+
+			List<Camera *> cameras;
+			world->get_camera_list(&cameras);
+
+			for (List<Camera *>::Element *E = cameras.front(); E; E = E->next()) {
+
+				Camera *camera = E->get();
+				Viewport *vp = camera->get_viewport();
+				if (!vp->is_audio_listener())
+					continue;
+
+				Vector3 local_pos = camera->get_global_transform().orthonormalized().affine_inverse().xform(global_pos);
+
+				float dist = local_pos.length();
+
+				Vector3 area_sound_pos;
+				Vector3 cam_area_pos;
+
+				if (area && area->is_using_reverb_bus() && area->get_reverb_uniformity() > 0) {
+					area_sound_pos = space_state->get_closest_point_to_object_volume(area->get_rid(), camera->get_global_transform().origin);
+					cam_area_pos = camera->get_global_transform().affine_inverse().xform(area_sound_pos);
+				}
+
+				if (max_distance > 0) {
+
+					float total_max = max_distance;
+
+					if (area && area->is_using_reverb_bus() && area->get_reverb_uniformity() > 0) {
+						total_max = MAX(total_max, cam_area_pos.length());
+					}
+					if (total_max > max_distance) {
+						continue; //cant hear this sound in this camera
+					}
+				}
+
+				float multiplier = Math::db2linear(_get_attenuation_db(dist));
+				if (max_distance > 0) {
+					multiplier *= MAX(0, 1.0 - (dist / max_distance));
+				}
+
+				Output output;
+				output.bus_index = bus_index;
+				output.reverb_bus_index = -1; //no reverb by default
+				output.viewport = vp;
+
+				float db_att = (1.0 - MIN(1.0, multiplier)) * attenuation_filter_db;
+
+				if (emission_angle_enabled) {
+					Vector3 camtopos = global_pos - camera->get_global_transform().origin;
+					float c = camtopos.normalized().dot(get_global_transform().basis.get_axis(2).normalized()); //it's z negative
+					float angle = Math::rad2deg(Math::acos(c));
+					if (angle > emission_angle)
+						db_att -= -emission_angle_filter_attenuation_db;
+				}
+
+				output.filter_gain = Math::db2linear(db_att);
+
+				Vector3 flat_pos = local_pos;
+				flat_pos.y = 0;
+				flat_pos.normalize();
+
+				switch (AudioServer::get_singleton()->get_speaker_mode()) {
+
+					case AudioServer::SPEAKER_MODE_STEREO: {
+
+						float c = flat_pos.x * 0.5 + 0.5;
+						output.vol[0].l = 1.0 - c;
+						output.vol[0].r = c;
+
+						output.vol[0] *= multiplier;
+
+					} break;
+					case AudioServer::SPEAKER_SURROUND_51: {
+
+						float xl = Vector3(-1, 0, -1).normalized().dot(flat_pos) * 0.5 + 0.5;
+						float xr = Vector3(1, 0, -1).normalized().dot(flat_pos) * 0.5 + 0.5;
+
+						output.vol[0].l = xl;
+						output.vol[1].r = 1.0 - xl;
+						output.vol[0].r = xr;
+						output.vol[1].l = 1.0 - xr;
+
+						output.vol[0] *= multiplier;
+						output.vol[1] *= multiplier;
+					} break;
+					case AudioServer::SPEAKER_SURROUND_71: {
+
+						float xl = Vector3(-1, 0, -1).normalized().dot(flat_pos) * 0.5 + 0.5;
+						float xr = Vector3(1, 0, -1).normalized().dot(flat_pos) * 0.5 + 0.5;
+
+						output.vol[0].l = xl;
+						output.vol[1].r = 1.0 - xl;
+						output.vol[0].r = xr;
+						output.vol[1].l = 1.0 - xr;
+
+						float c = flat_pos.x * 0.5 + 0.5;
+						output.vol[2].l = 1.0 - c;
+						output.vol[2].r = c;
+
+						output.vol[0] *= multiplier;
+						output.vol[1] *= multiplier;
+						output.vol[2] *= multiplier;
+
+					} break;
+				}
+
+				bool filled_reverb = false;
+				int vol_index_max = AudioServer::get_singleton()->get_speaker_mode() + 1;
+
+				if (area) {
+
+					if (area->is_overriding_audio_bus()) {
+						//override audio bus
+						StringName bus_name = area->get_audio_bus();
+						output.bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus_name);
+					}
+
+					if (area->is_using_reverb_bus()) {
+
+						filled_reverb = true;
+						StringName bus_name = area->get_reverb_bus();
+						output.reverb_bus_index = AudioServer::get_singleton()->thread_find_bus_index(bus_name);
+
+						float uniformity = area->get_reverb_uniformity();
+						float area_send = area->get_reverb_amount();
+
+						if (uniformity > 0.0) {
+
+							float distance = cam_area_pos.length();
+							float attenuation = Math::db2linear(_get_attenuation_db(distance));
+
+							//float dist_att_db = -20 * Math::log(dist + 0.00001); //logarithmic attenuation, like in real life
+
+							float center_val[3] = { 0.5, 0.25, 0.16666 };
+							AudioFrame center_frame(center_val[vol_index_max - 1], center_val[vol_index_max - 1]);
+
+							if (attenuation < 1.0) {
+								//pan the uniform sound
+								Vector3 rev_pos = cam_area_pos;
+								rev_pos.y = 0;
+								rev_pos.normalize();
+
+								switch (AudioServer::get_singleton()->get_speaker_mode()) {
+
+									case AudioServer::SPEAKER_MODE_STEREO: {
+
+										float c = rev_pos.x * 0.5 + 0.5;
+										output.reverb_vol[0].l = 1.0 - c;
+										output.reverb_vol[0].r = c;
+
+									} break;
+									case AudioServer::SPEAKER_SURROUND_51: {
+
+										float xl = Vector3(-1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5;
+										float xr = Vector3(1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5;
+
+										output.reverb_vol[0].l = xl;
+										output.reverb_vol[1].r = 1.0 - xl;
+										output.reverb_vol[0].r = xr;
+										output.reverb_vol[1].l = 1.0 - xr;
+
+									} break;
+									case AudioServer::SPEAKER_SURROUND_71: {
+
+										float xl = Vector3(-1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5;
+										float xr = Vector3(1, 0, -1).normalized().dot(rev_pos) * 0.5 + 0.5;
+
+										output.reverb_vol[0].l = xl;
+										output.reverb_vol[1].r = 1.0 - xl;
+										output.reverb_vol[0].r = xr;
+										output.reverb_vol[1].l = 1.0 - xr;
+
+										float c = rev_pos.x * 0.5 + 0.5;
+										output.reverb_vol[2].l = 1.0 - c;
+										output.reverb_vol[2].r = c;
+
+									} break;
+								}
+
+								for (int i = 0; i < vol_index_max; i++) {
+
+									output.reverb_vol[i] = output.reverb_vol[i].linear_interpolate(center_frame, attenuation);
+								}
+							} else {
+								for (int i = 0; i < vol_index_max; i++) {
+
+									output.reverb_vol[i] = center_frame;
+								}
+							}
+
+							for (int i = 0; i < vol_index_max; i++) {
+
+								output.reverb_vol[i] = output.vol[i].linear_interpolate(output.reverb_vol[i] * attenuation, uniformity);
+								output.reverb_vol[i] *= area_send;
+							}
+
+						} else {
+
+							for (int i = 0; i < vol_index_max; i++) {
+
+								output.reverb_vol[i] = output.vol[i] * area_send;
+							}
+						}
+					}
+				}
+
+				if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+
+					Vector3 camera_velocity = camera->get_doppler_tracked_velocity();
+
+					Vector3 local_velocity = camera->get_global_transform().orthonormalized().basis.xform_inv(linear_velocity - camera_velocity);
+
+					if (local_velocity == Vector3()) {
+						output.pitch_scale = 1.0;
+					} else {
+						float approaching = local_pos.normalized().dot(local_velocity.normalized());
+						float velocity = local_velocity.length();
+						float speed_of_sound = 343.0;
+
+						output.pitch_scale = speed_of_sound / (speed_of_sound + velocity * approaching);
+						output.pitch_scale = CLAMP(output.pitch_scale, (1 / 8.0), 8.0); //avoid crazy stuff
+					}
+
+				} else {
+					output.pitch_scale = 1.0;
+				}
+
+				if (!filled_reverb) {
+
+					for (int i = 0; i < vol_index_max; i++) {
+
+						output.reverb_vol[i] = AudioFrame(0, 0);
+					}
+				}
+
+				outputs[new_output_count] = output;
+				new_output_count++;
+				if (new_output_count == MAX_OUTPUTS)
+					break;
+			}
+
+			output_count = new_output_count;
+			output_ready = true;
+		}
+
+		//start playing if requested
+		if (setplay >= 0.0) {
+			setseek = setplay;
+			active = true;
+			setplay = -1;
+			_change_notify("playing"); //update property in editor
+		}
+
+		//stop playing if no longer active
+		if (!active) {
+			set_fixed_process_internal(false);
+			_change_notify("playing"); //update property in editor
+		}
+	}
+}
+
+void AudioStreamPlayer3D::set_stream(Ref<AudioStream> p_stream) {
+
+	ERR_FAIL_COND(!p_stream.is_valid());
+	AudioServer::get_singleton()->lock();
+
+	mix_buffer.resize(AudioServer::get_singleton()->thread_get_mix_buffer_size());
+
+	if (stream_playback.is_valid()) {
+		stream_playback.unref();
+		stream.unref();
+		active = false;
+		setseek = -1;
+	}
+
+	stream = p_stream;
+	stream_playback = p_stream->instance_playback();
+
+	if (stream_playback.is_null()) {
+		stream.unref();
+		ERR_FAIL_COND(stream_playback.is_null());
+	}
+
+	AudioServer::get_singleton()->unlock();
+}
+
+Ref<AudioStream> AudioStreamPlayer3D::get_stream() const {
+
+	return stream;
+}
+
+void AudioStreamPlayer3D::set_unit_db(float p_volume) {
+
+	unit_db = p_volume;
+}
+float AudioStreamPlayer3D::get_unit_db() const {
+
+	return unit_db;
+}
+
+void AudioStreamPlayer3D::set_unit_size(float p_volume) {
+
+	unit_size = p_volume;
+}
+float AudioStreamPlayer3D::get_unit_size() const {
+
+	return unit_size;
+}
+
+void AudioStreamPlayer3D::set_max_db(float p_boost) {
+
+	max_db = p_boost;
+}
+float AudioStreamPlayer3D::get_max_db() const {
+
+	return max_db;
+}
+
+void AudioStreamPlayer3D::play(float p_from_pos) {
+
+	if (stream_playback.is_valid()) {
+		setplay = p_from_pos;
+		output_ready = false;
+		set_fixed_process_internal(true);
+	}
+}
+
+void AudioStreamPlayer3D::seek(float p_seconds) {
+
+	if (stream_playback.is_valid()) {
+		setseek = p_seconds;
+	}
+}
+
+void AudioStreamPlayer3D::stop() {
+
+	if (stream_playback.is_valid()) {
+		active = false;
+		set_fixed_process_internal(false);
+		setplay = -1;
+	}
+}
+
+bool AudioStreamPlayer3D::is_playing() const {
+
+	if (stream_playback.is_valid()) {
+		return active; // && stream_playback->is_playing();
+	}
+
+	return false;
+}
+
+float AudioStreamPlayer3D::get_pos() {
+
+	if (stream_playback.is_valid()) {
+		return stream_playback->get_pos();
+	}
+
+	return 0;
+}
+
+void AudioStreamPlayer3D::set_bus(const StringName &p_bus) {
+
+	//if audio is active, must lock this
+	AudioServer::get_singleton()->lock();
+	bus = p_bus;
+	AudioServer::get_singleton()->unlock();
+}
+StringName AudioStreamPlayer3D::get_bus() const {
+
+	for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) {
+		if (AudioServer::get_singleton()->get_bus_name(i) == bus) {
+			return bus;
+		}
+	}
+	return "Master";
+}
+
+void AudioStreamPlayer3D::set_autoplay(bool p_enable) {
+
+	autoplay = p_enable;
+}
+bool AudioStreamPlayer3D::is_autoplay_enabled() {
+
+	return autoplay;
+}
+
+void AudioStreamPlayer3D::_set_playing(bool p_enable) {
+
+	if (p_enable)
+		play();
+	else
+		stop();
+}
+bool AudioStreamPlayer3D::_is_active() const {
+
+	return active;
+}
+
+void AudioStreamPlayer3D::_validate_property(PropertyInfo &property) const {
+
+	if (property.name == "bus") {
+
+		String options;
+		for (int i = 0; i < AudioServer::get_singleton()->get_bus_count(); i++) {
+			if (i > 0)
+				options += ",";
+			String name = AudioServer::get_singleton()->get_bus_name(i);
+			options += name;
+		}
+
+		property.hint_string = options;
+	}
+}
+
+void AudioStreamPlayer3D::_bus_layout_changed() {
+
+	_change_notify();
+}
+
+void AudioStreamPlayer3D::set_max_distance(float p_metres) {
+
+	ERR_FAIL_COND(p_metres < 0.0);
+	max_distance = p_metres;
+}
+
+float AudioStreamPlayer3D::get_max_distance() const {
+
+	return max_distance;
+}
+
+void AudioStreamPlayer3D::set_area_mask(uint32_t p_mask) {
+
+	area_mask = p_mask;
+}
+
+uint32_t AudioStreamPlayer3D::get_area_mask() const {
+
+	return area_mask;
+}
+
+void AudioStreamPlayer3D::set_emission_angle_enabled(bool p_enable) {
+	emission_angle_enabled = p_enable;
+	update_gizmo();
+}
+
+bool AudioStreamPlayer3D::is_emission_angle_enabled() const {
+	return emission_angle_enabled;
+}
+
+void AudioStreamPlayer3D::set_emission_angle(float p_angle) {
+	ERR_FAIL_COND(p_angle < 0 || p_angle > 90);
+	emission_angle = p_angle;
+	update_gizmo();
+}
+
+float AudioStreamPlayer3D::get_emission_angle() const {
+	return emission_angle;
+}
+
+void AudioStreamPlayer3D::set_emission_angle_filter_attenuation_db(float p_angle_attenuation_db) {
+
+	emission_angle_filter_attenuation_db = p_angle_attenuation_db;
+}
+
+float AudioStreamPlayer3D::get_emission_angle_filter_attenuation_db() const {
+
+	return emission_angle_filter_attenuation_db;
+}
+
+void AudioStreamPlayer3D::set_attenuation_filter_cutoff_hz(float p_hz) {
+
+	attenuation_filter_cutoff_hz = p_hz;
+}
+float AudioStreamPlayer3D::get_attenuation_filter_cutoff_hz() const {
+
+	return attenuation_filter_cutoff_hz;
+}
+
+void AudioStreamPlayer3D::set_attenuation_filter_db(float p_db) {
+
+	attenuation_filter_db = p_db;
+}
+float AudioStreamPlayer3D::get_attenuation_filter_db() const {
+
+	return attenuation_filter_db;
+}
+
+void AudioStreamPlayer3D::set_attenuation_model(AttenuationModel p_model) {
+	ERR_FAIL_INDEX(p_model, 3);
+	attenuation_model = p_model;
+}
+
+AudioStreamPlayer3D::AttenuationModel AudioStreamPlayer3D::get_attenuation_model() const {
+	return attenuation_model;
+}
+
+void AudioStreamPlayer3D::set_out_of_range_mode(OutOfRangeMode p_mode) {
+
+	ERR_FAIL_INDEX(p_mode, 2);
+	out_of_range_mode = p_mode;
+}
+
+AudioStreamPlayer3D::OutOfRangeMode AudioStreamPlayer3D::get_out_of_range_mode() const {
+
+	return out_of_range_mode;
+}
+
+void AudioStreamPlayer3D::set_doppler_tracking(DopplerTracking p_tracking) {
+
+	if (doppler_tracking == p_tracking)
+		return;
+
+	doppler_tracking = p_tracking;
+
+	if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+		set_notify_transform(true);
+		velocity_tracker->set_track_fixed_step(doppler_tracking == DOPPLER_TRACKING_FIXED_STEP);
+		velocity_tracker->reset(get_global_transform().origin);
+	} else {
+		set_notify_transform(false);
+	}
+}
+
+AudioStreamPlayer3D::DopplerTracking AudioStreamPlayer3D::get_doppler_tracking() const {
+
+	return doppler_tracking;
+}
+
+void AudioStreamPlayer3D::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_stream", "stream:AudioStream"), &AudioStreamPlayer3D::set_stream);
+	ClassDB::bind_method(D_METHOD("get_stream"), &AudioStreamPlayer3D::get_stream);
+
+	ClassDB::bind_method(D_METHOD("set_unit_db", "unit_db"), &AudioStreamPlayer3D::set_unit_db);
+	ClassDB::bind_method(D_METHOD("get_unit_db"), &AudioStreamPlayer3D::get_unit_db);
+
+	ClassDB::bind_method(D_METHOD("set_unit_size", "unit_size"), &AudioStreamPlayer3D::set_unit_size);
+	ClassDB::bind_method(D_METHOD("get_unit_size"), &AudioStreamPlayer3D::get_unit_size);
+
+	ClassDB::bind_method(D_METHOD("set_max_db", "max_db"), &AudioStreamPlayer3D::set_max_db);
+	ClassDB::bind_method(D_METHOD("get_max_db"), &AudioStreamPlayer3D::get_max_db);
+
+	ClassDB::bind_method(D_METHOD("play", "from_pos"), &AudioStreamPlayer3D::play, DEFVAL(0.0));
+	ClassDB::bind_method(D_METHOD("seek", "to_pos"), &AudioStreamPlayer3D::seek);
+	ClassDB::bind_method(D_METHOD("stop"), &AudioStreamPlayer3D::stop);
+
+	ClassDB::bind_method(D_METHOD("is_playing"), &AudioStreamPlayer3D::is_playing);
+	ClassDB::bind_method(D_METHOD("get_pos"), &AudioStreamPlayer3D::get_pos);
+
+	ClassDB::bind_method(D_METHOD("set_bus", "bus"), &AudioStreamPlayer3D::set_bus);
+	ClassDB::bind_method(D_METHOD("get_bus"), &AudioStreamPlayer3D::get_bus);
+
+	ClassDB::bind_method(D_METHOD("set_autoplay", "enable"), &AudioStreamPlayer3D::set_autoplay);
+	ClassDB::bind_method(D_METHOD("is_autoplay_enabled"), &AudioStreamPlayer3D::is_autoplay_enabled);
+
+	ClassDB::bind_method(D_METHOD("_set_playing", "enable"), &AudioStreamPlayer3D::_set_playing);
+	ClassDB::bind_method(D_METHOD("_is_active"), &AudioStreamPlayer3D::_is_active);
+
+	ClassDB::bind_method(D_METHOD("set_max_distance", "metres"), &AudioStreamPlayer3D::set_max_distance);
+	ClassDB::bind_method(D_METHOD("get_max_distance"), &AudioStreamPlayer3D::get_max_distance);
+
+	ClassDB::bind_method(D_METHOD("set_area_mask", "mask"), &AudioStreamPlayer3D::set_area_mask);
+	ClassDB::bind_method(D_METHOD("get_area_mask"), &AudioStreamPlayer3D::get_area_mask);
+
+	ClassDB::bind_method(D_METHOD("set_emission_angle", "degrees"), &AudioStreamPlayer3D::set_emission_angle);
+	ClassDB::bind_method(D_METHOD("get_emission_angle"), &AudioStreamPlayer3D::get_emission_angle);
+
+	ClassDB::bind_method(D_METHOD("set_emission_angle_enabled", "enabled"), &AudioStreamPlayer3D::set_emission_angle_enabled);
+	ClassDB::bind_method(D_METHOD("is_emission_angle_enabled"), &AudioStreamPlayer3D::is_emission_angle_enabled);
+
+	ClassDB::bind_method(D_METHOD("set_emission_angle_filter_attenuation_db", "db"), &AudioStreamPlayer3D::set_emission_angle_filter_attenuation_db);
+	ClassDB::bind_method(D_METHOD("get_emission_angle_filter_attenuation_db"), &AudioStreamPlayer3D::get_emission_angle_filter_attenuation_db);
+
+	ClassDB::bind_method(D_METHOD("set_attenuation_filter_cutoff_hz", "degrees"), &AudioStreamPlayer3D::set_attenuation_filter_cutoff_hz);
+	ClassDB::bind_method(D_METHOD("get_attenuation_filter_cutoff_hz"), &AudioStreamPlayer3D::get_attenuation_filter_cutoff_hz);
+
+	ClassDB::bind_method(D_METHOD("set_attenuation_filter_db", "db"), &AudioStreamPlayer3D::set_attenuation_filter_db);
+	ClassDB::bind_method(D_METHOD("get_attenuation_filter_db"), &AudioStreamPlayer3D::get_attenuation_filter_db);
+
+	ClassDB::bind_method(D_METHOD("set_attenuation_model", "model"), &AudioStreamPlayer3D::set_attenuation_model);
+	ClassDB::bind_method(D_METHOD("get_attenuation_model"), &AudioStreamPlayer3D::get_attenuation_model);
+
+	ClassDB::bind_method(D_METHOD("set_out_of_range_mode", "mode"), &AudioStreamPlayer3D::set_out_of_range_mode);
+	ClassDB::bind_method(D_METHOD("get_out_of_range_mode"), &AudioStreamPlayer3D::get_out_of_range_mode);
+
+	ClassDB::bind_method(D_METHOD("set_doppler_tracking", "mode"), &AudioStreamPlayer3D::set_doppler_tracking);
+	ClassDB::bind_method(D_METHOD("get_doppler_tracking"), &AudioStreamPlayer3D::get_doppler_tracking);
+
+	ClassDB::bind_method(D_METHOD("_bus_layout_changed"), &AudioStreamPlayer3D::_bus_layout_changed);
+
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "stream", PROPERTY_HINT_RESOURCE_TYPE, "AudioStream"), "set_stream", "get_stream");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "attenuation_model", PROPERTY_HINT_ENUM, "Inverse,InverseSquare,Log"), "set_attenuation_model", "get_attenuation_model");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "unit_db", PROPERTY_HINT_RANGE, "-80,80"), "set_unit_db", "get_unit_db");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "unit_size", PROPERTY_HINT_RANGE, "0.1,100,0.1"), "set_unit_size", "get_unit_size");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_db", PROPERTY_HINT_RANGE, "-24,6"), "set_max_db", "get_max_db");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "playing", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_EDITOR), "_set_playing", "_is_active");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "autoplay"), "set_autoplay", "is_autoplay_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_distance", PROPERTY_HINT_RANGE, "0,65536,1"), "set_max_distance", "get_max_distance");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "out_of_range_mode", PROPERTY_HINT_ENUM, "Mix,Pause"), "set_out_of_range_mode", "get_out_of_range_mode");
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "bus", PROPERTY_HINT_ENUM, ""), "set_bus", "get_bus");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "area_mask", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_area_mask", "get_area_mask");
+	ADD_GROUP("Emission Angle", "emission_angle");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "emission_angle_enabled"), "set_emission_angle_enabled", "is_emission_angle_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "emission_angle_degrees", PROPERTY_HINT_RANGE, "0.1,90,0.1"), "set_emission_angle", "get_emission_angle");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "emission_angle_filter_attenuation_db", PROPERTY_HINT_RANGE, "-80,0,0.1"), "set_emission_angle_filter_attenuation_db", "get_emission_angle_filter_attenuation_db");
+	ADD_GROUP("Attenuation Filter", "attenuation_filter_");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "attenuation_filter_cutoff_hz", PROPERTY_HINT_RANGE, "50,50000,1"), "set_attenuation_filter_cutoff_hz", "get_attenuation_filter_cutoff_hz");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "attenuation_filter_db", PROPERTY_HINT_RANGE, "-80,0,0.1"), "set_attenuation_filter_db", "get_attenuation_filter_db");
+	ADD_GROUP("Doppler", "doppler_");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "doppler_tracking", PROPERTY_HINT_ENUM, "Disabled,Idle,Fixed"), "set_doppler_tracking", "get_doppler_tracking");
+
+	BIND_CONSTANT(ATTENUATION_INVERSE_DISTANCE);
+	BIND_CONSTANT(ATTENUATION_INVERSE_SQUARE_DISTANCE);
+	BIND_CONSTANT(ATTENUATION_LOGARITHMIC);
+
+	BIND_CONSTANT(OUT_OF_RANGE_MIX);
+	BIND_CONSTANT(OUT_OF_RANGE_PAUSE);
+
+	BIND_CONSTANT(DOPPLER_TRACKING_DISABLED);
+	BIND_CONSTANT(DOPPLER_TRACKING_IDLE_STEP);
+	BIND_CONSTANT(DOPPLER_TRACKING_FIXED_STEP);
+}
+
+AudioStreamPlayer3D::AudioStreamPlayer3D() {
+
+	unit_db = 0;
+	unit_size = 1;
+	attenuation_model = ATTENUATION_INVERSE_DISTANCE;
+	max_db = 3;
+	autoplay = false;
+	setseek = -1;
+	active = false;
+	output_count = 0;
+	prev_output_count = 0;
+	max_distance = 0;
+	setplay = -1;
+	output_ready = false;
+	area_mask = 1;
+	emission_angle = 45;
+	emission_angle_enabled = false;
+	emission_angle_filter_attenuation_db = -12;
+	attenuation_filter_cutoff_hz = 5000;
+	attenuation_filter_db = -24;
+	out_of_range_mode = OUT_OF_RANGE_MIX;
+	doppler_tracking = DOPPLER_TRACKING_DISABLED;
+
+	velocity_tracker.instance();
+	AudioServer::get_singleton()->connect("bus_layout_changed", this, "_bus_layout_changed");
+}
+AudioStreamPlayer3D::~AudioStreamPlayer3D() {
+}
diff --git a/scene/3d/audio_stream_player_3d.h b/scene/3d/audio_stream_player_3d.h
new file mode 100644
index 0000000000..8603cab5a4
--- /dev/null
+++ b/scene/3d/audio_stream_player_3d.h
@@ -0,0 +1,175 @@
+#ifndef AUDIO_STREAM_PLAYER_3D_H
+#define AUDIO_STREAM_PLAYER_3D_H
+
+#include "scene/3d/spatial.h"
+#include "scene/3d/spatial_velocity_tracker.h"
+#include "servers/audio/audio_filter_sw.h"
+#include "servers/audio/audio_stream.h"
+#include "servers/audio_server.h"
+
+class Camera;
+class AudioStreamPlayer3D : public Spatial {
+
+	GDCLASS(AudioStreamPlayer3D, Spatial)
+public:
+	enum AttenuationModel {
+		ATTENUATION_INVERSE_DISTANCE,
+		ATTENUATION_INVERSE_SQUARE_DISTANCE,
+		ATTENUATION_LOGARITHMIC,
+	};
+
+	enum OutOfRangeMode {
+		OUT_OF_RANGE_MIX,
+		OUT_OF_RANGE_PAUSE,
+	};
+
+	enum DopplerTracking {
+		DOPPLER_TRACKING_DISABLED,
+		DOPPLER_TRACKING_IDLE_STEP,
+		DOPPLER_TRACKING_FIXED_STEP
+	};
+
+private:
+	enum {
+		MAX_OUTPUTS = 8,
+		MAX_INTERSECT_AREAS = 32
+
+	};
+
+	struct Output {
+
+		AudioFilterSW filter;
+		AudioFilterSW::Processor filter_process[6];
+		AudioFrame vol[3];
+		float filter_gain;
+		float pitch_scale;
+		int bus_index;
+		int reverb_bus_index;
+		AudioFrame reverb_vol[3];
+		Viewport *viewport; //pointer only used for reference to previous mix
+
+		Output() {
+			filter_gain = 0;
+			viewport = NULL;
+			reverb_bus_index = -1;
+			bus_index = -1;
+		}
+	};
+
+	Output outputs[MAX_OUTPUTS];
+	volatile int output_count;
+	volatile bool output_ready;
+
+	//these are used by audio thread to have a reference of previous volumes (for ramping volume and avoiding clicks)
+	Output prev_outputs[MAX_OUTPUTS];
+	int prev_output_count;
+
+	Ref<AudioStreamPlayback> stream_playback;
+	Ref<AudioStream> stream;
+	Vector<AudioFrame> mix_buffer;
+
+	volatile float setseek;
+	volatile bool active;
+	volatile float setplay;
+
+	AttenuationModel attenuation_model;
+	float unit_db;
+	float unit_size;
+	float max_db;
+	bool autoplay;
+	StringName bus;
+
+	void _mix_audio();
+	static void _mix_audios(void *self) { reinterpret_cast<AudioStreamPlayer3D *>(self)->_mix_audio(); }
+
+	void _set_playing(bool p_enable);
+	bool _is_active() const;
+
+	void _bus_layout_changed();
+
+	uint32_t area_mask;
+
+	bool emission_angle_enabled;
+	float emission_angle;
+	float emission_angle_filter_attenuation_db;
+	float attenuation_filter_cutoff_hz;
+	float attenuation_filter_db;
+
+	float max_distance;
+
+	Ref<SpatialVelocityTracker> velocity_tracker;
+
+	DopplerTracking doppler_tracking;
+
+	OutOfRangeMode out_of_range_mode;
+
+	float _get_attenuation_db(float p_distance) const;
+
+protected:
+	void _validate_property(PropertyInfo &property) const;
+	void _notification(int p_what);
+	static void _bind_methods();
+
+public:
+	void set_stream(Ref<AudioStream> p_stream);
+	Ref<AudioStream> get_stream() const;
+
+	void set_unit_db(float p_volume);
+	float get_unit_db() const;
+
+	void set_unit_size(float p_volume);
+	float get_unit_size() const;
+
+	void set_max_db(float p_boost);
+	float get_max_db() const;
+
+	void play(float p_from_pos = 0.0);
+	void seek(float p_seconds);
+	void stop();
+	bool is_playing() const;
+	float get_pos();
+
+	void set_bus(const StringName &p_bus);
+	StringName get_bus() const;
+
+	void set_autoplay(bool p_enable);
+	bool is_autoplay_enabled();
+
+	void set_max_distance(float p_metres);
+	float get_max_distance() const;
+
+	void set_area_mask(uint32_t p_mask);
+	uint32_t get_area_mask() const;
+
+	void set_emission_angle_enabled(bool p_enable);
+	bool is_emission_angle_enabled() const;
+
+	void set_emission_angle(float p_angle);
+	float get_emission_angle() const;
+
+	void set_emission_angle_filter_attenuation_db(float p_angle_attenuation_db);
+	float get_emission_angle_filter_attenuation_db() const;
+
+	void set_attenuation_filter_cutoff_hz(float p_hz);
+	float get_attenuation_filter_cutoff_hz() const;
+
+	void set_attenuation_filter_db(float p_db);
+	float get_attenuation_filter_db() const;
+
+	void set_attenuation_model(AttenuationModel p_model);
+	AttenuationModel get_attenuation_model() const;
+
+	void set_out_of_range_mode(OutOfRangeMode p_mode);
+	OutOfRangeMode get_out_of_range_mode() const;
+
+	void set_doppler_tracking(DopplerTracking p_tracking);
+	DopplerTracking get_doppler_tracking() const;
+
+	AudioStreamPlayer3D();
+	~AudioStreamPlayer3D();
+};
+
+VARIANT_ENUM_CAST(AudioStreamPlayer3D::AttenuationModel)
+VARIANT_ENUM_CAST(AudioStreamPlayer3D::OutOfRangeMode)
+VARIANT_ENUM_CAST(AudioStreamPlayer3D::DopplerTracking)
+#endif // AUDIO_STREAM_PLAYER_3D_H
diff --git a/scene/3d/body_shape.cpp b/scene/3d/body_shape.cpp
deleted file mode 100644
index 68f166c5b9..0000000000
--- a/scene/3d/body_shape.cpp
+++ /dev/null
@@ -1,920 +0,0 @@
-/*************************************************************************/
-/*  body_shape.cpp                                                       */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                    http://www.godotengine.org                         */
-/*************************************************************************/
-/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
-/*                                                                       */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the       */
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
-/*************************************************************************/
-#include "body_shape.h"
-#include "scene/resources/box_shape.h"
-#include "scene/resources/capsule_shape.h"
-#include "scene/resources/concave_polygon_shape.h"
-#include "scene/resources/convex_polygon_shape.h"
-#include "scene/resources/plane_shape.h"
-#include "scene/resources/ray_shape.h"
-#include "scene/resources/sphere_shape.h"
-#include "servers/visual_server.h"
-//TODO: Implement CylinderShape and HeightMapShape?
-#include "mesh_instance.h"
-#include "physics_body.h"
-#include "quick_hull.h"
-
-void CollisionShape::_update_body() {
-
-	if (!is_inside_tree() || !can_update_body)
-		return;
-	if (!get_tree()->is_editor_hint())
-		return;
-	if (get_parent() && get_parent()->cast_to<CollisionObject>())
-		get_parent()->cast_to<CollisionObject>()->_update_shapes_from_children();
-}
-
-void CollisionShape::make_convex_from_brothers() {
-
-	Node *p = get_parent();
-	if (!p)
-		return;
-
-	for (int i = 0; i < p->get_child_count(); i++) {
-
-		Node *n = p->get_child(i);
-		if (n->cast_to<MeshInstance>()) {
-
-			MeshInstance *mi = n->cast_to<MeshInstance>();
-			Ref<Mesh> m = mi->get_mesh();
-			if (m.is_valid()) {
-
-				Ref<Shape> s = m->create_convex_shape();
-				set_shape(s);
-			}
-		}
-	}
-}
-/*
-
-void CollisionShape::_update_indicator() {
-
-	while (VisualServer::get_singleton()->mesh_get_surface_count(indicator))
-		VisualServer::get_singleton()->mesh_remove_surface(indicator,0);
-
-	if (shape.is_null())
-		return;
-
-	PoolVector<Vector3> points;
-	PoolVector<Vector3> normals;
-
-	VS::PrimitiveType pt = VS::PRIMITIVE_TRIANGLES;
-
-	if (shape->cast_to<RayShape>()) {
-
-		RayShape *rs = shape->cast_to<RayShape>();
-		points.push_back(Vector3());
-		points.push_back(Vector3(0,0,rs->get_length()));
-		pt = VS::PRIMITIVE_LINES;
-	} else if (shape->cast_to<SphereShape>()) {
-
-		//VisualServer *vs=VisualServer::get_singleton();
-		SphereShape *shapeptr=shape->cast_to<SphereShape>();
-
-
-		Color col(0.4,1.0,1.0,0.5);
-
-		int lats=6;
-		int lons=12;
-		float size=shapeptr->get_radius();
-
-
-		for(int i = 1; i <= lats; i++) {
-			double lat0 = Math_PI * (-0.5 + (double) (i - 1) / lats);
-			double z0  = Math::sin(lat0);
-			double zr0 =  Math::cos(lat0);
-
-			double lat1 = Math_PI * (-0.5 + (double) i / lats);
-			double z1 = Math::sin(lat1);
-			double zr1 = Math::cos(lat1);
-
-			for(int j = lons; j >= 1; j--) {
-
-				double lng0 = 2 * Math_PI * (double) (j - 1) / lons;
-				double x0 = Math::cos(lng0);
-				double y0 = Math::sin(lng0);
-
-				double lng1 = 2 * Math_PI * (double) (j) / lons;
-				double x1 = Math::cos(lng1);
-				double y1 = Math::sin(lng1);
-
-				Vector3 v4=Vector3(x0 * zr0, z0, y0 *zr0)*size;
-				Vector3 v3=Vector3(x0 * zr1, z1, y0 *zr1)*size;
-				Vector3 v2=Vector3(x1 * zr1, z1, y1 *zr1)*size;
-				Vector3 v1=Vector3(x1 * zr0, z0, y1 *zr0)*size;
-
-				Vector<Vector3> line;
-				line.push_back(v1);
-				line.push_back(v2);
-				line.push_back(v3);
-				line.push_back(v4);
-
-
-				points.push_back(v1);
-				points.push_back(v2);
-				points.push_back(v3);
-
-				points.push_back(v1);
-				points.push_back(v3);
-				points.push_back(v4);
-
-				normals.push_back(v1.normalized());
-				normals.push_back(v2.normalized());
-				normals.push_back(v3.normalized());
-
-				normals.push_back(v1.normalized());
-				normals.push_back(v3.normalized());
-				normals.push_back(v4.normalized());
-
-			}
-		}
-	} else if (shape->cast_to<BoxShape>()) {
-
-		BoxShape *shapeptr=shape->cast_to<BoxShape>();
-
-		for (int i=0;i<6;i++) {
-
-
-			Vector3 face_points[4];
-
-
-			for (int j=0;j<4;j++) {
-
-				float v[3];
-				v[0]=1.0;
-				v[1]=1-2*((j>>1)&1);
-				v[2]=v[1]*(1-2*(j&1));
-
-				for (int k=0;k<3;k++) {
-
-					if (i<3)
-						face_points[j][(i+k)%3]=v[k]*(i>=3?-1:1);
-					else
-						face_points[3-j][(i+k)%3]=v[k]*(i>=3?-1:1);
-				}
-			}
-			Vector3 normal;
-			normal[i%3]=(i>=3?-1:1);
-
-			for(int j=0;j<4;j++)
-				face_points[j]*=shapeptr->get_extents();
-
-			points.push_back(face_points[0]);
-			points.push_back(face_points[1]);
-			points.push_back(face_points[2]);
-
-			points.push_back(face_points[0]);
-			points.push_back(face_points[2]);
-			points.push_back(face_points[3]);
-
-			for(int n=0;n<6;n++)
-				normals.push_back(normal);
-
-		}
-
-	} else if (shape->cast_to<ConvexPolygonShape>()) {
-
-		ConvexPolygonShape *shapeptr=shape->cast_to<ConvexPolygonShape>();
-
-		Geometry::MeshData md;
-		QuickHull::build(Variant(shapeptr->get_points()),md);
-
-		for(int i=0;i<md.faces.size();i++) {
-
-			for(int j=2;j<md.faces[i].indices.size();j++) {
-				points.push_back(md.vertices[md.faces[i].indices[0]]);
-				points.push_back(md.vertices[md.faces[i].indices[j-1]]);
-				points.push_back(md.vertices[md.faces[i].indices[j]]);
-				normals.push_back(md.faces[i].plane.normal);
-				normals.push_back(md.faces[i].plane.normal);
-				normals.push_back(md.faces[i].plane.normal);
-			}
-		}
-	} else if (shape->cast_to<ConcavePolygonShape>()) {
-
-		ConcavePolygonShape *shapeptr=shape->cast_to<ConcavePolygonShape>();
-
-		points = shapeptr->get_faces();
-		for(int i=0;i<points.size()/3;i++) {
-
-			Vector3 n = Plane( points[i*3+0],points[i*3+1],points[i*3+2] ).normal;
-			normals.push_back(n);
-			normals.push_back(n);
-			normals.push_back(n);
-		}
-
-	} else if (shape->cast_to<CapsuleShape>()) {
-
-		CapsuleShape *shapeptr=shape->cast_to<CapsuleShape>();
-
-		PoolVector<Plane> planes = Geometry::build_capsule_planes(shapeptr->get_radius(), shapeptr->get_height()/2.0, 12, Vector3::AXIS_Z);
-		Geometry::MeshData md = Geometry::build_convex_mesh(planes);
-
-		for(int i=0;i<md.faces.size();i++) {
-
-			for(int j=2;j<md.faces[i].indices.size();j++) {
-				points.push_back(md.vertices[md.faces[i].indices[0]]);
-				points.push_back(md.vertices[md.faces[i].indices[j-1]]);
-				points.push_back(md.vertices[md.faces[i].indices[j]]);
-				normals.push_back(md.faces[i].plane.normal);
-				normals.push_back(md.faces[i].plane.normal);
-				normals.push_back(md.faces[i].plane.normal);
-
-			}
-		}
-
-	} else if (shape->cast_to<PlaneShape>()) {
-
-		PlaneShape *shapeptr=shape->cast_to<PlaneShape>();
-
-		Plane p = shapeptr->get_plane();
-		Vector3 n1 = p.get_any_perpendicular_normal();
-		Vector3 n2 = p.normal.cross(n1).normalized();
-
-		Vector3 pface[4]={
-			p.normal*p.d+n1*100.0+n2*100.0,
-			p.normal*p.d+n1*100.0+n2*-100.0,
-			p.normal*p.d+n1*-100.0+n2*-100.0,
-			p.normal*p.d+n1*-100.0+n2*100.0,
-		};
-
-		points.push_back(pface[0]);
-		points.push_back(pface[1]);
-		points.push_back(pface[2]);
-
-		points.push_back(pface[0]);
-		points.push_back(pface[2]);
-		points.push_back(pface[3]);
-
-		normals.push_back(p.normal);
-		normals.push_back(p.normal);
-		normals.push_back(p.normal);
-		normals.push_back(p.normal);
-		normals.push_back(p.normal);
-		normals.push_back(p.normal);
-
-	}
-
-	if (!points.size())
-		return;
-	RID material = VisualServer::get_singleton()->fixed_material_create();
-	VisualServer::get_singleton()->fixed_material_set_param(material,VS::FIXED_MATERIAL_PARAM_DIFFUSE,Color(0,0.6,0.7,0.3));
-	VisualServer::get_singleton()->fixed_material_set_param(material,VS::FIXED_MATERIAL_PARAM_EMISSION,0.7);
-	if (normals.size()==0)
-		VisualServer::get_singleton()->material_set_flag(material,VS::MATERIAL_FLAG_UNSHADED,true);
-	VisualServer::get_singleton()->material_set_flag(material,VS::MATERIAL_FLAG_DOUBLE_SIDED,true);
-	Array d;
-	d.resize(VS::ARRAY_MAX);
-	d[VS::ARRAY_VERTEX]=points;
-	if (normals.size())
-		d[VS::ARRAY_NORMAL]=normals;
-	VisualServer::get_singleton()->mesh_add_surface(indicator,pt,d);
-	VisualServer::get_singleton()->mesh_surface_set_material(indicator,0,material,true);
-
-}
-
-*/
-void CollisionShape::_add_to_collision_object(Object *p_cshape) {
-
-	if (unparenting)
-		return;
-
-	CollisionObject *co = p_cshape->cast_to<CollisionObject>();
-	ERR_FAIL_COND(!co);
-
-	if (shape.is_valid()) {
-
-		update_shape_index = co->get_shape_count();
-		co->add_shape(shape, get_transform());
-		if (trigger)
-			co->set_shape_as_trigger(co->get_shape_count() - 1, true);
-	} else {
-		update_shape_index = -1;
-	}
-}
-
-void CollisionShape::_notification(int p_what) {
-
-	switch (p_what) {
-
-		case NOTIFICATION_ENTER_TREE: {
-			unparenting = false;
-			can_update_body = get_tree()->is_editor_hint();
-			set_notify_local_transform(!can_update_body);
-
-			if (get_tree()->is_debugging_collisions_hint()) {
-				_create_debug_shape();
-			}
-
-			//indicator_instance = VisualServer::get_singleton()->instance_create2(indicator,get_world()->get_scenario());
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
-			//VisualServer::get_singleton()->instance_set_transform(indicator_instance,get_global_transform());
-			if (can_update_body && updating_body) {
-				_update_body();
-			}
-		} break;
-		case NOTIFICATION_EXIT_TREE: {
-			/*	if (indicator_instance.is_valid()) {
-				VisualServer::get_singleton()->free(indicator_instance);
-				indicator_instance=RID();
-			}*/
-			can_update_body = false;
-			set_notify_local_transform(false);
-			if (debug_shape) {
-				debug_shape->queue_delete();
-				debug_shape = NULL;
-			}
-		} break;
-		case NOTIFICATION_UNPARENTED: {
-			unparenting = true;
-			if (can_update_body && updating_body)
-				_update_body();
-		} break;
-		case NOTIFICATION_PARENTED: {
-			if (can_update_body && updating_body)
-				_update_body();
-		} break;
-		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
-
-			if (!can_update_body && update_shape_index >= 0) {
-
-				CollisionObject *co = get_parent()->cast_to<CollisionObject>();
-				if (co) {
-					co->set_shape_transform(update_shape_index, get_transform());
-				}
-			}
-
-		} break;
-	}
-}
-
-void CollisionShape::resource_changed(RES res) {
-
-	update_gizmo();
-}
-
-void CollisionShape::_set_update_shape_index(int p_index) {
-
-	update_shape_index = p_index;
-}
-
-int CollisionShape::_get_update_shape_index() const {
-
-	return update_shape_index;
-}
-
-String CollisionShape::get_configuration_warning() const {
-
-	if (!get_parent()->cast_to<CollisionObject>()) {
-		return TTR("CollisionShape only serves to provide a collision shape to a CollisionObject derived node. Please only use it as a child of Area, StaticBody, RigidBody, KinematicBody, etc. to give them a shape.");
-	}
-
-	if (!shape.is_valid()) {
-		return TTR("A shape must be provided for CollisionShape to function. Please create a shape resource for it!");
-	}
-
-	return String();
-}
-
-void CollisionShape::_bind_methods() {
-
-	//not sure if this should do anything
-	ClassDB::bind_method(D_METHOD("resource_changed", "resource"), &CollisionShape::resource_changed);
-	ClassDB::bind_method(D_METHOD("set_shape", "shape"), &CollisionShape::set_shape);
-	ClassDB::bind_method(D_METHOD("get_shape"), &CollisionShape::get_shape);
-	ClassDB::bind_method(D_METHOD("_add_to_collision_object"), &CollisionShape::_add_to_collision_object);
-	ClassDB::bind_method(D_METHOD("set_trigger", "enable"), &CollisionShape::set_trigger);
-	ClassDB::bind_method(D_METHOD("is_trigger"), &CollisionShape::is_trigger);
-	ClassDB::bind_method(D_METHOD("make_convex_from_brothers"), &CollisionShape::make_convex_from_brothers);
-	ClassDB::set_method_flags("CollisionShape", "make_convex_from_brothers", METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR);
-	ClassDB::bind_method(D_METHOD("_set_update_shape_index", "index"), &CollisionShape::_set_update_shape_index);
-	ClassDB::bind_method(D_METHOD("_get_update_shape_index"), &CollisionShape::_get_update_shape_index);
-
-	ClassDB::bind_method(D_METHOD("get_collision_object_shape_index"), &CollisionShape::get_collision_object_shape_index);
-
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape"), "set_shape", "get_shape");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "trigger"), "set_trigger", "is_trigger");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "_update_shape_index", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "_set_update_shape_index", "_get_update_shape_index");
-}
-
-void CollisionShape::set_shape(const Ref<Shape> &p_shape) {
-
-	if (!shape.is_null())
-		shape->unregister_owner(this);
-	shape = p_shape;
-	if (!shape.is_null())
-		shape->register_owner(this);
-	update_gizmo();
-	if (updating_body) {
-		_update_body();
-	} else if (can_update_body && update_shape_index >= 0 && is_inside_tree()) {
-		CollisionObject *co = get_parent()->cast_to<CollisionObject>();
-		if (co) {
-			co->set_shape(update_shape_index, p_shape);
-		}
-	}
-}
-
-Ref<Shape> CollisionShape::get_shape() const {
-
-	return shape;
-}
-
-void CollisionShape::set_updating_body(bool p_update) {
-	updating_body = p_update;
-}
-
-bool CollisionShape::is_updating_body() const {
-
-	return updating_body;
-}
-
-void CollisionShape::set_trigger(bool p_trigger) {
-
-	trigger = p_trigger;
-	if (updating_body) {
-		_update_body();
-	} else if (can_update_body && update_shape_index >= 0 && is_inside_tree()) {
-		CollisionObject *co = get_parent()->cast_to<CollisionObject>();
-		if (co) {
-			co->set_shape_as_trigger(update_shape_index, p_trigger);
-		}
-	}
-}
-
-bool CollisionShape::is_trigger() const {
-
-	return trigger;
-}
-
-CollisionShape::CollisionShape() {
-
-	//indicator = VisualServer::get_singleton()->mesh_create();
-	updating_body = true;
-	unparenting = false;
-	update_shape_index = -1;
-	trigger = false;
-	can_update_body = false;
-	debug_shape = NULL;
-}
-
-CollisionShape::~CollisionShape() {
-	if (!shape.is_null())
-		shape->unregister_owner(this);
-	//VisualServer::get_singleton()->free(indicator);
-}
-
-void CollisionShape::_create_debug_shape() {
-
-	if (debug_shape) {
-		debug_shape->queue_delete();
-		debug_shape = NULL;
-	}
-
-	Ref<Shape> s = get_shape();
-
-	if (s.is_null())
-		return;
-
-	Ref<Mesh> mesh = s->get_debug_mesh();
-
-	MeshInstance *mi = memnew(MeshInstance);
-	mi->set_mesh(mesh);
-
-	add_child(mi);
-	debug_shape = mi;
-}
-
-#if 0
-#include "body_volume.h"
-
-#include "geometry.h"
-#include "scene/3d/physics_body.h"
-
-#define ADD_TRIANGLE(m_a, m_b, m_c, m_color)                                                       \
-	{                                                                                              \
-		Vector<Vector3> points;                                                                    \
-		points.resize(3);                                                                          \
-		points[0] = m_a;                                                                           \
-		points[1] = m_b;                                                                           \
-		points[2] = m_c;                                                                           \
-		Vector<Color> colors;                                                                      \
-		colors.resize(3);                                                                          \
-		colors[0] = m_color;                                                                       \
-		colors[1] = m_color;                                                                       \
-		colors[2] = m_color;                                                                       \
-		vs->poly_add_primitive(p_indicator, points, Vector<Vector3>(), colors, Vector<Vector3>()); \
-	}
-
-
-void CollisionShape::_notification(int p_what) {
-
-	switch (p_what) {
-		case NOTIFICATION_ENTER_SCENE: {
-
-
-			if (get_root_node()->get_editor() && !indicator.is_valid()) {
-
-				indicator=VisualServer::get_singleton()->poly_create();
-				RID mat=VisualServer::get_singleton()->fixed_material_create();
-				VisualServer::get_singleton()->material_set_flag( mat, VisualServer::MATERIAL_FLAG_UNSHADED, true );
-				VisualServer::get_singleton()->material_set_flag( mat, VisualServer::MATERIAL_FLAG_WIREFRAME, true );
-				VisualServer::get_singleton()->material_set_flag( mat, VisualServer::MATERIAL_FLAG_DOUBLE_SIDED, true );
-				VisualServer::get_singleton()->material_set_line_width( mat, 3 );
-
-				VisualServer::get_singleton()->poly_set_material(indicator,mat,true);
-
-				update_indicator(indicator);
-			}
-
-			if (indicator.is_valid()) {
-
-				indicator_instance=VisualServer::get_singleton()->instance_create2(indicator,get_world()->get_scenario());
-				VisualServer::get_singleton()->instance_attach_object_instance_ID(indicator_instance,get_instance_ID());
-			}
-			volume_changed();
-		} break;
-		case NOTIFICATION_EXIT_SCENE: {
-
-			if (indicator_instance.is_valid()) {
-
-				VisualServer::get_singleton()->free(indicator_instance);
-			}
-			volume_changed();
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
-
-			if (indicator_instance.is_valid()) {
-
-				VisualServer::get_singleton()->instance_set_transform(indicator_instance,get_global_transform());
-			}
-			volume_changed();
-		} break;
-		default: {}
-	}
-}
-
-void CollisionShape::volume_changed() {
-
-	if (indicator.is_valid())
-		update_indicator(indicator);
-
-	Object *parent=get_parent();
-	if (!parent)
-		return;
-	PhysicsBody *physics_body=parent->cast_to<PhysicsBody>();
-
-	ERR_EXPLAIN("CollisionShape parent is not of type PhysicsBody");
-	ERR_FAIL_COND(!physics_body);
-
-	physics_body->recompute_child_volumes();
-
-}
-
-RID CollisionShape::_get_visual_instance_rid() const {
-
-	return indicator_instance;
-
-}
-
-void CollisionShape::_bind_methods() {
-
-	ClassDB::bind_method("_get_visual_instance_rid",&CollisionShape::_get_visual_instance_rid);
-}
-
-CollisionShape::CollisionShape() {
-
-}
-
-CollisionShape::~CollisionShape() {
-
-	if (indicator.is_valid()) {
-
-		VisualServer::get_singleton()->free(indicator);
-	}
-
-}
-
-void CollisionShapeSphere::_set(const String& p_name, const Variant& p_value) {
-
-	if (p_name=="radius") {
-		radius=p_value;
-		volume_changed();
-	}
-
-}
-
-Variant CollisionShapeSphere::_get(const String& p_name) const {
-
-	if (p_name=="radius") {
-		return radius;
-	}
-
-	return Variant();
-}
-
-void CollisionShapeSphere::_get_property_list( List<PropertyInfo> *p_list) const {
-
-	p_list->push_back( PropertyInfo(Variant::REAL,"radius",PROPERTY_HINT_RANGE,"0.01,16384,0.01") );
-}
-
-void CollisionShapeSphere::update_indicator(RID p_indicator) {
-
-	VisualServer *vs=VisualServer::get_singleton();
-
-	vs->poly_clear(p_indicator);
-	Color col(0.4,1.0,1.0,0.5);
-
-	int lats=6;
-	int lons=12;
-	float size=radius;
-
-	for(int i = 1; i <= lats; i++) {
-		double lat0 = Math_PI * (-0.5 + (double) (i - 1) / lats);
-		double z0  = Math::sin(lat0);
-		double zr0 =  Math::cos(lat0);
-
-		double lat1 = Math_PI * (-0.5 + (double) i / lats);
-		double z1 = Math::sin(lat1);
-		double zr1 = Math::cos(lat1);
-
-		for(int j = lons; j >= 1; j--) {
-
-			double lng0 = 2 * Math_PI * (double) (j - 1) / lons;
-			double x0 = Math::cos(lng0);
-			double y0 = Math::sin(lng0);
-
-			double lng1 = 2 * Math_PI * (double) (j) / lons;
-			double x1 = Math::cos(lng1);
-			double y1 = Math::sin(lng1);
-
-			Vector3 v4=Vector3(x0 * zr0, z0, y0 *zr0)*size;
-			Vector3 v3=Vector3(x0 * zr1, z1, y0 *zr1)*size;
-			Vector3 v2=Vector3(x1 * zr1, z1, y1 *zr1)*size;
-			Vector3 v1=Vector3(x1 * zr0, z0, y1 *zr0)*size;
-
-			Vector<Vector3> line;
-			line.push_back(v1);
-			line.push_back(v2);
-			line.push_back(v3);
-			line.push_back(v4);
-
-			Vector<Color> cols;
-			cols.push_back(col);
-			cols.push_back(col);
-			cols.push_back(col);
-			cols.push_back(col);
-
-
-			VisualServer::get_singleton()->poly_add_primitive(p_indicator,line,Vector<Vector3>(),cols,Vector<Vector3>());
-		}
-	}
-}
-
-void CollisionShapeSphere::append_to_volume(Ref<Shape> p_volume) {
-
-	p_volume->add_sphere_shape(radius,get_transform());
-}
-
-
-CollisionShapeSphere::CollisionShapeSphere() {
-
-	radius=1.0;
-}
-
-/* BOX */
-
-
-void CollisionShapeBox::_set(const String& p_name, const Variant& p_value) {
-
-	if (p_name=="half_extents") {
-		half_extents=p_value;
-		volume_changed();
-	}
-
-}
-
-Variant CollisionShapeBox::_get(const String& p_name) const {
-
-	if (p_name=="half_extents") {
-		return half_extents;
-	}
-
-	return Variant();
-}
-
-void CollisionShapeBox::_get_property_list( List<PropertyInfo> *p_list) const {
-
-	p_list->push_back( PropertyInfo(Variant::VECTOR3,"half_extents" ) );
-}
-
-
-void CollisionShapeBox::update_indicator(RID p_indicator) {
-
-	VisualServer *vs=VisualServer::get_singleton();
-
-	vs->poly_clear(p_indicator);
-	Color col(0.4,1.0,1.0,0.5);
-
-
-	for (int i=0;i<6;i++) {
-
-
-		Vector3 face_points[4];
-
-		for (int j=0;j<4;j++) {
-
-			float v[3];
-			v[0]=1.0;
-			v[1]=1-2*((j>>1)&1);
-			v[2]=v[1]*(1-2*(j&1));
-
-			for (int k=0;k<3;k++) {
-
-				if (i<3)
-					face_points[j][(i+k)%3]=v[k]*(i>=3?-1:1);
-				else
-					face_points[3-j][(i+k)%3]=v[k]*(i>=3?-1:1);
-			}
-		}
-
-		for(int j=0;j<4;j++)
-			face_points[i]*=half_extents;
-
-		ADD_TRIANGLE(face_points[0],face_points[1],face_points[2],col);
-		ADD_TRIANGLE(face_points[2],face_points[3],face_points[0],col);
-
-	}
-}
-
-void CollisionShapeBox::append_to_volume(Ref<Shape> p_volume) {
-
-	p_volume->add_box_shape(half_extents,get_transform());
-}
-
-
-CollisionShapeBox::CollisionShapeBox() {
-
-	half_extents=Vector3(1,1,1);
-}
-
-/* CYLINDER */
-
-
-void CollisionShapeCylinder::_set(const String& p_name, const Variant& p_value) {
-
-	if (p_name=="radius") {
-		radius=p_value;
-		volume_changed();
-	}
-	if (p_name=="height") {
-		height=p_value;
-		volume_changed();
-	}
-
-}
-
-Variant CollisionShapeCylinder::_get(const String& p_name) const {
-
-	if (p_name=="radius") {
-		return radius;
-	}
-	if (p_name=="height") {
-		return height;
-	}
-	return Variant();
-}
-
-void CollisionShapeCylinder::_get_property_list( List<PropertyInfo> *p_list) const {
-
-	p_list->push_back( PropertyInfo(Variant::REAL,"radius",PROPERTY_HINT_RANGE,"0.01,16384,0.01") );
-	p_list->push_back( PropertyInfo(Variant::REAL,"height",PROPERTY_HINT_RANGE,"0.01,16384,0.01") );
-}
-
-
-void CollisionShapeCylinder::update_indicator(RID p_indicator) {
-
-	VisualServer *vs=VisualServer::get_singleton();
-
-	vs->poly_clear(p_indicator);
-	Color col(0.4,1.0,1.0,0.5);
-
-	PoolVector<Plane> planes = Geometry::build_cylinder_planes(radius, height, 12, Vector3::AXIS_Z);
-	Geometry::MeshData md = Geometry::build_convex_mesh(planes);
-
-	for(int i=0;i<md.faces.size();i++) {
-
-		for(int j=2;j<md.faces[i].indices.size();j++) {
-			ADD_TRIANGLE(md.vertices[md.faces[i].indices[0]],md.vertices[md.faces[i].indices[j-1]],md.vertices[md.faces[i].indices[j]],col);
-		}
-	}
-
-}
-
-void CollisionShapeCylinder::append_to_volume(Ref<Shape> p_volume) {
-
-	p_volume->add_cylinder_shape(radius,height*2.0,get_transform());
-}
-
-
-CollisionShapeCylinder::CollisionShapeCylinder() {
-
-	height=1;
-	radius=1;
-}
-
-/* CAPSULE */
-
-
-void CollisionShapeCapsule::_set(const String& p_name, const Variant& p_value) {
-
-	if (p_name=="radius") {
-		radius=p_value;
-		volume_changed();
-	}
-
-	if (p_name=="height") {
-		height=p_value;
-		volume_changed();
-	}
-
-}
-
-Variant CollisionShapeCapsule::_get(const String& p_name) const {
-
-	if (p_name=="radius") {
-		return radius;
-	}
-	if (p_name=="height") {
-		return height;
-	}
-	return Variant();
-}
-
-void CollisionShapeCapsule::_get_property_list( List<PropertyInfo> *p_list) const {
-
-	p_list->push_back( PropertyInfo(Variant::REAL,"radius",PROPERTY_HINT_RANGE,"0.01,16384,0.01") );
-	p_list->push_back( PropertyInfo(Variant::REAL,"height",PROPERTY_HINT_RANGE,"0.01,16384,0.01") );
-}
-
-
-void CollisionShapeCapsule::update_indicator(RID p_indicator) {
-
-	VisualServer *vs=VisualServer::get_singleton();
-
-	vs->poly_clear(p_indicator);
-	Color col(0.4,1.0,1.0,0.5);
-
-	PoolVector<Plane> planes = Geometry::build_capsule_planes(radius, height, 12, 3, Vector3::AXIS_Z);
-	Geometry::MeshData md = Geometry::build_convex_mesh(planes);
-
-	for(int i=0;i<md.faces.size();i++) {
-
-		for(int j=2;j<md.faces[i].indices.size();j++) {
-			ADD_TRIANGLE(md.vertices[md.faces[i].indices[0]],md.vertices[md.faces[i].indices[j-1]],md.vertices[md.faces[i].indices[j]],col);
-		}
-	}
-
-}
-
-void CollisionShapeCapsule::append_to_volume(Ref<Shape> p_volume) {
-
-
-	p_volume->add_capsule_shape(radius,height,get_transform());
-}
-
-
-CollisionShapeCapsule::CollisionShapeCapsule() {
-
-	height=1;
-	radius=1;
-}
-#endif
diff --git a/scene/3d/camera.cpp b/scene/3d/camera.cpp
index 0f4378acdd..697d91c863 100644
--- a/scene/3d/camera.cpp
+++ b/scene/3d/camera.cpp
@@ -94,6 +94,8 @@ bool Camera::_set(const StringName &p_name, const Variant &p_value) {
 		set_cull_mask(p_value);
 	} else if (p_name == "environment") {
 		set_environment(p_value);
+	} else if (p_name == "doppler/tracking") {
+		set_doppler_tracking(DopplerTracking(int(p_value)));
 	} else
 		return false;
 
@@ -131,6 +133,8 @@ bool Camera::_get(const StringName &p_name, Variant &r_ret) const {
 		r_ret = get_v_offset();
 	} else if (p_name == "environment") {
 		r_ret = get_environment();
+	} else if (p_name == "doppler/tracking") {
+		r_ret = get_doppler_tracking();
 	} else
 		return false;
 
@@ -171,6 +175,7 @@ void Camera::_get_property_list(List<PropertyInfo> *p_list) const {
 	p_list->push_back(PropertyInfo(Variant::OBJECT, "environment", PROPERTY_HINT_RESOURCE_TYPE, "Environment"));
 	p_list->push_back(PropertyInfo(Variant::REAL, "h_offset"));
 	p_list->push_back(PropertyInfo(Variant::REAL, "v_offset"));
+	p_list->push_back(PropertyInfo(Variant::INT, "doppler/tracking", PROPERTY_HINT_ENUM, "Disabled,Idle,Fixed"));
 }
 
 void Camera::_update_camera() {
@@ -209,6 +214,9 @@ void Camera::_notification(int p_what) {
 		case NOTIFICATION_TRANSFORM_CHANGED: {
 
 			_request_camera_update();
+			if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+				velocity_tracker->update_position(get_global_transform().origin);
+			}
 		} break;
 		case NOTIFICATION_EXIT_WORLD: {
 
@@ -507,6 +515,22 @@ Camera::KeepAspect Camera::get_keep_aspect_mode() const {
 	return keep_aspect;
 }
 
+void Camera::set_doppler_tracking(DopplerTracking p_tracking) {
+
+	if (doppler_tracking == p_tracking)
+		return;
+
+	doppler_tracking = p_tracking;
+	if (p_tracking != DOPPLER_TRACKING_DISABLED) {
+		velocity_tracker->set_track_fixed_step(doppler_tracking == DOPPLER_TRACKING_FIXED_STEP);
+		velocity_tracker->reset(get_global_transform().origin);
+	}
+}
+
+Camera::DopplerTracking Camera::get_doppler_tracking() const {
+	return doppler_tracking;
+}
+
 void Camera::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("project_ray_normal", "screen_point"), &Camera::project_ray_normal);
@@ -536,6 +560,8 @@ void Camera::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_environment:Environment"), &Camera::get_environment);
 	ClassDB::bind_method(D_METHOD("set_keep_aspect_mode", "mode"), &Camera::set_keep_aspect_mode);
 	ClassDB::bind_method(D_METHOD("get_keep_aspect_mode"), &Camera::get_keep_aspect_mode);
+	ClassDB::bind_method(D_METHOD("set_doppler_tracking", "mode"), &Camera::set_doppler_tracking);
+	ClassDB::bind_method(D_METHOD("get_doppler_tracking"), &Camera::get_doppler_tracking);
 	//ClassDB::bind_method(D_METHOD("_camera_make_current"),&Camera::_camera_make_current );
 
 	BIND_CONSTANT(PROJECTION_PERSPECTIVE);
@@ -543,6 +569,10 @@ void Camera::_bind_methods() {
 
 	BIND_CONSTANT(KEEP_WIDTH);
 	BIND_CONSTANT(KEEP_HEIGHT);
+
+	BIND_CONSTANT(DOPPLER_TRACKING_DISABLED)
+	BIND_CONSTANT(DOPPLER_TRACKING_IDLE_STEP)
+	BIND_CONSTANT(DOPPLER_TRACKING_FIXED_STEP)
 }
 
 float Camera::get_fov() const {
@@ -616,6 +646,14 @@ float Camera::get_h_offset() const {
 	return h_offset;
 }
 
+Vector3 Camera::get_doppler_tracked_velocity() const {
+
+	if (doppler_tracking != DOPPLER_TRACKING_DISABLED) {
+		return velocity_tracker->get_tracked_linear_velocity();
+	} else {
+		return Vector3();
+	}
+}
 Camera::Camera() {
 
 	camera = VisualServer::get_singleton()->camera_create();
@@ -633,6 +671,8 @@ Camera::Camera() {
 	h_offset = 0;
 	VisualServer::get_singleton()->camera_set_cull_mask(camera, layers);
 	//active=false;
+	velocity_tracker.instance();
+	doppler_tracking = DOPPLER_TRACKING_DISABLED;
 	set_notify_transform(true);
 }
 
diff --git a/scene/3d/camera.h b/scene/3d/camera.h
index 472cfaa008..43975892b4 100644
--- a/scene/3d/camera.h
+++ b/scene/3d/camera.h
@@ -31,6 +31,7 @@
 #define CAMERA_H
 
 #include "scene/3d/spatial.h"
+#include "scene/3d/spatial_velocity_tracker.h"
 #include "scene/main/viewport.h"
 #include "scene/resources/environment.h"
 /**
@@ -52,6 +53,12 @@ public:
 		KEEP_HEIGHT
 	};
 
+	enum DopplerTracking {
+		DOPPLER_TRACKING_DISABLED,
+		DOPPLER_TRACKING_IDLE_STEP,
+		DOPPLER_TRACKING_FIXED_STEP
+	};
+
 private:
 	bool force_change;
 	bool current;
@@ -80,6 +87,9 @@ private:
 	friend class Viewport;
 	void _update_audio_listener_state();
 
+	DopplerTracking doppler_tracking;
+	Ref<SpatialVelocityTracker> velocity_tracker;
+
 protected:
 	void _update_camera();
 	virtual void _request_camera_update();
@@ -140,11 +150,17 @@ public:
 	void set_h_offset(float p_offset);
 	float get_h_offset() const;
 
+	void set_doppler_tracking(DopplerTracking p_tracking);
+	DopplerTracking get_doppler_tracking() const;
+
+	Vector3 get_doppler_tracked_velocity() const;
+
 	Camera();
 	~Camera();
 };
 
 VARIANT_ENUM_CAST(Camera::Projection);
 VARIANT_ENUM_CAST(Camera::KeepAspect);
+VARIANT_ENUM_CAST(Camera::DopplerTracking);
 
 #endif
diff --git a/scene/3d/collision_object.cpp b/scene/3d/collision_object.cpp
index 1b27313d3b..874e5f01ea 100644
--- a/scene/3d/collision_object.cpp
+++ b/scene/3d/collision_object.cpp
@@ -30,17 +30,6 @@
 #include "collision_object.h"
 #include "scene/scene_string_names.h"
 #include "servers/physics_server.h"
-void CollisionObject::_update_shapes_from_children() {
-
-	shapes.clear();
-	for (int i = 0; i < get_child_count(); i++) {
-
-		Node *n = get_child(i);
-		n->call("_add_to_collision_object", this);
-	}
-
-	_update_shapes();
-}
 
 void CollisionObject::_notification(int p_what) {
 
@@ -87,91 +76,6 @@ void CollisionObject::_notification(int p_what) {
 	}
 }
 
-void CollisionObject::_update_shapes() {
-
-	if (!rid.is_valid())
-		return;
-
-	if (area)
-		PhysicsServer::get_singleton()->area_clear_shapes(rid);
-	else
-		PhysicsServer::get_singleton()->body_clear_shapes(rid);
-
-	for (int i = 0; i < shapes.size(); i++) {
-
-		if (shapes[i].shape.is_null())
-			continue;
-		if (area)
-			PhysicsServer::get_singleton()->area_add_shape(rid, shapes[i].shape->get_rid(), shapes[i].xform);
-		else {
-			PhysicsServer::get_singleton()->body_add_shape(rid, shapes[i].shape->get_rid(), shapes[i].xform);
-			if (shapes[i].trigger)
-				PhysicsServer::get_singleton()->body_set_shape_as_trigger(rid, i, shapes[i].trigger);
-		}
-	}
-}
-
-bool CollisionObject::_set(const StringName &p_name, const Variant &p_value) {
-	String name = p_name;
-
-	if (name == "shape_count") {
-
-		shapes.resize(p_value);
-		_update_shapes();
-		_change_notify();
-
-	} else if (name.begins_with("shapes/")) {
-
-		int idx = name.get_slicec('/', 1).to_int();
-		String what = name.get_slicec('/', 2);
-		if (what == "shape")
-			set_shape(idx, RefPtr(p_value));
-		else if (what == "transform")
-			set_shape_transform(idx, p_value);
-		else if (what == "trigger")
-			set_shape_as_trigger(idx, p_value);
-
-	} else
-		return false;
-
-	return true;
-}
-
-bool CollisionObject::_get(const StringName &p_name, Variant &r_ret) const {
-
-	String name = p_name;
-
-	if (name == "shape_count") {
-		r_ret = shapes.size();
-	} else if (name.begins_with("shapes/")) {
-
-		int idx = name.get_slicec('/', 1).to_int();
-		String what = name.get_slicec('/', 2);
-		if (what == "shape")
-			r_ret = get_shape(idx);
-		else if (what == "transform")
-			r_ret = get_shape_transform(idx);
-		else if (what == "trigger")
-			r_ret = is_shape_set_as_trigger(idx);
-
-	} else
-		return false;
-
-	return true;
-}
-
-void CollisionObject::_get_property_list(List<PropertyInfo> *p_list) const {
-
-	p_list->push_back(PropertyInfo(Variant::INT, "shape_count", PROPERTY_HINT_RANGE, "0,256,1", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_NO_INSTANCE_STATE));
-
-	for (int i = 0; i < shapes.size(); i++) {
-		String path = "shapes/" + itos(i) + "/";
-		p_list->push_back(PropertyInfo(Variant::OBJECT, path + "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_NO_INSTANCE_STATE));
-		p_list->push_back(PropertyInfo(Variant::TRANSFORM, path + "transform", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_NO_INSTANCE_STATE));
-		p_list->push_back(PropertyInfo(Variant::BOOL, path + "trigger", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_NO_INSTANCE_STATE));
-	}
-}
-
 void CollisionObject::_input_event(Node *p_camera, const Ref<InputEvent> &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape) {
 
 	if (get_script_instance()) {
@@ -219,17 +123,6 @@ bool CollisionObject::is_ray_pickable() const {
 
 void CollisionObject::_bind_methods() {
 
-	ClassDB::bind_method(D_METHOD("add_shape", "shape:Shape", "transform"), &CollisionObject::add_shape, DEFVAL(Transform()));
-	ClassDB::bind_method(D_METHOD("get_shape_count"), &CollisionObject::get_shape_count);
-	ClassDB::bind_method(D_METHOD("set_shape", "shape_idx", "shape:Shape"), &CollisionObject::set_shape);
-	ClassDB::bind_method(D_METHOD("set_shape_transform", "shape_idx", "transform"), &CollisionObject::set_shape_transform);
-	//    ClassDB::bind_method(D_METHOD("set_shape_transform","shape_idx","transform"),&CollisionObject::set_shape_transform);
-	ClassDB::bind_method(D_METHOD("set_shape_as_trigger", "shape_idx", "enable"), &CollisionObject::set_shape_as_trigger);
-	ClassDB::bind_method(D_METHOD("is_shape_set_as_trigger", "shape_idx"), &CollisionObject::is_shape_set_as_trigger);
-	ClassDB::bind_method(D_METHOD("get_shape:Shape", "shape_idx"), &CollisionObject::get_shape);
-	ClassDB::bind_method(D_METHOD("get_shape_transform", "shape_idx"), &CollisionObject::get_shape_transform);
-	ClassDB::bind_method(D_METHOD("remove_shape", "shape_idx"), &CollisionObject::remove_shape);
-	ClassDB::bind_method(D_METHOD("clear_shapes"), &CollisionObject::clear_shapes);
 	ClassDB::bind_method(D_METHOD("set_ray_pickable", "ray_pickable"), &CollisionObject::set_ray_pickable);
 	ClassDB::bind_method(D_METHOD("is_ray_pickable"), &CollisionObject::is_ray_pickable);
 	ClassDB::bind_method(D_METHOD("set_capture_input_on_drag", "enable"), &CollisionObject::set_capture_input_on_drag);
@@ -245,72 +138,176 @@ void CollisionObject::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "input_capture_on_drag"), "set_capture_input_on_drag", "get_capture_input_on_drag");
 }
 
-void CollisionObject::add_shape(const Ref<Shape> &p_shape, const Transform &p_transform) {
+uint32_t CollisionObject::create_shape_owner(Object *p_owner) {
+
+	ShapeData sd;
+	uint32_t id;
+
+	if (shapes.size() == 0) {
+		id = 1;
+	} else {
+		id = shapes.back()->key() + 1;
+	}
+
+	sd.owner = p_owner;
+
+	shapes[id] = sd;
 
-	ShapeData sdata;
-	sdata.shape = p_shape;
-	sdata.xform = p_transform;
-	shapes.push_back(sdata);
-	_update_shapes();
+	return id;
 }
-int CollisionObject::get_shape_count() const {
 
-	return shapes.size();
+void CollisionObject::remove_shape_owner(uint32_t owner) {
+
+	ERR_FAIL_COND(!shapes.has(owner));
+
+	shape_owner_clear_shapes(owner);
+
+	shapes.erase(owner);
 }
-void CollisionObject::set_shape(int p_shape_idx, const Ref<Shape> &p_shape) {
 
-	ERR_FAIL_INDEX(p_shape_idx, shapes.size());
-	shapes[p_shape_idx].shape = p_shape;
-	_update_shapes();
+void CollisionObject::shape_owner_set_disabled(uint32_t p_owner, bool p_disabled) {
+	ERR_FAIL_COND(!shapes.has(p_owner));
+
+	ShapeData &sd = shapes[p_owner];
+	sd.disabled = p_disabled;
+	for (int i = 0; i < sd.shapes.size(); i++) {
+		if (area) {
+			PhysicsServer::get_singleton()->area_set_shape_disabled(rid, sd.shapes[i].index, p_disabled);
+		} else {
+			PhysicsServer::get_singleton()->body_set_shape_disabled(rid, sd.shapes[i].index, p_disabled);
+		}
+	}
 }
 
-void CollisionObject::set_shape_transform(int p_shape_idx, const Transform &p_transform) {
+bool CollisionObject::is_shape_owner_disabled(uint32_t p_owner) const {
 
-	ERR_FAIL_INDEX(p_shape_idx, shapes.size());
-	shapes[p_shape_idx].xform = p_transform;
+	ERR_FAIL_COND_V(!shapes.has(p_owner), false);
 
-	_update_shapes();
+	return shapes[p_owner].disabled;
 }
 
-Ref<Shape> CollisionObject::get_shape(int p_shape_idx) const {
+void CollisionObject::get_shape_owners(List<uint32_t> *r_owners) {
 
-	ERR_FAIL_INDEX_V(p_shape_idx, shapes.size(), Ref<Shape>());
-	return shapes[p_shape_idx].shape;
+	for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+		r_owners->push_back(E->key());
+	}
 }
-Transform CollisionObject::get_shape_transform(int p_shape_idx) const {
 
-	ERR_FAIL_INDEX_V(p_shape_idx, shapes.size(), Transform());
-	return shapes[p_shape_idx].xform;
+void CollisionObject::shape_owner_set_transform(uint32_t p_owner, const Transform &p_transform) {
+
+	ERR_FAIL_COND(!shapes.has(p_owner));
+
+	ShapeData &sd = shapes[p_owner];
+	sd.xform = p_transform;
+	for (int i = 0; i < sd.shapes.size(); i++) {
+		if (area) {
+			PhysicsServer::get_singleton()->area_set_shape_transform(rid, sd.shapes[i].index, p_transform);
+		} else {
+			PhysicsServer::get_singleton()->body_set_shape_transform(rid, sd.shapes[i].index, p_transform);
+		}
+	}
 }
-void CollisionObject::remove_shape(int p_shape_idx) {
+Transform CollisionObject::shape_owner_get_transform(uint32_t p_owner) const {
 
-	ERR_FAIL_INDEX(p_shape_idx, shapes.size());
-	shapes.remove(p_shape_idx);
+	ERR_FAIL_COND_V(!shapes.has(p_owner), Transform());
 
-	_update_shapes();
+	return shapes[p_owner].xform;
 }
 
-void CollisionObject::clear_shapes() {
+Object *CollisionObject::shape_owner_get_owner(uint32_t p_owner) const {
 
-	shapes.clear();
+	ERR_FAIL_COND_V(!shapes.has(p_owner), NULL);
 
-	_update_shapes();
+	return shapes[p_owner].owner;
 }
 
-void CollisionObject::set_shape_as_trigger(int p_shape_idx, bool p_trigger) {
+void CollisionObject::shape_owner_add_shape(uint32_t p_owner, const Ref<Shape> &p_shape) {
 
-	ERR_FAIL_INDEX(p_shape_idx, shapes.size());
-	shapes[p_shape_idx].trigger = p_trigger;
-	if (!area && rid.is_valid()) {
+	ERR_FAIL_COND(!shapes.has(p_owner));
+	ERR_FAIL_COND(p_shape.is_null());
 
-		PhysicsServer::get_singleton()->body_set_shape_as_trigger(rid, p_shape_idx, p_trigger);
+	ShapeData &sd = shapes[p_owner];
+	ShapeData::ShapeBase s;
+	s.index = total_subshapes;
+	s.shape = p_shape;
+	if (area) {
+		PhysicsServer::get_singleton()->area_add_shape(rid, p_shape->get_rid(), sd.xform);
+	} else {
+		PhysicsServer::get_singleton()->body_add_shape(rid, p_shape->get_rid(), sd.xform);
 	}
+	sd.shapes.push_back(s);
+
+	total_subshapes++;
 }
+int CollisionObject::shape_owner_get_shape_count(uint32_t p_owner) const {
 
-bool CollisionObject::is_shape_set_as_trigger(int p_shape_idx) const {
+	ERR_FAIL_COND_V(!shapes.has(p_owner), 0);
 
-	ERR_FAIL_INDEX_V(p_shape_idx, shapes.size(), false);
-	return shapes[p_shape_idx].trigger;
+	return shapes[p_owner].shapes.size();
+}
+Ref<Shape> CollisionObject::shape_owner_get_shape(uint32_t p_owner, int p_shape) const {
+
+	ERR_FAIL_COND_V(!shapes.has(p_owner), Ref<Shape>());
+	ERR_FAIL_INDEX_V(p_shape, shapes[p_owner].shapes.size(), Ref<Shape>());
+
+	return shapes[p_owner].shapes[p_shape].shape;
+}
+int CollisionObject::shape_owner_get_shape_index(uint32_t p_owner, int p_shape) const {
+
+	ERR_FAIL_COND_V(!shapes.has(p_owner), -1);
+	ERR_FAIL_INDEX_V(p_shape, shapes[p_owner].shapes.size(), -1);
+
+	return shapes[p_owner].shapes[p_shape].index;
+}
+
+void CollisionObject::shape_owner_remove_shape(uint32_t p_owner, int p_shape) {
+
+	ERR_FAIL_COND(!shapes.has(p_owner));
+	ERR_FAIL_INDEX(p_shape, shapes[p_owner].shapes.size());
+
+	int index_to_remove = shapes[p_owner].shapes[p_shape].index;
+	if (area) {
+		PhysicsServer::get_singleton()->area_remove_shape(rid, index_to_remove);
+	} else {
+		PhysicsServer::get_singleton()->body_remove_shape(rid, index_to_remove);
+	}
+
+	shapes[p_owner].shapes.remove(p_shape);
+
+	for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+		for (int i = 0; i < E->get().shapes.size(); i++) {
+			if (E->get().shapes[i].index > index_to_remove) {
+				E->get().shapes[i].index -= 1;
+			}
+		}
+	}
+
+	total_subshapes--;
+}
+
+void CollisionObject::shape_owner_clear_shapes(uint32_t p_owner) {
+
+	ERR_FAIL_COND(!shapes.has(p_owner));
+
+	while (shape_owner_get_shape_count(p_owner) > 0) {
+		shape_owner_remove_shape(p_owner, 0);
+	}
+}
+
+uint32_t CollisionObject::shape_find_owner(int p_shape_index) const {
+
+	ERR_FAIL_INDEX_V(p_shape_index, total_subshapes, 0);
+
+	for (const Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
+		for (int i = 0; i < E->get().shapes.size(); i++) {
+			if (E->get().shapes[i].index == p_shape_index) {
+				return E->key();
+			}
+		}
+	}
+
+	//in theory it should be unreachable
+	return 0;
 }
 
 CollisionObject::CollisionObject(RID p_rid, bool p_area) {
@@ -320,6 +317,8 @@ CollisionObject::CollisionObject(RID p_rid, bool p_area) {
 	capture_input_on_drag = false;
 	ray_pickable = true;
 	set_notify_transform(true);
+	total_subshapes = 0;
+
 	if (p_area) {
 		PhysicsServer::get_singleton()->area_attach_object_instance_ID(rid, get_instance_ID());
 	} else {
diff --git a/scene/3d/collision_object.h b/scene/3d/collision_object.h
index 3822fb0d5a..fac05b6e8c 100644
--- a/scene/3d/collision_object.h
+++ b/scene/3d/collision_object.h
@@ -41,33 +41,36 @@ class CollisionObject : public Spatial {
 	RID rid;
 
 	struct ShapeData {
+
+		Object *owner;
 		Transform xform;
-		Ref<Shape> shape;
-		bool trigger;
+		struct ShapeBase {
+			Ref<Shape> shape;
+			int index;
+		};
+
+		Vector<ShapeBase> shapes;
+		bool disabled;
 
 		ShapeData() {
-			trigger = false;
+			disabled = false;
+			owner = NULL;
 		}
 	};
 
+	int total_subshapes;
+
+	Map<uint32_t, ShapeData> shapes;
+
 	bool capture_input_on_drag;
 	bool ray_pickable;
-	Vector<ShapeData> shapes;
 
 	void _update_pickable();
-	void _update_shapes();
-
-	friend class CollisionShape;
-	friend class CollisionPolygon;
-	void _update_shapes_from_children();
 
 protected:
 	CollisionObject(RID p_rid, bool p_area);
 
 	void _notification(int p_what);
-	bool _set(const StringName &p_name, const Variant &p_value);
-	bool _get(const StringName &p_name, Variant &r_ret) const;
-	void _get_property_list(List<PropertyInfo> *p_list) const;
 	static void _bind_methods();
 	friend class Viewport;
 	virtual void _input_event(Node *p_camera, const Ref<InputEvent> &p_input_event, const Vector3 &p_pos, const Vector3 &p_normal, int p_shape);
@@ -75,16 +78,26 @@ protected:
 	virtual void _mouse_exit();
 
 public:
-	void add_shape(const Ref<Shape> &p_shape, const Transform &p_transform = Transform());
-	int get_shape_count() const;
-	void set_shape(int p_shape_idx, const Ref<Shape> &p_shape);
-	void set_shape_transform(int p_shape_idx, const Transform &p_transform);
-	Ref<Shape> get_shape(int p_shape_idx) const;
-	Transform get_shape_transform(int p_shape_idx) const;
-	void remove_shape(int p_shape_idx);
-	void clear_shapes();
-	void set_shape_as_trigger(int p_shape_idx, bool p_trigger);
-	bool is_shape_set_as_trigger(int p_shape_idx) const;
+	uint32_t create_shape_owner(Object *p_owner);
+	void remove_shape_owner(uint32_t owner);
+	void get_shape_owners(List<uint32_t> *r_owners);
+
+	void shape_owner_set_transform(uint32_t p_owner, const Transform &p_transform);
+	Transform shape_owner_get_transform(uint32_t p_owner) const;
+	Object *shape_owner_get_owner(uint32_t p_owner) const;
+
+	void shape_owner_set_disabled(uint32_t p_owner, bool p_disabled);
+	bool is_shape_owner_disabled(uint32_t p_owner) const;
+
+	void shape_owner_add_shape(uint32_t p_owner, const Ref<Shape> &p_shape);
+	int shape_owner_get_shape_count(uint32_t p_owner) const;
+	Ref<Shape> shape_owner_get_shape(uint32_t p_owner, int p_shape) const;
+	int shape_owner_get_shape_index(uint32_t p_owner, int p_shape) const;
+
+	void shape_owner_remove_shape(uint32_t p_owner, int p_shape);
+	void shape_owner_clear_shapes(uint32_t p_owner);
+
+	uint32_t shape_find_owner(int p_shape_index) const;
 
 	void set_ray_pickable(bool p_ray_pickable);
 	bool is_ray_pickable() const;
diff --git a/scene/3d/collision_polygon.cpp b/scene/3d/collision_polygon.cpp
index d9321f7134..0c61c96d07 100644
--- a/scene/3d/collision_polygon.cpp
+++ b/scene/3d/collision_polygon.cpp
@@ -33,186 +33,82 @@
 #include "scene/resources/concave_polygon_shape.h"
 #include "scene/resources/convex_polygon_shape.h"
 
-void CollisionPolygon::_add_to_collision_object(Object *p_obj) {
+void CollisionPolygon::_build_polygon() {
 
-	if (!can_update_body)
+	if (!parent)
 		return;
 
-	CollisionObject *co = p_obj->cast_to<CollisionObject>();
-	ERR_FAIL_COND(!co);
+	parent->shape_owner_clear_shapes(owner_id);
 
 	if (polygon.size() == 0)
 		return;
 
-	bool solids = build_mode == BUILD_SOLIDS;
-
 	Vector<Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon);
 	if (decomp.size() == 0)
 		return;
 
-	if (true || solids) {
-
-		//here comes the sun, lalalala
-		//decompose concave into multiple convex polygons and add them
-		shape_from = co->get_shape_count();
-		for (int i = 0; i < decomp.size(); i++) {
-			Ref<ConvexPolygonShape> convex = memnew(ConvexPolygonShape);
-			PoolVector<Vector3> cp;
-			int cs = decomp[i].size();
-			cp.resize(cs * 2);
-			{
-				PoolVector<Vector3>::Write w = cp.write();
-				int idx = 0;
-				for (int j = 0; j < cs; j++) {
-
-					Vector2 d = decomp[i][j];
-					w[idx++] = Vector3(d.x, d.y, depth * 0.5);
-					w[idx++] = Vector3(d.x, d.y, -depth * 0.5);
-				}
+	//here comes the sun, lalalala
+	//decompose concave into multiple convex polygons and add them
+
+	for (int i = 0; i < decomp.size(); i++) {
+		Ref<ConvexPolygonShape> convex = memnew(ConvexPolygonShape);
+		PoolVector<Vector3> cp;
+		int cs = decomp[i].size();
+		cp.resize(cs * 2);
+		{
+			PoolVector<Vector3>::Write w = cp.write();
+			int idx = 0;
+			for (int j = 0; j < cs; j++) {
+
+				Vector2 d = decomp[i][j];
+				w[idx++] = Vector3(d.x, d.y, depth * 0.5);
+				w[idx++] = Vector3(d.x, d.y, -depth * 0.5);
 			}
-
-			convex->set_points(cp);
-			co->add_shape(convex, get_transform());
-		}
-		shape_to = co->get_shape_count() - 1;
-		if (shape_to < shape_from) {
-			shape_from = -1;
-			shape_to = -1;
-		}
-
-	} else {
-#if 0
-		Ref<ConcavePolygonShape> concave = memnew( ConcavePolygonShape );
-
-		PoolVector<Vector2> segments;
-		segments.resize(polygon.size()*2);
-		PoolVector<Vector2>::Write w=segments.write();
-
-		for(int i=0;i<polygon.size();i++) {
-			w[(i<<1)+0]=polygon[i];
-			w[(i<<1)+1]=polygon[(i+1)%polygon.size()];
 		}
 
-		w=PoolVector<Vector2>::Write();
-		concave->set_segments(segments);
-
-		co->add_shape(concave,get_transform());
-#endif
+		convex->set_points(cp);
+		parent->shape_owner_add_shape(owner_id, convex);
+		parent->shape_owner_set_disabled(owner_id, disabled);
 	}
-
-	//co->add_shape(shape,get_transform());
-}
-
-void CollisionPolygon::_update_parent() {
-
-	if (!can_update_body)
-		return;
-
-	Node *parent = get_parent();
-	if (!parent)
-		return;
-	CollisionObject *co = parent->cast_to<CollisionObject>();
-	if (!co)
-		return;
-	co->_update_shapes_from_children();
-}
-
-void CollisionPolygon::_set_shape_range(const Vector2 &p_range) {
-
-	shape_from = p_range.x;
-	shape_to = p_range.y;
-}
-
-Vector2 CollisionPolygon::_get_shape_range() const {
-
-	return Vector2(shape_from, shape_to);
 }
 
 void CollisionPolygon::_notification(int p_what) {
 
 	switch (p_what) {
-		case NOTIFICATION_ENTER_TREE: {
-			can_update_body = get_tree()->is_editor_hint();
-			set_notify_local_transform(!can_update_body);
 
-			//indicator_instance = VisualServer::get_singleton()->instance_create2(indicator,get_world()->get_scenario());
-		} break;
-		case NOTIFICATION_EXIT_TREE: {
-			can_update_body = false;
-			set_notify_local_transform(false);
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
-
-			if (!is_inside_tree())
-				break;
-			if (can_update_body) {
-				_update_parent();
+		case NOTIFICATION_PARENTED: {
+			parent = get_parent()->cast_to<CollisionObject>();
+			if (parent) {
+				owner_id = parent->create_shape_owner(this);
+				_build_polygon();
+				parent->shape_owner_set_transform(owner_id, get_transform());
+				parent->shape_owner_set_disabled(owner_id, disabled);
 			}
-
 		} break;
 		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
-			if (!can_update_body && shape_from >= 0 && shape_to >= 0) {
-
-				CollisionObject *co = get_parent()->cast_to<CollisionObject>();
-				if (co) {
-					for (int i = shape_from; i <= shape_to; i++) {
-						co->set_shape_transform(i, get_transform());
-					}
-				}
-			}
 
-		} break;
-#if 0
-		case NOTIFICATION_DRAW: {
-			for(int i=0;i<polygon.size();i++) {
-
-				Vector2 p = polygon[i];
-				Vector2 n = polygon[(i+1)%polygon.size()];
-				draw_line(p,n,Color(0,0.6,0.7,0.5),3);
+			if (parent) {
+				parent->shape_owner_set_transform(owner_id, get_transform());
 			}
 
-			Vector< Vector<Vector2> > decomp = Geometry::decompose_polygon(polygon);
-#define DEBUG_DECOMPOSE
-#ifdef DEBUG_DECOMPOSE
-			Color c(0.4,0.9,0.1);
-			for(int i=0;i<decomp.size();i++) {
-
-				c.set_hsv( Math::fmod(c.get_h() + 0.738,1),c.get_s(),c.get_v(),0.5);
-				draw_colored_polygon(decomp[i],c);
+		} break;
+		case NOTIFICATION_UNPARENTED: {
+			if (parent) {
+				parent->remove_shape_owner(owner_id);
 			}
-#endif
-
+			owner_id = 0;
+			parent = NULL;
 		} break;
-#endif
 	}
 }
 
 void CollisionPolygon::set_polygon(const Vector<Point2> &p_polygon) {
 
 	polygon = p_polygon;
-	if (can_update_body) {
-
-		for (int i = 0; i < polygon.size(); i++) {
-
-			Vector3 p1(polygon[i].x, polygon[i].y, depth * 0.5);
-
-			if (i == 0)
-				aabb = Rect3(p1, Vector3());
-			else
-				aabb.expand_to(p1);
-
-			Vector3 p2(polygon[i].x, polygon[i].y, -depth * 0.5);
-			aabb.expand_to(p2);
-		}
-		if (aabb == Rect3()) {
-
-			aabb = Rect3(Vector3(-1, -1, -1), Vector3(2, 2, 2));
-		} else {
-			aabb.position -= aabb.size * 0.3;
-			aabb.size += aabb.size * 0.6;
-		}
-		_update_parent();
+	if (parent) {
+		_build_polygon();
 	}
+	update_configuration_warning();
 	update_gizmo();
 }
 
@@ -221,20 +117,6 @@ Vector<Point2> CollisionPolygon::get_polygon() const {
 	return polygon;
 }
 
-void CollisionPolygon::set_build_mode(BuildMode p_mode) {
-
-	ERR_FAIL_INDEX(p_mode, 2);
-	build_mode = p_mode;
-	if (!can_update_body)
-		return;
-	_update_parent();
-}
-
-CollisionPolygon::BuildMode CollisionPolygon::get_build_mode() const {
-
-	return build_mode;
-}
-
 Rect3 CollisionPolygon::get_item_rect() const {
 
 	return aabb;
@@ -243,9 +125,7 @@ Rect3 CollisionPolygon::get_item_rect() const {
 void CollisionPolygon::set_depth(float p_depth) {
 
 	depth = p_depth;
-	if (!can_update_body)
-		return;
-	_update_parent();
+	_build_polygon();
 	update_gizmo();
 }
 
@@ -254,6 +134,17 @@ float CollisionPolygon::get_depth() const {
 	return depth;
 }
 
+void CollisionPolygon::set_disabled(bool p_disabled) {
+	disabled = p_disabled;
+	if (parent) {
+		parent->shape_owner_set_disabled(owner_id, p_disabled);
+	}
+}
+
+bool CollisionPolygon::is_disabled() const {
+	return disabled;
+}
+
 String CollisionPolygon::get_configuration_warning() const {
 
 	if (!get_parent()->cast_to<CollisionObject>()) {
@@ -269,36 +160,26 @@ String CollisionPolygon::get_configuration_warning() const {
 
 void CollisionPolygon::_bind_methods() {
 
-	ClassDB::bind_method(D_METHOD("_add_to_collision_object"), &CollisionPolygon::_add_to_collision_object);
-
-	ClassDB::bind_method(D_METHOD("set_build_mode", "build_mode"), &CollisionPolygon::set_build_mode);
-	ClassDB::bind_method(D_METHOD("get_build_mode"), &CollisionPolygon::get_build_mode);
-
 	ClassDB::bind_method(D_METHOD("set_depth", "depth"), &CollisionPolygon::set_depth);
 	ClassDB::bind_method(D_METHOD("get_depth"), &CollisionPolygon::get_depth);
 
 	ClassDB::bind_method(D_METHOD("set_polygon", "polygon"), &CollisionPolygon::set_polygon);
 	ClassDB::bind_method(D_METHOD("get_polygon"), &CollisionPolygon::get_polygon);
 
-	ClassDB::bind_method(D_METHOD("_set_shape_range", "shape_range"), &CollisionPolygon::_set_shape_range);
-	ClassDB::bind_method(D_METHOD("_get_shape_range"), &CollisionPolygon::_get_shape_range);
-
-	ClassDB::bind_method(D_METHOD("get_collision_object_first_shape"), &CollisionPolygon::get_collision_object_first_shape);
-	ClassDB::bind_method(D_METHOD("get_collision_object_last_shape"), &CollisionPolygon::get_collision_object_last_shape);
+	ClassDB::bind_method(D_METHOD("set_disabled", "disabled"), &CollisionPolygon::set_disabled);
+	ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionPolygon::is_disabled);
 
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "build_mode", PROPERTY_HINT_ENUM, "Solids,Triangles"), "set_build_mode", "get_build_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "depth"), "set_depth", "get_depth");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled");
 	ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR2_ARRAY, "polygon"), "set_polygon", "get_polygon");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "shape_range", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "_set_shape_range", "_get_shape_range");
 }
 
 CollisionPolygon::CollisionPolygon() {
 
-	shape_from = -1;
-	shape_to = -1;
-	can_update_body = false;
-
 	aabb = Rect3(Vector3(-1, -1, -1), Vector3(2, 2, 2));
-	build_mode = BUILD_SOLIDS;
 	depth = 1.0;
+	set_notify_local_transform(true);
+	parent = NULL;
+	owner_id = 0;
+	disabled = false;
 }
diff --git a/scene/3d/collision_polygon.h b/scene/3d/collision_polygon.h
index d45b4738ae..beefae182c 100644
--- a/scene/3d/collision_polygon.h
+++ b/scene/3d/collision_polygon.h
@@ -33,55 +33,42 @@
 #include "scene/3d/spatial.h"
 #include "scene/resources/shape.h"
 
+class CollisionObject;
 class CollisionPolygon : public Spatial {
 
 	GDCLASS(CollisionPolygon, Spatial);
 
-public:
-	enum BuildMode {
-		BUILD_SOLIDS,
-		BUILD_TRIANGLES,
-	};
-
 protected:
 	float depth;
 	Rect3 aabb;
-	BuildMode build_mode;
 	Vector<Point2> polygon;
 
-	void _add_to_collision_object(Object *p_obj);
-	void _update_parent();
+	uint32_t owner_id;
+	CollisionObject *parent;
 
-	bool can_update_body;
-	int shape_from;
-	int shape_to;
+	bool disabled;
 
-	void _set_shape_range(const Vector2 &p_range);
-	Vector2 _get_shape_range() const;
+	void _build_polygon();
 
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
-	void set_build_mode(BuildMode p_mode);
-	BuildMode get_build_mode() const;
-
 	void set_depth(float p_depth);
 	float get_depth() const;
 
 	void set_polygon(const Vector<Point2> &p_polygon);
 	Vector<Point2> get_polygon() const;
 
-	virtual Rect3 get_item_rect() const;
+	void set_disabled(bool p_disabled);
+	bool is_disabled() const;
 
-	int get_collision_object_first_shape() const { return shape_from; }
-	int get_collision_object_last_shape() const { return shape_to; }
+	virtual Rect3 get_item_rect() const;
 
 	String get_configuration_warning() const;
 
 	CollisionPolygon();
 };
 
-VARIANT_ENUM_CAST(CollisionPolygon::BuildMode);
 #endif // COLLISION_POLYGON_H
diff --git a/scene/3d/collision_shape.cpp b/scene/3d/collision_shape.cpp
new file mode 100644
index 0000000000..4fd215bd1a
--- /dev/null
+++ b/scene/3d/collision_shape.cpp
@@ -0,0 +1,214 @@
+/*************************************************************************/
+/*  body_shape.cpp                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                    http://www.godotengine.org                         */
+/*************************************************************************/
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+#include "collision_shape.h"
+#include "scene/resources/box_shape.h"
+#include "scene/resources/capsule_shape.h"
+#include "scene/resources/concave_polygon_shape.h"
+#include "scene/resources/convex_polygon_shape.h"
+#include "scene/resources/plane_shape.h"
+#include "scene/resources/ray_shape.h"
+#include "scene/resources/sphere_shape.h"
+#include "servers/visual_server.h"
+//TODO: Implement CylinderShape and HeightMapShape?
+#include "mesh_instance.h"
+#include "physics_body.h"
+#include "quick_hull.h"
+
+void CollisionShape::make_convex_from_brothers() {
+
+	Node *p = get_parent();
+	if (!p)
+		return;
+
+	for (int i = 0; i < p->get_child_count(); i++) {
+
+		Node *n = p->get_child(i);
+		if (n->cast_to<MeshInstance>()) {
+
+			MeshInstance *mi = n->cast_to<MeshInstance>();
+			Ref<Mesh> m = mi->get_mesh();
+			if (m.is_valid()) {
+
+				Ref<Shape> s = m->create_convex_shape();
+				set_shape(s);
+			}
+		}
+	}
+}
+
+void CollisionShape::_notification(int p_what) {
+
+	switch (p_what) {
+
+		case NOTIFICATION_PARENTED: {
+			parent = get_parent()->cast_to<CollisionObject>();
+			if (parent) {
+				owner_id = parent->create_shape_owner(this);
+				if (shape.is_valid()) {
+					parent->shape_owner_add_shape(owner_id, shape);
+				}
+				parent->shape_owner_set_transform(owner_id, get_transform());
+				parent->shape_owner_set_disabled(owner_id, disabled);
+			}
+		} break;
+		case NOTIFICATION_ENTER_TREE: {
+			if (get_tree()->is_debugging_collisions_hint()) {
+				_create_debug_shape();
+			}
+
+		} break;
+		case NOTIFICATION_LOCAL_TRANSFORM_CHANGED: {
+			if (parent) {
+				parent->shape_owner_set_transform(owner_id, get_transform());
+			}
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			if (parent) {
+				parent->remove_shape_owner(owner_id);
+			}
+			owner_id = 0;
+			parent = NULL;
+		} break;
+		case NOTIFICATION_UNPARENTED: {
+			if (parent) {
+				parent->remove_shape_owner(owner_id);
+			}
+			owner_id = 0;
+			parent = NULL;
+		} break;
+	}
+}
+
+void CollisionShape::resource_changed(RES res) {
+
+	update_gizmo();
+}
+
+String CollisionShape::get_configuration_warning() const {
+
+	if (!get_parent()->cast_to<CollisionObject>()) {
+		return TTR("CollisionShape only serves to provide a collision shape to a CollisionObject derived node. Please only use it as a child of Area, StaticBody, RigidBody, KinematicBody, etc. to give them a shape.");
+	}
+
+	if (!shape.is_valid()) {
+		return TTR("A shape must be provided for CollisionShape to function. Please create a shape resource for it!");
+	}
+
+	return String();
+}
+
+void CollisionShape::_bind_methods() {
+
+	//not sure if this should do anything
+	ClassDB::bind_method(D_METHOD("resource_changed", "resource"), &CollisionShape::resource_changed);
+	ClassDB::bind_method(D_METHOD("set_shape", "shape"), &CollisionShape::set_shape);
+	ClassDB::bind_method(D_METHOD("get_shape"), &CollisionShape::get_shape);
+	ClassDB::bind_method(D_METHOD("set_disabled", "enable"), &CollisionShape::set_disabled);
+	ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionShape::is_disabled);
+	ClassDB::bind_method(D_METHOD("make_convex_from_brothers"), &CollisionShape::make_convex_from_brothers);
+	ClassDB::set_method_flags("CollisionShape", "make_convex_from_brothers", METHOD_FLAGS_DEFAULT | METHOD_FLAG_EDITOR);
+
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape"), "set_shape", "get_shape");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled");
+}
+
+void CollisionShape::set_shape(const Ref<Shape> &p_shape) {
+
+	if (!shape.is_null())
+		shape->unregister_owner(this);
+	shape = p_shape;
+	if (!shape.is_null())
+		shape->register_owner(this);
+	update_gizmo();
+	if (parent) {
+		parent->shape_owner_clear_shapes(owner_id);
+		if (shape.is_valid()) {
+			parent->shape_owner_add_shape(owner_id, shape);
+		}
+	}
+
+	update_configuration_warning();
+}
+
+Ref<Shape> CollisionShape::get_shape() const {
+
+	return shape;
+}
+
+void CollisionShape::set_disabled(bool p_disabled) {
+
+	disabled = p_disabled;
+	update_gizmo();
+	if (parent) {
+		parent->shape_owner_set_disabled(owner_id, p_disabled);
+	}
+}
+
+bool CollisionShape::is_disabled() const {
+
+	return disabled;
+}
+
+CollisionShape::CollisionShape() {
+
+	//indicator = VisualServer::get_singleton()->mesh_create();
+	disabled = false;
+	debug_shape = NULL;
+	parent = NULL;
+	owner_id = 0;
+	set_notify_local_transform(true);
+}
+
+CollisionShape::~CollisionShape() {
+	if (!shape.is_null())
+		shape->unregister_owner(this);
+	//VisualServer::get_singleton()->free(indicator);
+}
+
+void CollisionShape::_create_debug_shape() {
+
+	if (debug_shape) {
+		debug_shape->queue_delete();
+		debug_shape = NULL;
+	}
+
+	Ref<Shape> s = get_shape();
+
+	if (s.is_null())
+		return;
+
+	Ref<Mesh> mesh = s->get_debug_mesh();
+
+	MeshInstance *mi = memnew(MeshInstance);
+	mi->set_mesh(mesh);
+
+	add_child(mi);
+	debug_shape = mi;
+}
diff --git a/scene/3d/body_shape.h b/scene/3d/collision_shape.h
index f4392dda62..277e0dfa77 100644
--- a/scene/3d/body_shape.h
+++ b/scene/3d/collision_shape.h
@@ -32,7 +32,7 @@
 
 #include "scene/3d/spatial.h"
 #include "scene/resources/shape.h"
-
+class CollisionObject;
 class CollisionShape : public Spatial {
 
 	GDCLASS(CollisionShape, Spatial);
@@ -40,34 +40,13 @@ class CollisionShape : public Spatial {
 
 	Ref<Shape> shape;
 
-	/*
-	RID _get_visual_instance_rid() const;
-
-
-	void _update_indicator();
-
-	RID material;
-	RID indicator;
-	RID indicator_instance;
-	*/
+	uint32_t owner_id;
+	CollisionObject *parent;
 
 	Node *debug_shape;
 
 	void resource_changed(RES res);
-
-	bool updating_body;
-	bool unparenting;
-	bool trigger;
-
-	bool can_update_body;
-
-	int update_shape_index;
-
-	void _update_body();
-	void _add_to_collision_object(Object *p_cshape);
-
-	void _set_update_shape_index(int p_index);
-	int _get_update_shape_index() const;
+	bool disabled;
 
 	void _create_debug_shape();
 
@@ -81,13 +60,8 @@ public:
 	void set_shape(const Ref<Shape> &p_shape);
 	Ref<Shape> get_shape() const;
 
-	void set_updating_body(bool p_update);
-	bool is_updating_body() const;
-
-	void set_trigger(bool p_trigger);
-	bool is_trigger() const;
-
-	int get_collision_object_shape_index() const { return _get_update_shape_index(); }
+	void set_disabled(bool p_disabled);
+	bool is_disabled() const;
 
 	String get_configuration_warning() const;
 
diff --git a/scene/3d/gi_probe.cpp b/scene/3d/gi_probe.cpp
index adca1492c3..581dfb32c8 100644
--- a/scene/3d/gi_probe.cpp
+++ b/scene/3d/gi_probe.cpp
@@ -95,6 +95,16 @@ float GIProbeData::get_bias() const {
 	return VS::get_singleton()->gi_probe_get_bias(probe);
 }
 
+void GIProbeData::set_normal_bias(float p_range) {
+
+	VS::get_singleton()->gi_probe_set_normal_bias(probe, p_range);
+}
+
+float GIProbeData::get_normal_bias() const {
+
+	return VS::get_singleton()->gi_probe_get_normal_bias(probe);
+}
+
 void GIProbeData::set_propagation(float p_range) {
 
 	VS::get_singleton()->gi_probe_set_propagation(probe, p_range);
@@ -158,6 +168,9 @@ void GIProbeData::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_bias", "bias"), &GIProbeData::set_bias);
 	ClassDB::bind_method(D_METHOD("get_bias"), &GIProbeData::get_bias);
 
+	ClassDB::bind_method(D_METHOD("set_normal_bias", "bias"), &GIProbeData::set_normal_bias);
+	ClassDB::bind_method(D_METHOD("get_normal_bias"), &GIProbeData::get_normal_bias);
+
 	ClassDB::bind_method(D_METHOD("set_propagation", "propagation"), &GIProbeData::set_propagation);
 	ClassDB::bind_method(D_METHOD("get_propagation"), &GIProbeData::get_propagation);
 
@@ -175,6 +188,7 @@ void GIProbeData::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "dynamic_range", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_dynamic_range", "get_dynamic_range");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "energy", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_energy", "get_energy");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "bias", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_bias", "get_bias");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "normal_bias", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_normal_bias", "get_normal_bias");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "propagation", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_propagation", "get_propagation");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "interior", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_interior", "is_interior");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "compress", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR), "set_compress", "is_compressed");
@@ -265,6 +279,18 @@ float GIProbe::get_bias() const {
 	return bias;
 }
 
+void GIProbe::set_normal_bias(float p_normal_bias) {
+
+	normal_bias = p_normal_bias;
+	if (probe_data.is_valid()) {
+		probe_data->set_normal_bias(normal_bias);
+	}
+}
+float GIProbe::get_normal_bias() const {
+
+	return normal_bias;
+}
+
 void GIProbe::set_propagation(float p_propagation) {
 
 	propagation = p_propagation;
@@ -1261,6 +1287,7 @@ void GIProbe::bake(Node *p_from_node, bool p_create_visual_debug) {
 		probe_data->set_dynamic_range(dynamic_range);
 		probe_data->set_energy(energy);
 		probe_data->set_bias(bias);
+		probe_data->set_normal_bias(normal_bias);
 		probe_data->set_propagation(propagation);
 		probe_data->set_interior(interior);
 		probe_data->set_compress(compress);
@@ -1434,6 +1461,9 @@ void GIProbe::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_bias", "max"), &GIProbe::set_bias);
 	ClassDB::bind_method(D_METHOD("get_bias"), &GIProbe::get_bias);
 
+	ClassDB::bind_method(D_METHOD("set_normal_bias", "max"), &GIProbe::set_normal_bias);
+	ClassDB::bind_method(D_METHOD("get_normal_bias"), &GIProbe::get_normal_bias);
+
 	ClassDB::bind_method(D_METHOD("set_propagation", "max"), &GIProbe::set_propagation);
 	ClassDB::bind_method(D_METHOD("get_propagation"), &GIProbe::get_propagation);
 
@@ -1453,6 +1483,7 @@ void GIProbe::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "energy", PROPERTY_HINT_RANGE, "0,16,0.01"), "set_energy", "get_energy");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "propagation", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_propagation", "get_propagation");
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "bias", PROPERTY_HINT_RANGE, "0,4,0.001"), "set_bias", "get_bias");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "normal_bias", PROPERTY_HINT_RANGE, "0,4,0.001"), "set_normal_bias", "get_normal_bias");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "interior"), "set_interior", "is_interior");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "compress"), "set_compress", "is_compressed");
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "data", PROPERTY_HINT_RESOURCE_TYPE, "GIProbeData"), "set_probe_data", "get_probe_data");
@@ -1468,7 +1499,8 @@ GIProbe::GIProbe() {
 	subdiv = SUBDIV_128;
 	dynamic_range = 4;
 	energy = 1.0;
-	bias = 1.8;
+	bias = 0.0;
+	normal_bias = 0.8;
 	propagation = 1.0;
 	extents = Vector3(10, 10, 10);
 	color_scan_cell_width = 4;
diff --git a/scene/3d/gi_probe.h b/scene/3d/gi_probe.h
index b5ee86455e..3f5a50a560 100644
--- a/scene/3d/gi_probe.h
+++ b/scene/3d/gi_probe.h
@@ -67,6 +67,9 @@ public:
 	void set_bias(float p_range);
 	float get_bias() const;
 
+	void set_normal_bias(float p_range);
+	float get_normal_bias() const;
+
 	void set_interior(bool p_enable);
 	bool is_interior() const;
 
@@ -163,6 +166,7 @@ private:
 	int dynamic_range;
 	float energy;
 	float bias;
+	float normal_bias;
 	float propagation;
 	bool interior;
 	bool compress;
@@ -204,6 +208,9 @@ public:
 	void set_bias(float p_bias);
 	float get_bias() const;
 
+	void set_normal_bias(float p_normal_bias);
+	float get_normal_bias() const;
+
 	void set_propagation(float p_propagation);
 	float get_propagation() const;
 
diff --git a/scene/3d/mesh_instance.cpp b/scene/3d/mesh_instance.cpp
index 62e1a95209..558932ceb9 100644
--- a/scene/3d/mesh_instance.cpp
+++ b/scene/3d/mesh_instance.cpp
@@ -29,7 +29,7 @@
 /*************************************************************************/
 #include "mesh_instance.h"
 
-#include "body_shape.h"
+#include "collision_shape.h"
 #include "core_string_names.h"
 #include "physics_body.h"
 #include "scene/resources/material.h"
@@ -194,7 +194,9 @@ Node *MeshInstance::create_trimesh_collision_node() {
 		return NULL;
 
 	StaticBody *static_body = memnew(StaticBody);
-	static_body->add_shape(shape);
+	CollisionShape *cshape = memnew(CollisionShape);
+	cshape->set_shape(shape);
+	static_body->add_child(cshape);
 	return static_body;
 }
 
@@ -205,13 +207,11 @@ void MeshInstance::create_trimesh_collision() {
 	static_body->set_name(String(get_name()) + "_col");
 
 	add_child(static_body);
-	if (get_owner())
+	if (get_owner()) {
+		CollisionShape *cshape = static_body->get_child(0)->cast_to<CollisionShape>();
 		static_body->set_owner(get_owner());
-	CollisionShape *cshape = memnew(CollisionShape);
-	cshape->set_shape(static_body->get_shape(0));
-	static_body->add_child(cshape);
-	if (get_owner())
 		cshape->set_owner(get_owner());
+	}
 }
 
 Node *MeshInstance::create_convex_collision_node() {
@@ -224,7 +224,9 @@ Node *MeshInstance::create_convex_collision_node() {
 		return NULL;
 
 	StaticBody *static_body = memnew(StaticBody);
-	static_body->add_shape(shape);
+	CollisionShape *cshape = memnew(CollisionShape);
+	cshape->set_shape(shape);
+	static_body->add_child(cshape);
 	return static_body;
 }
 
@@ -235,13 +237,11 @@ void MeshInstance::create_convex_collision() {
 	static_body->set_name(String(get_name()) + "_col");
 
 	add_child(static_body);
-	if (get_owner())
+	if (get_owner()) {
+		CollisionShape *cshape = static_body->get_child(0)->cast_to<CollisionShape>();
 		static_body->set_owner(get_owner());
-	CollisionShape *cshape = memnew(CollisionShape);
-	cshape->set_shape(static_body->get_shape(0));
-	static_body->add_child(cshape);
-	if (get_owner())
 		cshape->set_owner(get_owner());
+	}
 }
 
 void MeshInstance::_notification(int p_what) {
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 718daab75a..7e599ce2f5 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -879,313 +879,105 @@ RigidBody::~RigidBody() {
 //////////////////////////////////////////////////////
 //////////////////////////
 
-Variant KinematicBody::_get_collider() const {
-
-	ObjectID oid = get_collider();
-	if (oid == 0)
-		return Variant();
-	Object *obj = ObjectDB::get_instance(oid);
-	if (!obj)
-		return Variant();
-
-	Reference *ref = obj->cast_to<Reference>();
-	if (ref) {
-		return Ref<Reference>(ref);
-	}
+Dictionary KinematicBody::_move(const Vector3 &p_motion) {
+
+	Collision col;
+	if (move(p_motion, col)) {
+		Dictionary d;
+		d["position"] = col.collision;
+		d["normal"] = col.collision;
+		d["local_shape"] = col.local_shape;
+		d["travel"] = col.travel;
+		d["remainder"] = col.remainder;
+		d["collider_id"] = col.collider;
+		if (col.collider) {
+			d["collider"] = ObjectDB::get_instance(col.collider);
+		} else {
+			d["collider"] = Variant();
+		}
 
-	return obj;
-}
+		d["collider_shape_index"] = col.collider_shape;
+		d["collider_metadata"] = col.collider_metadata;
 
-bool KinematicBody::_ignores_mode(PhysicsServer::BodyMode p_mode) const {
+		return d;
 
-	switch (p_mode) {
-		case PhysicsServer::BODY_MODE_STATIC: return !collide_static;
-		case PhysicsServer::BODY_MODE_KINEMATIC: return !collide_kinematic;
-		case PhysicsServer::BODY_MODE_RIGID: return !collide_rigid;
-		case PhysicsServer::BODY_MODE_CHARACTER: return !collide_character;
+	} else {
+		return Dictionary();
 	}
-
-	return true;
 }
 
-void KinematicBody::revert_motion() {
+bool KinematicBody::move(const Vector3 &p_motion, Collision &r_collision) {
 
 	Transform gt = get_global_transform();
-	gt.origin -= travel; //I do hope this is correct.
-	travel = Vector3();
-	set_global_transform(gt);
-}
-
-Vector3 KinematicBody::get_travel() const {
-
-	return travel;
-}
-
-Vector3 KinematicBody::move(const Vector3 &p_motion) {
-
-	//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(), Vector3());
-	PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
-	ERR_FAIL_COND_V(!dss, Vector3());
-	const int max_shapes = 32;
-	Vector3 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 |= PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
-	if (collide_kinematic)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
-	if (collide_rigid)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
-	if (collide_character)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
-
-	//print_line("motion: "+p_motion+" margin: "+rtos(margin));
-
-	//print_line("margin: "+rtos(margin));
-
-	float m = margin;
-	//m=0.001;
-
-	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), m, sr, max_shapes, res_shapes, exclude, get_collision_layer(), mask)) {
-				collided = true;
-			}
-		}
-
-		if (!collided)
-			break;
-
-		//print_line("have to recover");
-		Vector3 recover_motion;
-		bool all_outside = true;
-		for (int j = 0; j < 8; j++) {
-			for (int i = 0; i < res_shapes; i++) {
-
-				Vector3 a = sr[i * 2 + 0];
-				Vector3 b = sr[i * 2 + 1];
-//print_line(String()+a+" -> "+b);
-#if 0
-				float d = a.distance_to(b);
-
-				/*
-				if (d<margin)
-					continue;
-				*/
-				recover_motion+=(b-a)*0.2;
-#else
-				float dist = a.distance_to(b);
-				if (dist > CMP_EPSILON) {
-					Vector3 norm = (b - a).normalized();
-					if (dist > margin * 0.5)
-						all_outside = false;
-					float adv = norm.dot(recover_motion);
-					//print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv));
-					recover_motion += norm * MAX(dist - adv, 0) * 0.4;
-				}
-#endif
-			}
-		}
-
-		if (recover_motion == Vector3()) {
-			collided = false;
-			break;
-		}
-
-		//print_line("**** RECOVER: "+recover_motion);
-
-		Transform gt = get_global_transform();
-		gt.origin += recover_motion;
-		set_global_transform(gt);
-
-		recover_attempts--;
-
-		if (all_outside)
-			break;
-
-	} while (recover_attempts);
-
-	//move second
-	float safe = 1.0;
-	float unsafe = 1.0;
-	int best_shape = -1;
-
-	PhysicsDirectSpaceState::ShapeRestInfo rest;
-
-	//print_line("pos: "+get_global_transform().origin);
-	//print_line("motion: "+p_motion);
-
-	for (int i = 0; i < get_shape_count(); i++) {
-
-		if (is_shape_set_as_trigger(i))
-			continue;
-
-		float lsafe, lunsafe;
-		PhysicsDirectSpaceState::ShapeRestInfo lrest;
-		bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0, lsafe, lunsafe, exclude, get_collision_layer(), mask, &lrest);
-		//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
-		if (!valid) {
-			safe = 0;
-			unsafe = 0;
-			best_shape = i; //sadly it's the best
-			//print_line("initial stuck");
-
-			break;
-		}
-		if (lsafe == 1.0) {
-			//print_line("initial free");
-			continue;
-		}
-		if (lsafe < safe) {
-
-			//print_line("initial at "+rtos(lsafe));
-			safe = lsafe;
-			safe = MAX(0, lsafe - 0.01);
-			unsafe = lunsafe;
-			best_shape = i;
-			rest = lrest;
-		}
-	}
-
-	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
-
-	if (safe >= 1) {
-		//not collided
-		colliding = false;
-	} else {
-
-		colliding = true;
-
-		if (true || (safe == 0 && unsafe == 0)) { //use it always because it's more precise than GJK
-			//no advance, use rest info from collision
-			Transform ugt = get_global_transform();
-			ugt.origin += p_motion * unsafe;
-
-			PhysicsDirectSpaceState::ShapeRestInfo rest_info;
-			bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt * get_shape_transform(best_shape), m, &rest, exclude, get_collision_layer(), mask);
-			if (!c2) {
-				//should not happen, but floating point precision is so weird..
-				colliding = false;
-			}
-
-			//print_line("Rest Travel: "+rest.normal);
-		}
-
-		if (colliding) {
-
-			collision = rest.point;
-			normal = rest.normal;
-			collider = rest.collider_id;
-			collider_vel = rest.linear_velocity;
-			collider_shape = rest.shape;
-		}
+	PhysicsServer::MotionResult result;
+	bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, margin, &result);
+
+	if (colliding) {
+		r_collision.collider_metadata = result.collider_metadata;
+		r_collision.collider_shape = result.collider_shape;
+		r_collision.collider_vel = result.collider_velocity;
+		r_collision.collision = result.collision_point;
+		r_collision.normal = result.collision_normal;
+		r_collision.collider = result.collider_id;
+		r_collision.travel = result.motion;
+		r_collision.remainder = result.remainder;
+		r_collision.local_shape = result.collision_local_shape;
 	}
 
-	Vector3 motion = p_motion * safe;
-	/*
-	if (colliding)
-		motion+=normal*0.001;
-	*/
-	Transform gt = get_global_transform();
-	gt.origin += motion;
+	gt.origin += result.motion;
 	set_global_transform(gt);
-	travel = motion;
 
-	return p_motion - motion;
+	return colliding;
 }
 
-Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, const Vector3 &p_ceil_direction, float p_slope_stop_min_velocity, int p_max_bounces, float p_floor_max_angle, float p_ceil_max_angle) {
-
-	/*
-	Things to note:
-	1. This function is basically the KinematicBody2D function ported over.
-	2. The 'travel' variable and stuff relating to it exists more or less for this function's sake.
-	3. Someone is going to have to document this, so here's an example for them:
-	vel = move_and_slide(vel, Vector3(0, 1, 0), Vector3(0, -1, 0), 0.1);
-	Very useful for FPS controllers so long as you control horizontal motion properly - even for Quake-style AABB colliders.
-	The slope stop system is... rather weird, and it's correct operation depends on what scale your game is built on,
-	 but as far as I can tell in theory it's suppposed to be a way of turning impassable slopes into invisible walls.
-	It can also be a pain, since there's a better-known way of defining such things: "let gravity do the work".
-	If you don't like it, set it to positive infinity.
-	4. Might be a bug somewhere else in physics: When there are two CollisionShape nodes with a shared Shape, only one is considered, I think.
-	   Test this further.
-	*/
+Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, float p_slope_stop_min_velocity, int p_max_bounces, float p_floor_max_angle) {
 
-	Vector3 motion = (move_and_slide_floor_velocity + p_linear_velocity) * get_fixed_process_delta_time();
+	Vector3 motion = (floor_velocity + p_linear_velocity) * get_fixed_process_delta_time();
 	Vector3 lv = p_linear_velocity;
 
-	move_and_slide_on_floor = false;
-	move_and_slide_on_ceiling = false;
-	move_and_slide_on_wall = false;
-	move_and_slide_colliders.clear();
-	move_and_slide_floor_velocity = Vector3();
+	on_floor = false;
+	on_ceiling = false;
+	on_wall = false;
+	colliders.clear();
+	floor_velocity = Vector3();
 
 	while (p_max_bounces) {
 
-		motion = move(motion);
+		Collision collision;
 
-		if (is_colliding()) {
+		bool collided = move(motion, collision);
 
-			bool hit_horizontal = false; //hit floor or ceiling
+		if (collided) {
 
-			if (p_floor_direction != Vector3()) {
-				if (get_collision_normal().dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor
+			motion = collision.remainder;
 
-					hit_horizontal = true;
-					move_and_slide_on_floor = true;
-					move_and_slide_floor_velocity = get_collider_velocity();
+			if (p_floor_direction == Vector3()) {
+				//all is a wall
+				on_wall = true;
+			} else {
+				if (collision.normal.dot(p_floor_direction) >= Math::cos(p_floor_max_angle)) { //floor
 
-					//Note: These two lines are the only lines that really changed between 3D/2D, see if it can't be reused somehow???
-					Vector2 hz_velocity = Vector2(lv.x - move_and_slide_floor_velocity.x, lv.z - move_and_slide_floor_velocity.z);
-					if (get_travel().length() < 1 && hz_velocity.length() < p_slope_stop_min_velocity) {
-						revert_motion();
-						return Vector3();
-					}
-				}
-			}
+					on_floor = true;
+					floor_velocity = collision.collider_vel;
 
-			if (p_ceil_direction != Vector3()) {
-				if (get_collision_normal().dot(p_ceil_direction) >= Math::cos(p_ceil_max_angle)) { //ceiling
-					hit_horizontal = true;
-					move_and_slide_on_ceiling = true;
+					/*if (collision.travel.length() < 0.01 && ABS((lv.x - floor_velocity.x)) < p_slope_stop_min_velocity) {
+						Transform gt = get_global_transform();
+						gt.elements[2] -= collision.travel;
+						set_global_transform(gt);
+						return Vector3();
+					}*/
+				} else if (collision.normal.dot(-p_floor_direction) >= Math::cos(p_floor_max_angle)) { //ceiling
+					on_ceiling = true;
+				} else {
+					on_wall = true;
 				}
 			}
 
-			//if it hit something but didn't hit a floor or ceiling, it is by default a wall
-			//(this imitates the pre-specifiable-ceiling logic more or less, except ceiling is optional)
-			if (!hit_horizontal) {
-				move_and_slide_on_wall = true;
-			}
-
-			Vector3 n = get_collision_normal();
+			Vector3 n = collision.normal;
 			motion = motion.slide(n);
 			lv = lv.slide(n);
-			Variant collider = _get_collider();
-			if (collider.get_type() != Variant::NIL) {
-				move_and_slide_colliders.push_back(collider);
-			}
+
+			colliders.push_back(collision);
 
 		} else {
 			break;
@@ -1199,199 +991,148 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 	return lv;
 }
 
-bool KinematicBody::is_move_and_slide_on_floor() const {
-
-	return move_and_slide_on_floor;
-}
-bool KinematicBody::is_move_and_slide_on_wall() const {
+bool KinematicBody::is_on_floor() const {
 
-	return move_and_slide_on_wall;
+	return on_floor;
 }
-bool KinematicBody::is_move_and_slide_on_ceiling() const {
+bool KinematicBody::is_on_wall() const {
 
-	return move_and_slide_on_ceiling;
+	return on_wall;
 }
-Array KinematicBody::get_move_and_slide_colliders() const {
+bool KinematicBody::is_on_ceiling() const {
 
-	return move_and_slide_colliders;
+	return on_ceiling;
 }
 
-Vector3 KinematicBody::move_to(const Vector3 &p_position) {
+Vector3 KinematicBody::get_floor_velocity() const {
 
-	return move(p_position - get_global_transform().origin);
+	return floor_velocity;
 }
 
-bool KinematicBody::can_teleport_to(const Vector3 &p_position) {
+bool KinematicBody::test_move(const Transform &p_from, const Vector3 &p_motion) {
 
 	ERR_FAIL_COND_V(!is_inside_tree(), false);
-	PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
-	ERR_FAIL_COND_V(!dss, false);
-
-	uint32_t mask = 0;
-	if (collide_static)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
-	if (collide_kinematic)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
-	if (collide_rigid)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
-	if (collide_character)
-		mask |= PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
-
-	Transform xform = get_global_transform();
-	xform.origin = p_position;
-
-	Set<RID> exclude;
-	exclude.insert(get_rid());
-
-	for (int i = 0; i < get_shape_count(); i++) {
-
-		if (is_shape_set_as_trigger(i))
-			continue;
-
-		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i), 0, NULL, 1, exclude, get_collision_layer(), mask);
-		if (col)
-			return false;
-	}
 
-	return true;
+	return PhysicsServer::get_singleton()->body_test_motion(get_rid(), p_from, p_motion, margin);
 }
 
-bool KinematicBody::is_colliding() const {
+void KinematicBody::set_safe_margin(float p_margin) {
 
-	ERR_FAIL_COND_V(!is_inside_tree(), false);
-
-	return colliding;
+	margin = p_margin;
 }
-Vector3 KinematicBody::get_collision_pos() const {
 
-	ERR_FAIL_COND_V(!colliding, Vector3());
-	return collision;
-}
-Vector3 KinematicBody::get_collision_normal() const {
+float KinematicBody::get_safe_margin() const {
 
-	ERR_FAIL_COND_V(!colliding, Vector3());
-	return normal;
+	return margin;
 }
 
-Vector3 KinematicBody::get_collider_velocity() const {
+int KinematicBody::get_collision_count() const {
 
-	return collider_vel;
+	return colliders.size();
 }
+Vector3 KinematicBody::get_collision_position(int p_collision) const {
 
-ObjectID KinematicBody::get_collider() const {
-
-	ERR_FAIL_COND_V(!colliding, 0);
-	return collider;
-}
-int KinematicBody::get_collider_shape() const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
 
-	ERR_FAIL_COND_V(!colliding, -1);
-	return collider_shape;
+	return colliders[p_collision].collision;
 }
-void KinematicBody::set_collide_with_static_bodies(bool p_enable) {
-
-	collide_static = p_enable;
+Vector3 KinematicBody::get_collision_normal(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
+	return colliders[p_collision].normal;
 }
-bool KinematicBody::can_collide_with_static_bodies() const {
 
-	return collide_static;
+Vector3 KinematicBody::get_collision_travel(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
+	return colliders[p_collision].travel;
 }
-
-void KinematicBody::set_collide_with_rigid_bodies(bool p_enable) {
-
-	collide_rigid = p_enable;
+Vector3 KinematicBody::get_collision_remainder(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
+	return colliders[p_collision].remainder;
 }
-bool KinematicBody::can_collide_with_rigid_bodies() const {
-
-	return collide_rigid;
+Object *KinematicBody::get_collision_local_shape(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
+	uint32_t owner = shape_find_owner(colliders[p_collision].local_shape);
+	return shape_owner_get_owner(owner);
 }
+Object *KinematicBody::get_collision_collider(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
 
-void KinematicBody::set_collide_with_kinematic_bodies(bool p_enable) {
+	if (colliders[p_collision].collider) {
+		return ObjectDB::get_instance(colliders[p_collision].collider);
+	}
 
-	collide_kinematic = p_enable;
+	return NULL;
 }
-bool KinematicBody::can_collide_with_kinematic_bodies() const {
+ObjectID KinematicBody::get_collision_collider_id(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), 0);
 
-	return collide_kinematic;
+	return colliders[p_collision].collider;
 }
+Object *KinematicBody::get_collision_collider_shape(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), NULL);
+	Object *collider = get_collision_collider(p_collision);
+	if (collider) {
+		CollisionObject *obj2d = collider->cast_to<CollisionObject>();
+		if (obj2d) {
+			uint32_t owner = shape_find_owner(colliders[p_collision].collider_shape);
+			return obj2d->shape_owner_get_owner(owner);
+		}
+	}
 
-void KinematicBody::set_collide_with_character_bodies(bool p_enable) {
-
-	collide_character = p_enable;
+	return NULL;
 }
-bool KinematicBody::can_collide_with_character_bodies() const {
-
-	return collide_character;
+int KinematicBody::get_collision_collider_shape_index(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), -1);
+	return colliders[p_collision].collider_shape;
 }
-
-void KinematicBody::set_collision_margin(float p_margin) {
-
-	margin = p_margin;
+Vector3 KinematicBody::get_collision_collider_velocity(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Vector3());
+	return colliders[p_collision].collider_vel;
 }
-
-float KinematicBody::get_collision_margin() const {
-
-	return margin;
+Variant KinematicBody::get_collision_collider_metadata(int p_collision) const {
+	ERR_FAIL_INDEX_V(p_collision, colliders.size(), Variant());
+	return colliders[p_collision].collider_metadata;
 }
 
 void KinematicBody::_bind_methods() {
 
-	ClassDB::bind_method(D_METHOD("move", "rel_vec"), &KinematicBody::move);
-	ClassDB::bind_method(D_METHOD("move_to", "position"), &KinematicBody::move_to);
-	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "ceil_normal", "slope_stop_min_velocity", "max_bounces", "floor_max_angle", "ceil_max_angle"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(Vector3(0, 0, 0)), DEFVAL(5), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(Math::deg2rad((float)45)));
-
-	ClassDB::bind_method(D_METHOD("can_teleport_to", "position"), &KinematicBody::can_teleport_to);
+	ClassDB::bind_method(D_METHOD("move", "rel_vec"), &KinematicBody::_move);
+	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "floor_normal", "slope_stop_min_velocity", "max_bounces", "floor_max_angle"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(0.05), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)));
 
-	ClassDB::bind_method(D_METHOD("is_colliding"), &KinematicBody::is_colliding);
+	ClassDB::bind_method(D_METHOD("test_move", "from", "rel_vec"), &KinematicBody::test_move);
 
-	ClassDB::bind_method(D_METHOD("get_collision_pos"), &KinematicBody::get_collision_pos);
-	ClassDB::bind_method(D_METHOD("get_collision_normal"), &KinematicBody::get_collision_normal);
-	ClassDB::bind_method(D_METHOD("get_collider_velocity"), &KinematicBody::get_collider_velocity);
-	ClassDB::bind_method(D_METHOD("get_collider:Variant"), &KinematicBody::_get_collider);
-	ClassDB::bind_method(D_METHOD("get_collider_shape"), &KinematicBody::get_collider_shape);
+	ClassDB::bind_method(D_METHOD("is_on_floor"), &KinematicBody::is_on_floor);
+	ClassDB::bind_method(D_METHOD("is_on_ceiling"), &KinematicBody::is_on_ceiling);
+	ClassDB::bind_method(D_METHOD("is_on_wall"), &KinematicBody::is_on_wall);
+	ClassDB::bind_method(D_METHOD("get_floor_velocity"), &KinematicBody::get_floor_velocity);
 
-	ClassDB::bind_method(D_METHOD("set_collide_with_static_bodies", "enable"), &KinematicBody::set_collide_with_static_bodies);
-	ClassDB::bind_method(D_METHOD("can_collide_with_static_bodies"), &KinematicBody::can_collide_with_static_bodies);
+	ClassDB::bind_method(D_METHOD("set_safe_margin", "pixels"), &KinematicBody::set_safe_margin);
+	ClassDB::bind_method(D_METHOD("get_safe_margin", "pixels"), &KinematicBody::get_safe_margin);
 
-	ClassDB::bind_method(D_METHOD("set_collide_with_kinematic_bodies", "enable"), &KinematicBody::set_collide_with_kinematic_bodies);
-	ClassDB::bind_method(D_METHOD("can_collide_with_kinematic_bodies"), &KinematicBody::can_collide_with_kinematic_bodies);
+	ClassDB::bind_method(D_METHOD("get_collision_count"), &KinematicBody::get_collision_count);
+	ClassDB::bind_method(D_METHOD("get_collision_position", "collision"), &KinematicBody::get_collision_position);
+	ClassDB::bind_method(D_METHOD("get_collision_normal", "collision"), &KinematicBody::get_collision_normal);
+	ClassDB::bind_method(D_METHOD("get_collision_travel", "collision"), &KinematicBody::get_collision_travel);
+	ClassDB::bind_method(D_METHOD("get_collision_remainder", "collision"), &KinematicBody::get_collision_remainder);
+	ClassDB::bind_method(D_METHOD("get_collision_local_shape", "collision"), &KinematicBody::get_collision_local_shape);
+	ClassDB::bind_method(D_METHOD("get_collision_collider", "collision"), &KinematicBody::get_collision_collider);
+	ClassDB::bind_method(D_METHOD("get_collision_collider_id", "collision"), &KinematicBody::get_collision_collider_id);
+	ClassDB::bind_method(D_METHOD("get_collision_collider_shape", "collision"), &KinematicBody::get_collision_collider_shape);
+	ClassDB::bind_method(D_METHOD("get_collision_collider_shape_index", "collision"), &KinematicBody::get_collision_collider_shape_index);
+	ClassDB::bind_method(D_METHOD("get_collision_collider_velocity", "collision"), &KinematicBody::get_collision_collider_velocity);
+	ClassDB::bind_method(D_METHOD("get_collision_collider_metadata", "collision"), &KinematicBody::get_collision_collider_metadata);
 
-	ClassDB::bind_method(D_METHOD("set_collide_with_rigid_bodies", "enable"), &KinematicBody::set_collide_with_rigid_bodies);
-	ClassDB::bind_method(D_METHOD("can_collide_with_rigid_bodies"), &KinematicBody::can_collide_with_rigid_bodies);
-
-	ClassDB::bind_method(D_METHOD("set_collide_with_character_bodies", "enable"), &KinematicBody::set_collide_with_character_bodies);
-	ClassDB::bind_method(D_METHOD("can_collide_with_character_bodies"), &KinematicBody::can_collide_with_character_bodies);
-
-	ClassDB::bind_method(D_METHOD("set_collision_margin", "pixels"), &KinematicBody::set_collision_margin);
-	ClassDB::bind_method(D_METHOD("get_collision_margin", "pixels"), &KinematicBody::get_collision_margin);
-
-	ClassDB::bind_method(D_METHOD("get_travel"), &KinematicBody::get_travel);
-	ClassDB::bind_method(D_METHOD("revert_motion"), &KinematicBody::revert_motion);
-
-	ClassDB::bind_method(D_METHOD("get_move_and_slide_colliders"), &KinematicBody::get_move_and_slide_colliders);
-	ClassDB::bind_method(D_METHOD("is_move_and_slide_on_floor"), &KinematicBody::is_move_and_slide_on_floor);
-	ClassDB::bind_method(D_METHOD("is_move_and_slide_on_ceiling"), &KinematicBody::is_move_and_slide_on_ceiling);
-	ClassDB::bind_method(D_METHOD("is_move_and_slide_on_wall"), &KinematicBody::is_move_and_slide_on_wall);
-
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with/static"), "set_collide_with_static_bodies", "can_collide_with_static_bodies");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with/kinematic"), "set_collide_with_kinematic_bodies", "can_collide_with_kinematic_bodies");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with/rigid"), "set_collide_with_rigid_bodies", "can_collide_with_rigid_bodies");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "collide_with/character"), "set_collide_with_character_bodies", "can_collide_with_character_bodies");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_collision_margin", "get_collision_margin");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
 
 KinematicBody::KinematicBody()
 	: PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC) {
 
-	collide_static = true;
-	collide_rigid = true;
-	collide_kinematic = true;
-	collide_character = true;
-
-	colliding = false;
-	collider = 0;
 	margin = 0.001;
-	collider_shape = 0;
+
+	on_floor = false;
+	on_ceiling = false;
+	on_wall = false;
 }
 KinematicBody::~KinematicBody() {
 }
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index db4147f8f6..f86d7d957f 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -263,75 +263,60 @@ class KinematicBody : public PhysicsBody {
 
 	GDCLASS(KinematicBody, PhysicsBody);
 
-	float margin;
-	bool collide_static;
-	bool collide_rigid;
-	bool collide_kinematic;
-	bool collide_character;
-
-	bool colliding;
-	Vector3 collision;
-	Vector3 normal;
-	Vector3 collider_vel;
-	ObjectID collider;
-	int collider_shape;
-	Vector3 travel;
-
-	Vector3 move_and_slide_floor_velocity;
-	bool move_and_slide_on_floor;
-	bool move_and_slide_on_ceiling;
-	bool move_and_slide_on_wall;
-	Array move_and_slide_colliders;
-
-	Variant _get_collider() const;
-
-	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
-
-protected:
-	static void _bind_methods();
-
 public:
-	enum {
-		SLIDE_FLAG_FLOOR,
-		SLIDE_FLAG_WALL,
-		SLIDE_FLAG_ROOF
+	struct Collision {
+		Vector3 collision;
+		Vector3 normal;
+		Vector3 collider_vel;
+		ObjectID collider;
+		int collider_shape;
+		Variant collider_metadata;
+		Vector3 remainder;
+		Vector3 travel;
+		int local_shape;
 	};
 
-	Vector3 move(const Vector3 &p_motion);
-	Vector3 move_to(const Vector3 &p_position);
-
-	bool can_teleport_to(const Vector3 &p_position);
-	bool is_colliding() const;
-
-	Vector3 get_travel() const; // Set by move and others. Consider unreliable except immediately after a move call.
-	void revert_motion();
-
-	Vector3 get_collision_pos() const;
-	Vector3 get_collision_normal() const;
-	Vector3 get_collider_velocity() const;
-	ObjectID get_collider() const;
-	int get_collider_shape() const;
-
-	void set_collide_with_static_bodies(bool p_enable);
-	bool can_collide_with_static_bodies() const;
+private:
+	float margin;
 
-	void set_collide_with_rigid_bodies(bool p_enable);
-	bool can_collide_with_rigid_bodies() const;
+	Vector3 floor_velocity;
+	bool on_floor;
+	bool on_ceiling;
+	bool on_wall;
+	Vector<Collision> colliders;
 
-	void set_collide_with_kinematic_bodies(bool p_enable);
-	bool can_collide_with_kinematic_bodies() const;
+	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
 
-	void set_collide_with_character_bodies(bool p_enable);
-	bool can_collide_with_character_bodies() const;
+	Dictionary _move(const Vector3 &p_motion);
 
-	void set_collision_margin(float p_margin);
-	float get_collision_margin() const;
+protected:
+	static void _bind_methods();
 
-	Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), const Vector3 &p_ceil_direction = Vector3(0, 0, 0), float p_slope_stop_min_velocity = 5, int p_max_bounces = 4, float p_floor_max_angle = Math::deg2rad((float)45), float p_ceil_max_angle = Math::deg2rad((float)45));
-	bool is_move_and_slide_on_floor() const;
-	bool is_move_and_slide_on_wall() const;
-	bool is_move_and_slide_on_ceiling() const;
-	Array get_move_and_slide_colliders() const;
+public:
+	bool move(const Vector3 &p_motion, Collision &r_collision);
+	bool test_move(const Transform &p_from, const Vector3 &p_motion);
+
+	void set_safe_margin(float p_margin);
+	float get_safe_margin() const;
+
+	Vector3 move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction = Vector3(0, 0, 0), float p_slope_stop_min_velocity = 0.05, int p_max_bounces = 4, float p_floor_max_angle = Math::deg2rad((float)45));
+	bool is_on_floor() const;
+	bool is_on_wall() const;
+	bool is_on_ceiling() const;
+	Vector3 get_floor_velocity() const;
+
+	int get_collision_count() const;
+	Vector3 get_collision_position(int p_collision) const;
+	Vector3 get_collision_normal(int p_collision) const;
+	Vector3 get_collision_travel(int p_collision) const;
+	Vector3 get_collision_remainder(int p_collision) const;
+	Object *get_collision_local_shape(int p_collision) const;
+	Object *get_collision_collider(int p_collision) const;
+	ObjectID get_collision_collider_id(int p_collision) const;
+	Object *get_collision_collider_shape(int p_collision) const;
+	int get_collision_collider_shape_index(int p_collision) const;
+	Vector3 get_collision_collider_velocity(int p_collision) const;
+	Variant get_collision_collider_metadata(int p_collision) const;
 
 	KinematicBody();
 	~KinematicBody();
diff --git a/scene/3d/reflection_probe.cpp b/scene/3d/reflection_probe.cpp
index 86a62bec97..0dc5b58e45 100644
--- a/scene/3d/reflection_probe.cpp
+++ b/scene/3d/reflection_probe.cpp
@@ -162,7 +162,7 @@ bool ReflectionProbe::are_shadows_enabled() const {
 void ReflectionProbe::set_cull_mask(uint32_t p_layers) {
 
 	cull_mask = p_layers;
-	VS::get_singleton()->reflection_probe_set_enable_shadows(probe, p_layers);
+	VS::get_singleton()->reflection_probe_set_cull_mask(probe, p_layers);
 }
 uint32_t ReflectionProbe::get_cull_mask() const {
 
diff --git a/scene/3d/remote_transform.cpp b/scene/3d/remote_transform.cpp
index b36f444bea..4dcfb5f94e 100644
--- a/scene/3d/remote_transform.cpp
+++ b/scene/3d/remote_transform.cpp
@@ -63,7 +63,45 @@ void RemoteTransform::_update_remote() {
 		return;
 
 	//todo make faster
-	n->set_global_transform(get_global_transform());
+	if (use_global_coordinates) {
+
+		if (update_remote_position && update_remote_rotation && update_remote_scale) {
+			n->set_global_transform(get_global_transform());
+		} else {
+			Transform n_trans = n->get_global_transform();
+			Transform our_trans = get_global_transform();
+
+			if (!update_remote_position)
+				our_trans.set_origin(n_trans.get_origin());
+
+			n->set_global_transform(our_trans);
+
+			if (!update_remote_rotation)
+				n->set_rotation(n_trans.basis.get_rotation());
+
+			if (!update_remote_scale)
+				n->set_scale(n_trans.basis.get_scale());
+		}
+
+	} else {
+		if (update_remote_position && update_remote_rotation && update_remote_scale) {
+			n->set_global_transform(get_global_transform());
+		} else {
+			Transform n_trans = n->get_transform();
+			Transform our_trans = get_transform();
+
+			if (!update_remote_position)
+				our_trans.set_origin(n_trans.get_origin());
+
+			n->set_transform(our_trans);
+
+			if (!update_remote_rotation)
+				n->set_rotation(n_trans.basis.get_rotation());
+
+			if (!update_remote_scale)
+				n->set_scale(n_trans.basis.get_scale());
+		}
+	}
 }
 
 void RemoteTransform::_notification(int p_what) {
@@ -102,6 +140,41 @@ NodePath RemoteTransform::get_remote_node() const {
 	return remote_node;
 }
 
+void RemoteTransform::set_use_global_coordinates(const bool p_enable) {
+	use_global_coordinates = p_enable;
+}
+
+bool RemoteTransform::get_use_global_coordinates() const {
+	return use_global_coordinates;
+}
+
+void RemoteTransform::set_update_position(const bool p_update) {
+	update_remote_position = p_update;
+	_update_remote();
+}
+
+bool RemoteTransform::get_update_position() const {
+	return update_remote_position;
+}
+
+void RemoteTransform::set_update_rotation(const bool p_update) {
+	update_remote_rotation = p_update;
+	_update_remote();
+}
+
+bool RemoteTransform::get_update_rotation() const {
+	return update_remote_rotation;
+}
+
+void RemoteTransform::set_update_scale(const bool p_update) {
+	update_remote_scale = p_update;
+	_update_remote();
+}
+
+bool RemoteTransform::get_update_scale() const {
+	return update_remote_scale;
+}
+
 String RemoteTransform::get_configuration_warning() const {
 
 	if (!has_node(remote_node) || !get_node(remote_node) || !get_node(remote_node)->cast_to<Spatial>()) {
@@ -116,11 +189,32 @@ void RemoteTransform::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_remote_node", "path"), &RemoteTransform::set_remote_node);
 	ClassDB::bind_method(D_METHOD("get_remote_node"), &RemoteTransform::get_remote_node);
 
+	ClassDB::bind_method(D_METHOD("set_use_global_coordinates", "use_global_coordinates"), &RemoteTransform::set_use_global_coordinates);
+	ClassDB::bind_method(D_METHOD("get_use_global_coordinates"), &RemoteTransform::get_use_global_coordinates);
+
+	ClassDB::bind_method(D_METHOD("set_update_position", "update_remote_position"), &RemoteTransform::set_update_position);
+	ClassDB::bind_method(D_METHOD("get_update_position"), &RemoteTransform::get_update_position);
+	ClassDB::bind_method(D_METHOD("set_update_rotation", "update_remote_rotation"), &RemoteTransform::set_update_rotation);
+	ClassDB::bind_method(D_METHOD("get_update_rotation"), &RemoteTransform::get_update_rotation);
+	ClassDB::bind_method(D_METHOD("set_update_scale", "update_remote_scale"), &RemoteTransform::set_update_scale);
+	ClassDB::bind_method(D_METHOD("get_update_scale"), &RemoteTransform::get_update_scale);
+
 	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "remote_path"), "set_remote_node", "get_remote_node");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_global_coordinates"), "set_use_global_coordinates", "get_use_global_coordinates");
+
+	ADD_GROUP("Update", "update_");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "update_position"), "set_update_position", "get_update_position");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "update_rotation"), "set_update_rotation", "get_update_rotation");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "update_scale"), "set_update_scale", "get_update_scale");
 }
 
 RemoteTransform::RemoteTransform() {
 
+	use_global_coordinates = true;
+	update_remote_position = true;
+	update_remote_rotation = true;
+	update_remote_scale = true;
+
 	cache = 0;
 	set_notify_transform(true);
 }
diff --git a/scene/3d/remote_transform.h b/scene/3d/remote_transform.h
index 9fa3011185..c25b57966e 100644
--- a/scene/3d/remote_transform.h
+++ b/scene/3d/remote_transform.h
@@ -39,6 +39,11 @@ class RemoteTransform : public Spatial {
 
 	ObjectID cache;
 
+	bool use_global_coordinates;
+	bool update_remote_position;
+	bool update_remote_rotation;
+	bool update_remote_scale;
+
 	void _update_remote();
 	void _update_cache();
 
@@ -50,6 +55,18 @@ public:
 	void set_remote_node(const NodePath &p_remote_node);
 	NodePath get_remote_node() const;
 
+	void set_use_global_coordinates(const bool p_enable);
+	bool get_use_global_coordinates() const;
+
+	void set_update_position(const bool p_update);
+	bool get_update_position() const;
+
+	void set_update_rotation(const bool p_update);
+	bool get_update_rotation() const;
+
+	void set_update_scale(const bool p_update);
+	bool get_update_scale() const;
+
 	virtual String get_configuration_warning() const;
 
 	RemoteTransform();
diff --git a/scene/3d/spatial_velocity_tracker.cpp b/scene/3d/spatial_velocity_tracker.cpp
new file mode 100644
index 0000000000..dc822d0446
--- /dev/null
+++ b/scene/3d/spatial_velocity_tracker.cpp
@@ -0,0 +1,104 @@
+#include "spatial_velocity_tracker.h"
+#include "engine.h"
+
+void SpatialVelocityTracker::set_track_fixed_step(bool p_track_fixed_step) {
+
+	fixed_step = p_track_fixed_step;
+}
+
+bool SpatialVelocityTracker::is_tracking_fixed_step() const {
+
+	return fixed_step;
+}
+void SpatialVelocityTracker::update_position(const Vector3 &p_position) {
+
+	PositionHistory ph;
+	ph.position = p_position;
+	if (fixed_step) {
+		ph.frame = Engine::get_singleton()->get_fixed_frames();
+	} else {
+		ph.frame = Engine::get_singleton()->get_idle_frame_ticks();
+	}
+
+	if (position_history_len == 0 || position_history[0].frame != ph.frame) { //in same frame, use latest
+		position_history_len = MIN(position_history.size(), position_history_len + 1);
+		for (int i = position_history_len - 1; i > 0; i--) {
+			position_history[i] = position_history[i - 1];
+		}
+	}
+
+	position_history[0] = ph;
+}
+Vector3 SpatialVelocityTracker::get_tracked_linear_velocity() const {
+
+	Vector3 linear_velocity;
+
+	float max_time = 1 / 5.0; //maximum time to interpolate a velocity
+
+	Vector3 distance_accum;
+	float time_accum = 0.0;
+	float base_time = 0.0;
+
+	if (position_history_len) {
+		if (fixed_step) {
+			uint64_t base = Engine::get_singleton()->get_fixed_frames();
+			base_time = float(base - position_history[0].frame) / Engine::get_singleton()->get_iterations_per_second();
+		} else {
+			uint64_t base = Engine::get_singleton()->get_idle_frame_ticks();
+			base_time = double(base - position_history[0].frame) / 1000000.0;
+		}
+	}
+
+	for (int i = 0; i < position_history_len - 1; i++) {
+		float delta = 0.0;
+		uint64_t diff = position_history[i].frame - position_history[i + 1].frame;
+		Vector3 distance = position_history[i].position - position_history[i + 1].position;
+
+		if (fixed_step) {
+			delta = float(diff) / Engine::get_singleton()->get_iterations_per_second();
+		} else {
+			delta = double(diff) / 1000000.0;
+		}
+
+		if (base_time + time_accum + delta > max_time)
+			break;
+
+		distance_accum += distance;
+		time_accum += delta;
+	}
+
+	if (time_accum) {
+		linear_velocity = distance_accum / time_accum;
+	}
+
+	return linear_velocity;
+}
+
+void SpatialVelocityTracker::reset(const Vector3 &p_new_pos) {
+
+	PositionHistory ph;
+	ph.position = p_new_pos;
+	if (fixed_step) {
+		ph.frame = Engine::get_singleton()->get_fixed_frames();
+	} else {
+		ph.frame = Engine::get_singleton()->get_idle_frame_ticks();
+	}
+
+	position_history[0] = ph;
+	position_history_len = 1;
+}
+
+void SpatialVelocityTracker::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_track_fixed_step", "enable"), &SpatialVelocityTracker::set_track_fixed_step);
+	ClassDB::bind_method(D_METHOD("is_tracking_fixed_step"), &SpatialVelocityTracker::is_tracking_fixed_step);
+	ClassDB::bind_method(D_METHOD("update_position", "position"), &SpatialVelocityTracker::update_position);
+	ClassDB::bind_method(D_METHOD("get_tracked_linear_velocity"), &SpatialVelocityTracker::get_tracked_linear_velocity);
+	ClassDB::bind_method(D_METHOD("reset", "position"), &SpatialVelocityTracker::reset);
+}
+
+SpatialVelocityTracker::SpatialVelocityTracker() {
+	position_history.resize(4); // should be configurable
+	position_history_len = 0;
+	fixed_step = false;
+}
diff --git a/scene/3d/spatial_velocity_tracker.h b/scene/3d/spatial_velocity_tracker.h
new file mode 100644
index 0000000000..65f3eaca93
--- /dev/null
+++ b/scene/3d/spatial_velocity_tracker.h
@@ -0,0 +1,31 @@
+#ifndef SPATIAL_VELOCITY_TRACKER_H
+#define SPATIAL_VELOCITY_TRACKER_H
+
+#include "scene/3d/spatial.h"
+
+class SpatialVelocityTracker : public Reference {
+	GDCLASS(SpatialVelocityTracker, Reference)
+
+	struct PositionHistory {
+		uint64_t frame;
+		Vector3 position;
+	};
+
+	bool fixed_step;
+	Vector<PositionHistory> position_history;
+	int position_history_len;
+
+protected:
+	static void _bind_methods();
+
+public:
+	void reset(const Vector3 &p_new_pos);
+	void set_track_fixed_step(bool p_use_fixed_step);
+	bool is_tracking_fixed_step() const;
+	void update_position(const Vector3 &p_position);
+	Vector3 get_tracked_linear_velocity() const;
+
+	SpatialVelocityTracker();
+};
+
+#endif // SPATIAL_VELOCITY_TRACKER_H