Lots of work on Audio & Physics engine:

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

SConstruct

@@ -210,8 +210,8 @@ if (env_base['target'] == 'debug'):
     env_base.Append(CPPFLAGS=['-DDEBUG_MEMORY_ALLOC'])
     env_base.Append(CPPFLAGS=['-DSCI_NAMESPACE'])
 
-if (env_base['deprecated'] != 'no'):
-    env_base.Append(CPPFLAGS=['-DENABLE_DEPRECATED'])
+if (env_base['deprecated'] == 'no'):
+    env_base.Append(CPPFLAGS=['-DDISABLE_DEPRECATED'])
 
 env_base.platforms = {}
 

core/engine.cpp

@@ -119,4 +119,6 @@ Engine::Engine() {
 	_fixed_frames = 0;
 	_idle_frames = 0;
 	_in_fixed = false;
+	_frame_ticks = 0;
+	_frame_step = 0;
 }

core/engine.h

@@ -42,6 +42,8 @@ class Engine {
 	String _custom_level;
 	uint64_t frames_drawn;
 	uint32_t _frame_delay;
+	uint64_t _frame_ticks;
+	float _frame_step;
 
 	int ips;
 	float _fps;
@@ -72,6 +74,8 @@ public:
 	uint64_t get_fixed_frames() const { return _fixed_frames; }
 	uint64_t get_idle_frames() const { return _idle_frames; }
 	bool is_in_fixed_frame() const { return _in_fixed; }
+	uint64_t get_idle_frame_ticks() const { return _frame_ticks; }
+	float get_idle_frame_step() const { return _frame_step; }
 
 	void set_time_scale(float p_scale);
 	float get_time_scale() const;

core/io/resource_format_binary.cpp

@@ -29,6 +29,7 @@
 /*************************************************************************/
 #include "resource_format_binary.h"
 #include "global_config.h"
+#include "image.h"
 #include "io/file_access_compressed.h"
 #include "io/marshalls.h"
 #include "os/dir_access.h"
@@ -54,7 +55,6 @@ enum {
 	VARIANT_TRANSFORM = 17,
 	VARIANT_MATRIX32 = 18,
 	VARIANT_COLOR = 20,
-	//VARIANT_IMAGE = 21, - no longer variant type
 	VARIANT_NODE_PATH = 22,
 	VARIANT_RID = 23,
 	VARIANT_OBJECT = 24,
@@ -70,7 +70,13 @@ enum {
 	VARIANT_VECTOR2_ARRAY = 37,
 	VARIANT_INT64 = 40,
 	VARIANT_DOUBLE = 41,
-
+#ifndef DISABLE_DEPRECATED
+	VARIANT_IMAGE = 21, // - no longer variant type
+	IMAGE_ENCODING_EMPTY = 0,
+	IMAGE_ENCODING_RAW = 1,
+	IMAGE_ENCODING_LOSSLESS = 2,
+	IMAGE_ENCODING_LOSSY = 3,
+#endif
 	OBJECT_EMPTY = 0,
 	OBJECT_EXTERNAL_RESOURCE = 1,
 	OBJECT_INTERNAL_RESOURCE = 2,
@@ -541,7 +547,69 @@ Error ResourceInteractiveLoaderBinary::parse_variant(Variant &r_v) {
 			w = PoolVector<Color>::Write();
 			r_v = array;
 		} break;
+#ifndef DISABLE_DEPRECATED
+		case VARIANT_IMAGE: {
+			uint32_t encoding = f->get_32();
+			if (encoding == IMAGE_ENCODING_EMPTY) {
+				r_v = Ref<Image>();
+				break;
+			} else if (encoding == IMAGE_ENCODING_RAW) {
+				uint32_t width = f->get_32();
+				uint32_t height = f->get_32();
+				uint32_t mipmaps = f->get_32();
+				uint32_t format = f->get_32();
+				const uint32_t format_version_shift = 24;
+				const uint32_t format_version_mask = format_version_shift - 1;
+
+				uint32_t format_version = format >> format_version_shift;
+
+				const uint32_t current_version = 0;
+				if (format_version > current_version) {
+
+					ERR_PRINT("Format version for encoded binary image is too new");
+					return ERR_PARSE_ERROR;
+				}
+
+				Image::Format fmt = Image::Format(format & format_version_mask); //if format changes, we can add a compatibility bit on top
+
+				uint32_t datalen = f->get_32();
+
+				PoolVector<uint8_t> imgdata;
+				imgdata.resize(datalen);
+				PoolVector<uint8_t>::Write w = imgdata.write();
+				f->get_buffer(w.ptr(), datalen);
+				_advance_padding(datalen);
+				w = PoolVector<uint8_t>::Write();
+
+				Ref<Image> image;
+				image.instance();
+				image->create(width, height, mipmaps, fmt, imgdata);
+				r_v = image;
+
+			} else {
+				//compressed
+				PoolVector<uint8_t> data;
+				data.resize(f->get_32());
+				PoolVector<uint8_t>::Write w = data.write();
+				f->get_buffer(w.ptr(), data.size());
+				w = PoolVector<uint8_t>::Write();
+
+				Ref<Image> image;
 
+				if (encoding == IMAGE_ENCODING_LOSSY && Image::lossy_unpacker) {
+
+					image = Image::lossy_unpacker(data);
+				} else if (encoding == IMAGE_ENCODING_LOSSLESS && Image::lossless_unpacker) {
+
+					image = Image::lossless_unpacker(data);
+				}
+				_advance_padding(data.size());
+
+				r_v = image;
+			}
+
+		} break;
+#endif
 		default: {
 			ERR_FAIL_V(ERR_FILE_CORRUPT);
 		} break;
@@ -1644,7 +1712,6 @@ void ResourceFormatSaverBinaryInstance::_find_resources(const Variant &p_variant
 			get_string_index(np.get_property());
 
 		} break;
-
 		default: {}
 	}
 }

core/math/audio_frame.h

@@ -102,6 +102,16 @@ struct AudioFrame {
 		r = ::undenormalise(r);
 	}
 
+	_FORCE_INLINE_ AudioFrame linear_interpolate(const AudioFrame &p_b, float p_t) const {
+
+		AudioFrame res = *this;
+
+		res.l += (p_t * (p_b.l - l));
+		res.r += (p_t * (p_b.r - r));
+
+		return res;
+	}
+
 	_ALWAYS_INLINE_ AudioFrame(float p_l, float p_r) {
 		l = p_l;
 		r = p_r;

core/math/vector3.h

@@ -100,6 +100,7 @@ struct Vector3 {
 
 	_FORCE_INLINE_ Vector3 abs() const;
 	_FORCE_INLINE_ Vector3 floor() const;
+	_FORCE_INLINE_ Vector3 sign() const;
 	_FORCE_INLINE_ Vector3 ceil() const;
 
 	_FORCE_INLINE_ real_t distance_to(const Vector3 &p_b) const;
@@ -187,6 +188,11 @@ Vector3 Vector3::abs() const {
 	return Vector3(Math::abs(x), Math::abs(y), Math::abs(z));
 }
 
+Vector3 Vector3::sign() const {
+
+	return Vector3(SGN(x), SGN(y), SGN(z));
+}
+
 Vector3 Vector3::floor() const {
 
 	return Vector3(Math::floor(x), Math::floor(y), Math::floor(z));

drivers/gles3/rasterizer_scene_gles3.cpp

@@ -4199,7 +4199,7 @@ void RasterizerSceneGLES3::render_scene(const Transform &p_cam_transform, const
 	glEnable(GL_DEPTH_TEST);
 	glDisable(GL_SCISSOR_TEST);
 
-	render_list.sort_by_depth(true);
+	render_list.sort_by_reverse_depth(true);
 
 	if (state.directional_light_count == 0) {
 		directional_light = NULL;

drivers/gles3/rasterizer_scene_gles3.h

@@ -666,7 +666,7 @@ public:
 			uint64_t sort_key;
 		};
 
-		Element *_elements;
+		Element *base_elements;
 		Element **elements;
 
 		int element_count;
@@ -700,11 +700,11 @@ public:
 		struct SortByDepth {
 
 			_FORCE_INLINE_ bool operator()(const Element *A, const Element *B) const {
-				return A->instance->depth > B->instance->depth;
+				return A->instance->depth < B->instance->depth;
 			}
 		};
 
-		void sort_by_depth(bool p_alpha) {
+		void sort_by_depth(bool p_alpha) { //used for shadows
 
 			SortArray<Element *, SortByDepth> sorter;
 			if (p_alpha) {
@@ -714,11 +714,28 @@ public:
 			}
 		}
 
+		struct SortByReverseDepth {
+
+			_FORCE_INLINE_ bool operator()(const Element *A, const Element *B) const {
+				return A->instance->depth > B->instance->depth;
+			}
+		};
+
+		void sort_by_reverse_depth(bool p_alpha) { //used for alpha
+
+			SortArray<Element *, SortByReverseDepth> sorter;
+			if (p_alpha) {
+				sorter.sort(&elements[max_elements - alpha_element_count], alpha_element_count);
+			} else {
+				sorter.sort(elements, element_count);
+			}
+		}
+
 		_FORCE_INLINE_ Element *add_element() {
 
 			if (element_count + alpha_element_count >= max_elements)
 				return NULL;
-			elements[element_count] = &_elements[element_count];
+			elements[element_count] = &base_elements[element_count];
 			return elements[element_count++];
 		}
 
@@ -727,7 +744,7 @@ public:
 			if (element_count + alpha_element_count >= max_elements)
 				return NULL;
 			int idx = max_elements - alpha_element_count - 1;
-			elements[idx] = &_elements[idx];
+			elements[idx] = &base_elements[idx];
 			alpha_element_count++;
 			return elements[idx];
 		}
@@ -737,9 +754,9 @@ public:
 			element_count = 0;
 			alpha_element_count = 0;
 			elements = memnew_arr(Element *, max_elements);
-			_elements = memnew_arr(Element, max_elements);
+			base_elements = memnew_arr(Element, max_elements);
 			for (int i = 0; i < max_elements; i++)
-				elements[i] = &_elements[i]; // assign elements
+				elements[i] = &base_elements[i]; // assign elements
 		}
 
 		RenderList() {
@@ -749,7 +766,7 @@ public:
 
 		~RenderList() {
 			memdelete_arr(elements);
-			memdelete_arr(_elements);
+			memdelete_arr(base_elements);
 		}
 	};
 

editor/editor_node.cpp

@@ -6343,21 +6343,17 @@ EditorNode::EditorNode() {
 
 	add_editor_plugin( memnew( ShaderEditorPlugin(this,false) ) );*/
 	add_editor_plugin(memnew(CameraEditorPlugin(this)));
-	//	add_editor_plugin( memnew( SampleEditorPlugin(this) ) );
-	//	add_editor_plugin( memnew( SampleLibraryEditorPlugin(this) ) );
 	add_editor_plugin(memnew(ThemeEditorPlugin(this)));
 	add_editor_plugin(memnew(MultiMeshEditorPlugin(this)));
 	add_editor_plugin(memnew(MeshInstanceEditorPlugin(this)));
 	add_editor_plugin(memnew(AnimationTreeEditorPlugin(this)));
-	//add_editor_plugin( memnew( SamplePlayerEditorPlugin(this) ) ); - this is kind of useless at this point
 	//add_editor_plugin( memnew( MeshLibraryEditorPlugin(this) ) );
-	//add_editor_plugin( memnew( StreamEditorPlugin(this) ) );
 	add_editor_plugin(memnew(StyleBoxEditorPlugin(this)));
 	add_editor_plugin(memnew(ParticlesEditorPlugin(this)));
 	add_editor_plugin(memnew(ResourcePreloaderEditorPlugin(this)));
 	add_editor_plugin(memnew(ItemListEditorPlugin(this)));
 	//add_editor_plugin( memnew( RichTextEditorPlugin(this) ) );
-	//add_editor_plugin( memnew( CollisionPolygonEditorPlugin(this) ) );
+	add_editor_plugin(memnew(CollisionPolygonEditorPlugin(this)));
 	add_editor_plugin(memnew(CollisionPolygon2DEditorPlugin(this)));
 	add_editor_plugin(memnew(TileSetEditorPlugin(this)));
 	add_editor_plugin(memnew(TileMapEditorPlugin(this)));
@@ -6367,7 +6363,6 @@ EditorNode::EditorNode() {
 	add_editor_plugin(memnew(GIProbeEditorPlugin(this)));
 	add_editor_plugin(memnew(Path2DEditorPlugin(this)));
 	add_editor_plugin(memnew(PathEditorPlugin(this)));
-	//add_editor_plugin( memnew( BakedLightEditorPlugin(this) ) );
 	add_editor_plugin(memnew(Line2DEditorPlugin(this)));
 	add_editor_plugin(memnew(Polygon2DEditorPlugin(this)));
 	add_editor_plugin(memnew(LightOccluder2DEditorPlugin(this)));

editor/import/resource_importer_scene.cpp

@@ -33,7 +33,7 @@
 #include "io/resource_saver.h"
 #include "scene/resources/packed_scene.h"
 
-#include "scene/3d/body_shape.h"
+#include "scene/3d/collision_shape.h"
 #include "scene/3d/mesh_instance.h"
 #include "scene/3d/navigation.h"
 #include "scene/3d/physics_body.h"
@@ -369,10 +369,8 @@ Node *ResourceImporterScene::_fix_node(Node *p_node, Node *p_root, Map<Ref<Array
 			p_node = col;
 
 			StaticBody *sb = col->cast_to<StaticBody>();
-			CollisionShape *colshape = memnew(CollisionShape);
-			colshape->set_shape(sb->get_shape(0));
+			CollisionShape *colshape = sb->get_child(0)->cast_to<CollisionShape>();
 			colshape->set_name("shape");
-			sb->add_child(colshape);
 			colshape->set_owner(p_node->get_owner());
 		} else if (p_node->has_meta("empty_draw_type")) {
 			String empty_draw_type = String(p_node->get_meta("empty_draw_type"));
@@ -463,8 +461,7 @@ Node *ResourceImporterScene::_fix_node(Node *p_node, Node *p_root, Map<Ref<Array
 		p_node->add_child(col);
 
 		StaticBody *sb = col->cast_to<StaticBody>();
-		CollisionShape *colshape = memnew(CollisionShape);
-		colshape->set_shape(sb->get_shape(0));
+		CollisionShape *colshape = sb->get_child(0)->cast_to<CollisionShape>();
 		colshape->set_name("shape");
 		col->add_child(colshape);
 		colshape->set_owner(p_node->get_owner());

editor/plugins/collision_polygon_editor_plugin.cpp

@@ -35,339 +35,304 @@
 #include "scene/3d/camera.h"
 #include "spatial_editor_plugin.h"
 
-#if 0
-
 void CollisionPolygonEditor::_notification(int p_what) {
 
-	switch(p_what) {
+	switch (p_what) {
 
 		case NOTIFICATION_READY: {
 
-			button_create->set_icon( get_icon("Edit","EditorIcons"));
-			button_edit->set_icon( get_icon("MovePoint","EditorIcons"));
+			button_create->set_icon(get_icon("Edit", "EditorIcons"));
+			button_edit->set_icon(get_icon("MovePoint", "EditorIcons"));
 			button_edit->set_pressed(true);
-			get_tree()->connect("node_removed",this,"_node_removed");
-
+			get_tree()->connect("node_removed", this, "_node_removed");
 
 		} break;
 		case NOTIFICATION_PROCESS: {
 
 			if (node->get_depth() != prev_depth) {
 				_polygon_draw();
-				prev_depth=node->get_depth();
+				prev_depth = node->get_depth();
 			}
 
 		} break;
 	}
-
 }
 void CollisionPolygonEditor::_node_removed(Node *p_node) {
 
-	if(p_node==node) {
-		node=NULL;
-		if (imgeom->get_parent()==p_node)
+	if (p_node == node) {
+		node = NULL;
+		if (imgeom->get_parent() == p_node)
 			p_node->remove_child(imgeom);
 		hide();
 		set_process(false);
 	}
-
 }
 
-
 void CollisionPolygonEditor::_menu_option(int p_option) {
 
-	switch(p_option) {
+	switch (p_option) {
 
 		case MODE_CREATE: {
 
-			mode=MODE_CREATE;
+			mode = MODE_CREATE;
 			button_create->set_pressed(true);
 			button_edit->set_pressed(false);
 		} break;
 		case MODE_EDIT: {
 
-			mode=MODE_EDIT;
+			mode = MODE_EDIT;
 			button_create->set_pressed(false);
 			button_edit->set_pressed(true);
 		} break;
-
 	}
 }
 
 void CollisionPolygonEditor::_wip_close() {
 
 	undo_redo->create_action(TTR("Create Poly3D"));
-	undo_redo->add_undo_method(node,"set_polygon",node->get_polygon());
-	undo_redo->add_do_method(node,"set_polygon",wip);
-	undo_redo->add_do_method(this,"_polygon_draw");
-	undo_redo->add_undo_method(this,"_polygon_draw");
+	undo_redo->add_undo_method(node, "set_polygon", node->get_polygon());
+	undo_redo->add_do_method(node, "set_polygon", wip);
+	undo_redo->add_do_method(this, "_polygon_draw");
+	undo_redo->add_undo_method(this, "_polygon_draw");
 	wip.clear();
-	wip_active=false;
-	mode=MODE_EDIT;
+	wip_active = false;
+	mode = MODE_EDIT;
 	button_edit->set_pressed(true);
 	button_create->set_pressed(false);
-	edited_point=-1;
+	edited_point = -1;
 	undo_redo->commit_action();
-
 }
 
-bool CollisionPolygonEditor::forward_spatial_gui_input(Camera* p_camera,const InputEvent& p_event) {
+bool CollisionPolygonEditor::forward_spatial_gui_input(Camera *p_camera, const Ref<InputEvent> &p_event) {
 
 	if (!node)
 		return false;
 
 	Transform gt = node->get_global_transform();
 	Transform gi = gt.affine_inverse();
-	float depth = node->get_depth()*0.5;
+	float depth = node->get_depth() * 0.5;
 	Vector3 n = gt.basis.get_axis(2).normalized();
-	Plane p(gt.origin+n*depth,n);
-
-
-	switch(p_event.type) {
+	Plane p(gt.origin + n * depth, n);
 
-		case InputEvent::MOUSE_BUTTON: {
+	Ref<InputEventMouseButton> mb = p_event;
 
-			const InputEventMouseButton &mb=p_event.mouse_button;
+	if (mb.is_valid()) {
 
+		Vector2 gpoint = mb->get_position();
+		Vector3 ray_from = p_camera->project_ray_origin(gpoint);
+		Vector3 ray_dir = p_camera->project_ray_normal(gpoint);
 
+		Vector3 spoint;
 
-			Vector2 gpoint=Point2(mb->get_pos().x,mb->get_pos().y);
-			Vector3 ray_from = p_camera->project_ray_origin(gpoint);
-			Vector3 ray_dir = p_camera->project_ray_normal(gpoint);
-
-			Vector3 spoint;
-
-			if (!p.intersects_ray(ray_from,ray_dir,&spoint))
-				break;
+		if (!p.intersects_ray(ray_from, ray_dir, &spoint))
+			return false;
 
-			spoint = gi.xform(spoint);
+		spoint = gi.xform(spoint);
 
-			Vector2 cpoint(spoint.x,spoint.y);
+		Vector2 cpoint(spoint.x, spoint.y);
 
-			cpoint=CanvasItemEditor::get_singleton()->snap_point(cpoint);
+		cpoint = CanvasItemEditor::get_singleton()->snap_point(cpoint);
 
-			Vector<Vector2> poly = node->get_polygon();
+		Vector<Vector2> poly = node->get_polygon();
 
-			//first check if a point is to be added (segment split)
-			real_t grab_threshold=EDITOR_DEF("editors/poly_editor/point_grab_radius",8);
+		//first check if a point is to be added (segment split)
+		real_t grab_treshold = EDITOR_DEF("editors/poly_editor/point_grab_radius", 8);
 
-			switch(mode) {
+		switch (mode) {
 
+			case MODE_CREATE: {
 
-				case MODE_CREATE: {
+				if (mb->get_button_index() == BUTTON_LEFT && mb->is_pressed()) {
 
-					if (mb->get_button_index()==BUTTON_LEFT && mb->is_pressed()) {
+					if (!wip_active) {
 
+						wip.clear();
+						wip.push_back(cpoint);
+						wip_active = true;
+						edited_point_pos = cpoint;
+						_polygon_draw();
+						edited_point = 1;
+						return true;
+					} else {
 
-						if (!wip_active) {
+						if (wip.size() > 1 && p_camera->unproject_position(gt.xform(Vector3(wip[0].x, wip[0].y, depth))).distance_to(gpoint) < grab_treshold) {
+							//wip closed
+							_wip_close();
 
-							wip.clear();
-							wip.push_back( cpoint );
-							wip_active=true;
-							edited_point_pos=cpoint;
-							_polygon_draw();
-							edited_point=1;
 							return true;
 						} else {
 
-
-							if (wip.size()>1 && p_camera->unproject_position(gt.xform(Vector3(wip[0].x,wip[0].y,depth))).distance_to(gpoint)<grab_threshold) {
-								//wip closed
-								_wip_close();
-
-								return true;
-							} else {
-
-								wip.push_back( cpoint );
-								edited_point=wip.size();
-								_polygon_draw();
-								return true;
-
-								//add wip point
-							}
+							wip.push_back(cpoint);
+							edited_point = wip.size();
+							_polygon_draw();
+							return true;
 						}
-					} else if (mb->get_button_index()==BUTTON_RIGHT && mb->is_pressed() && wip_active) {
-						_wip_close();
 					}
+				} else if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && wip_active) {
+					_wip_close();
+				}
 
+			} break;
 
+			case MODE_EDIT: {
 
-				} break;
+				if (mb->get_button_index() == BUTTON_LEFT) {
+					if (mb->is_pressed()) {
 
-				case MODE_EDIT: {
+						if (mb->get_control()) {
 
-					if (mb->get_button_index()==BUTTON_LEFT) {
-						if (mb->is_pressed()) {
+							if (poly.size() < 3) {
 
-							if (mb->get_control()) {
-
-
-								if (poly.size() < 3) {
-
-									undo_redo->create_action(TTR("Edit Poly"));
-									undo_redo->add_undo_method(node,"set_polygon",poly);
-									poly.push_back(cpoint);
-									undo_redo->add_do_method(node,"set_polygon",poly);
-									undo_redo->add_do_method(this,"_polygon_draw");
-									undo_redo->add_undo_method(this,"_polygon_draw");
-									undo_redo->commit_action();
-									return true;
-								}
-
-								//search edges
-								int closest_idx=-1;
-								Vector2 closest_pos;
-								real_t closest_dist=1e10;
-								for(int i=0;i<poly.size();i++) {
-
-									Vector2 points[2] ={
-										p_camera->unproject_position(gt.xform(Vector3(poly[i].x,poly[i].y,depth))),
-										p_camera->unproject_position(gt.xform(Vector3(poly[(i+1)%poly.size()].x,poly[(i+1)%poly.size()].y,depth)))
-									};
-
-									Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint,points);
-									if (cp.distance_squared_to(points[0])<CMP_EPSILON2 || cp.distance_squared_to(points[1])<CMP_EPSILON2)
-										continue; //not valid to reuse point
-
-									real_t d = cp.distance_to(gpoint);
-									if (d<closest_dist && d<grab_threshold) {
-										closest_dist=d;
-										closest_pos=cp;
-										closest_idx=i;
-									}
-
-
-								}
-
-								if (closest_idx>=0) {
-
-									pre_move_edit=poly;
-									poly.insert(closest_idx+1,cpoint);
-									edited_point=closest_idx+1;
-									edited_point_pos=cpoint;
-									node->set_polygon(poly);
-									_polygon_draw();
-									return true;
-								}
-							} else {
-
-								//look for points to move
-
-								int closest_idx=-1;
-								Vector2 closest_pos;
-								real_t closest_dist=1e10;
-								for(int i=0;i<poly.size();i++) {
-
-									Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x,poly[i].y,depth)));
-
-									real_t d = cp.distance_to(gpoint);
-									if (d<closest_dist && d<grab_threshold) {
-										closest_dist=d;
-										closest_pos=cp;
-										closest_idx=i;
-									}
+								undo_redo->create_action(TTR("Edit Poly"));
+								undo_redo->add_undo_method(node, "set_polygon", poly);
+								poly.push_back(cpoint);
+								undo_redo->add_do_method(node, "set_polygon", poly);
+								undo_redo->add_do_method(this, "_polygon_draw");
+								undo_redo->add_undo_method(this, "_polygon_draw");
+								undo_redo->commit_action();
+								return true;
+							}
 
+							//search edges
+							int closest_idx = -1;
+							Vector2 closest_pos;
+							real_t closest_dist = 1e10;
+							for (int i = 0; i < poly.size(); i++) {
+
+								Vector2 points[2] = {
+									p_camera->unproject_position(gt.xform(Vector3(poly[i].x, poly[i].y, depth))),
+									p_camera->unproject_position(gt.xform(Vector3(poly[(i + 1) % poly.size()].x, poly[(i + 1) % poly.size()].y, depth)))
+								};
+
+								Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint, points);
+								if (cp.distance_squared_to(points[0]) < CMP_EPSILON2 || cp.distance_squared_to(points[1]) < CMP_EPSILON2)
+									continue; //not valid to reuse point
+
+								real_t d = cp.distance_to(gpoint);
+								if (d < closest_dist && d < grab_treshold) {
+									closest_dist = d;
+									closest_pos = cp;
+									closest_idx = i;
 								}
+							}
 
-								if (closest_idx>=0) {
+							if (closest_idx >= 0) {
 
-									pre_move_edit=poly;
-									edited_point=closest_idx;
-									edited_point_pos=poly[closest_idx];
-									_polygon_draw();
-									return true;
-								}
+								pre_move_edit = poly;
+								poly.insert(closest_idx + 1, cpoint);
+								edited_point = closest_idx + 1;
+								edited_point_pos = cpoint;
+								node->set_polygon(poly);
+								_polygon_draw();
+								return true;
 							}
 						} else {
 
-							if (edited_point!=-1) {
+							//look for points to move
 
-								//apply
+							int closest_idx = -1;
+							Vector2 closest_pos;
+							real_t closest_dist = 1e10;
+							for (int i = 0; i < poly.size(); i++) {
 
-								ERR_FAIL_INDEX_V(edited_point,poly.size(),false);
-								poly[edited_point]=edited_point_pos;
-								undo_redo->create_action(TTR("Edit Poly"));
-								undo_redo->add_do_method(node,"set_polygon",poly);
-								undo_redo->add_undo_method(node,"set_polygon",pre_move_edit);
-								undo_redo->add_do_method(this,"_polygon_draw");
-								undo_redo->add_undo_method(this,"_polygon_draw");
-								undo_redo->commit_action();
+								Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x, poly[i].y, depth)));
 
-								edited_point=-1;
-								return true;
+								real_t d = cp.distance_to(gpoint);
+								if (d < closest_dist && d < grab_treshold) {
+									closest_dist = d;
+									closest_pos = cp;
+									closest_idx = i;
+								}
 							}
-						}
-					} if (mb->get_button_index()==BUTTON_RIGHT && mb->is_pressed() && edited_point==-1) {
-
 
+							if (closest_idx >= 0) {
 
-						int closest_idx=-1;
-						Vector2 closest_pos;
-						real_t closest_dist=1e10;
-						for(int i=0;i<poly.size();i++) {
-
-							Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x,poly[i].y,depth)));
-
-							real_t d = cp.distance_to(gpoint);
-							if (d<closest_dist && d<grab_threshold) {
-								closest_dist=d;
-								closest_pos=cp;
-								closest_idx=i;
+								pre_move_edit = poly;
+								edited_point = closest_idx;
+								edited_point_pos = poly[closest_idx];
+								_polygon_draw();
+								return true;
 							}
-
 						}
+					} else {
 
-						if (closest_idx>=0) {
+						if (edited_point != -1) {
 
+							//apply
 
-							undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
-							undo_redo->add_undo_method(node,"set_polygon",poly);
-							poly.remove(closest_idx);
-							undo_redo->add_do_method(node,"set_polygon",poly);
-							undo_redo->add_do_method(this,"_polygon_draw");
-							undo_redo->add_undo_method(this,"_polygon_draw");
+							ERR_FAIL_INDEX_V(edited_point, poly.size(), false);
+							poly[edited_point] = edited_point_pos;
+							undo_redo->create_action(TTR("Edit Poly"));
+							undo_redo->add_do_method(node, "set_polygon", poly);
+							undo_redo->add_undo_method(node, "set_polygon", pre_move_edit);
+							undo_redo->add_do_method(this, "_polygon_draw");
+							undo_redo->add_undo_method(this, "_polygon_draw");
 							undo_redo->commit_action();
+
+							edited_point = -1;
 							return true;
 						}
-
 					}
+				}
+				if (mb->get_button_index() == BUTTON_RIGHT && mb->is_pressed() && edited_point == -1) {
 
+					int closest_idx = -1;
+					Vector2 closest_pos;
+					real_t closest_dist = 1e10;
+					for (int i = 0; i < poly.size(); i++) {
 
+						Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x, poly[i].y, depth)));
 
-				} break;
-			}
-
+						real_t d = cp.distance_to(gpoint);
+						if (d < closest_dist && d < grab_treshold) {
+							closest_dist = d;
+							closest_pos = cp;
+							closest_idx = i;
+						}
+					}
 
+					if (closest_idx >= 0) {
 
-		} break;
-		case InputEvent::MOUSE_MOTION: {
+						undo_redo->create_action(TTR("Edit Poly (Remove Point)"));
+						undo_redo->add_undo_method(node, "set_polygon", poly);
+						poly.remove(closest_idx);
+						undo_redo->add_do_method(node, "set_polygon", poly);
+						undo_redo->add_do_method(this, "_polygon_draw");
+						undo_redo->add_undo_method(this, "_polygon_draw");
+						undo_redo->commit_action();
+						return true;
+					}
+				}
 
-			const InputEventMouseMotion &mm=p_event.mouse_motion;
+			} break;
+		}
+	}
 
-			if (edited_point!=-1 && (wip_active || mm->get_button_mask()&BUTTON_MASK_LEFT)) {
+	Ref<InputEventMouseMotion> mm = p_event;
 
-				Vector2 gpoint = Point2(mm.x,mm.y);
+	if (mm.is_valid()) {
 
-				Vector3 ray_from = p_camera->project_ray_origin(gpoint);
-				Vector3 ray_dir = p_camera->project_ray_normal(gpoint);
+		if (edited_point != -1 && (wip_active || mm->get_button_mask() & BUTTON_MASK_LEFT)) {
 
-				Vector3 spoint;
+			Vector2 gpoint = mm->get_position();
 
-				if (!p.intersects_ray(ray_from,ray_dir,&spoint))
-					break;
+			Vector3 ray_from = p_camera->project_ray_origin(gpoint);
+			Vector3 ray_dir = p_camera->project_ray_normal(gpoint);
 
-				spoint = gi.xform(spoint);
+			Vector3 spoint;
 
-				Vector2 cpoint(spoint.x,spoint.y);
+			if (!p.intersects_ray(ray_from, ray_dir, &spoint))
+				return false;
 
-				cpoint=CanvasItemEditor::get_singleton()->snap_point(cpoint);
-				edited_point_pos = cpoint;
+			spoint = gi.xform(spoint);
 
-				_polygon_draw();
+			Vector2 cpoint(spoint.x, spoint.y);
 
-			}
+			cpoint = CanvasItemEditor::get_singleton()->snap_point(cpoint);
+			edited_point_pos = cpoint;
 
-		} break;
+			_polygon_draw();
+		}
 	}
 
 	return false;
@@ -380,41 +345,38 @@ void CollisionPolygonEditor::_polygon_draw() {
 	Vector<Vector2> poly;
 
 	if (wip_active)
-		poly=wip;
+		poly = wip;
 	else
-		poly=node->get_polygon();
-
+		poly = node->get_polygon();
 
-	float depth = node->get_depth()*0.5;
+	float depth = node->get_depth() * 0.5;
 
 	imgeom->clear();
 	imgeom->set_material_override(line_material);
-	imgeom->begin(Mesh::PRIMITIVE_LINES,Ref<Texture>());
-
+	imgeom->begin(Mesh::PRIMITIVE_LINES, Ref<Texture>());
 
 	Rect2 rect;
 
-	for(int i=0;i<poly.size();i++) {
-
+	for (int i = 0; i < poly.size(); i++) {
 
-		Vector2 p,p2;
-		p = i==edited_point ? edited_point_pos : poly[i];
-		if ((wip_active && i==poly.size()-1) || (((i+1)%poly.size())==edited_point))
-			p2=edited_point_pos;
+		Vector2 p, p2;
+		p = i == edited_point ? edited_point_pos : poly[i];
+		if ((wip_active && i == poly.size() - 1) || (((i + 1) % poly.size()) == edited_point))
+			p2 = edited_point_pos;
 		else
-			p2 = poly[(i+1)%poly.size()];
+			p2 = poly[(i + 1) % poly.size()];
 
-		if (i==0)
-			rect.pos=p;
+		if (i == 0)
+			rect.position = p;
 		else
 			rect.expand_to(p);
 
-		Vector3 point = Vector3(p.x,p.y,depth);
-		Vector3 next_point = Vector3(p2.x,p2.y,depth);
+		Vector3 point = Vector3(p.x, p.y, depth);
+		Vector3 next_point = Vector3(p2.x, p2.y, depth);
 
-		imgeom->set_color(Color(1,0.3,0.1,0.8));
+		imgeom->set_color(Color(1, 0.3, 0.1, 0.8));
 		imgeom->add_vertex(point);
-		imgeom->set_color(Color(1,0.3,0.1,0.8));
+		imgeom->set_color(Color(1, 0.3, 0.1, 0.8));
 		imgeom->add_vertex(next_point);
 
 		//Color col=Color(1,0.3,0.1,0.8);
@@ -422,60 +384,59 @@ void CollisionPolygonEditor::_polygon_draw() {
 		//vpc->draw_texture(handle,point-handle->get_size()*0.5);
 	}
 
-	rect=rect.grow(1);
-
-	AABB r;
-	r.pos.x=rect.pos.x;
-	r.pos.y=rect.pos.y;
-	r.pos.z=depth;
-	r.size.x=rect.size.x;
-	r.size.y=rect.size.y;
-	r.size.z=0;
-
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos);
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0.3,0,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos);
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0.0,0.3,0));
-
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(r.size.x,0,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(r.size.x,0,0)-Vector3(0.3,0,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(r.size.x,0,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(r.size.x,0,0)+Vector3(0,0.3,0));
-
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0,r.size.y,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0,r.size.y,0)-Vector3(0,0.3,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0,r.size.y,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+Vector3(0,r.size.y,0)+Vector3(0.3,0,0));
-
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+r.size);
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+r.size-Vector3(0.3,0,0));
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+r.size);
-	imgeom->set_color(Color(0.8,0.8,0.8,0.2));
-	imgeom->add_vertex(r.pos+r.size-Vector3(0.0,0.3,0));
+	rect = rect.grow(1);
+
+	Rect3 r;
+	r.position.x = rect.position.x;
+	r.position.y = rect.position.y;
+	r.position.z = depth;
+	r.size.x = rect.size.x;
+	r.size.y = rect.size.y;
+	r.size.z = 0;
+
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position);
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0.3, 0, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position);
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0.0, 0.3, 0));
+
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(r.size.x, 0, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(r.size.x, 0, 0) - Vector3(0.3, 0, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(r.size.x, 0, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(r.size.x, 0, 0) + Vector3(0, 0.3, 0));
+
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0, r.size.y, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0, r.size.y, 0) - Vector3(0, 0.3, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0, r.size.y, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + Vector3(0, r.size.y, 0) + Vector3(0.3, 0, 0));
+
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + r.size);
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + r.size - Vector3(0.3, 0, 0));
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + r.size);
+	imgeom->set_color(Color(0.8, 0.8, 0.8, 0.2));
+	imgeom->add_vertex(r.position + r.size - Vector3(0.0, 0.3, 0));
 
 	imgeom->end();
 
-
-	while(m->get_surface_count()) {
+	while (m->get_surface_count()) {
 		m->surface_remove(0);
 	}
 
-	if (poly.size()==0)
+	if (poly.size() == 0)
 		return;
 
 	Array a;
@@ -484,78 +445,69 @@ void CollisionPolygonEditor::_polygon_draw() {
 	{
 
 		va.resize(poly.size());
-		PoolVector<Vector3>::Write w=va.write();
-		for(int i=0;i<poly.size();i++) {
-
+		PoolVector<Vector3>::Write w = va.write();
+		for (int i = 0; i < poly.size(); i++) {
 
-			Vector2 p,p2;
-			p = i==edited_point ? edited_point_pos : poly[i];
+			Vector2 p, p2;
+			p = i == edited_point ? edited_point_pos : poly[i];
 
-			Vector3 point = Vector3(p.x,p.y,depth);
-			w[i]=point;
+			Vector3 point = Vector3(p.x, p.y, depth);
+			w[i] = point;
 		}
 	}
-	a[Mesh::ARRAY_VERTEX]=va;
-	m->add_surface(Mesh::PRIMITIVE_POINTS,a);
-	m->surface_set_material(0,handle_material);
-
+	a[Mesh::ARRAY_VERTEX] = va;
+	m->add_surface_from_arrays(Mesh::PRIMITIVE_POINTS, a);
+	m->surface_set_material(0, handle_material);
 }
 
-
-
 void CollisionPolygonEditor::edit(Node *p_collision_polygon) {
 
-
-
 	if (p_collision_polygon) {
 
-		node=p_collision_polygon->cast_to<CollisionPolygon>();
+		node = p_collision_polygon->cast_to<CollisionPolygon>();
 		wip.clear();
-		wip_active=false;
-		edited_point=-1;
+		wip_active = false;
+		edited_point = -1;
 		p_collision_polygon->add_child(imgeom);
 		_polygon_draw();
 		set_process(true);
-		prev_depth=-1;
+		prev_depth = -1;
 
 	} else {
-		node=NULL;
+		node = NULL;
 
 		if (imgeom->get_parent())
 			imgeom->get_parent()->remove_child(imgeom);
 
 		set_process(false);
 	}
-
 }
 
 void CollisionPolygonEditor::_bind_methods() {
 
-	ClassDB::bind_method(D_METHOD("_menu_option"),&CollisionPolygonEditor::_menu_option);
-	ClassDB::bind_method(D_METHOD("_polygon_draw"),&CollisionPolygonEditor::_polygon_draw);
-	ClassDB::bind_method(D_METHOD("_node_removed"),&CollisionPolygonEditor::_node_removed);
-
+	ClassDB::bind_method(D_METHOD("_menu_option"), &CollisionPolygonEditor::_menu_option);
+	ClassDB::bind_method(D_METHOD("_polygon_draw"), &CollisionPolygonEditor::_polygon_draw);
+	ClassDB::bind_method(D_METHOD("_node_removed"), &CollisionPolygonEditor::_node_removed);
 }
 
 CollisionPolygonEditor::CollisionPolygonEditor(EditorNode *p_editor) {
 
-
-	node=NULL;
-	editor=p_editor;
+	node = NULL;
+	editor = p_editor;
 	undo_redo = editor->get_undo_redo();
 
-	add_child( memnew( VSeparator ));
-	button_create = memnew( ToolButton );
+	add_child(memnew(VSeparator));
+	button_create = memnew(ToolButton);
 	add_child(button_create);
-	button_create->connect("pressed",this,"_menu_option",varray(MODE_CREATE));
+	button_create->connect("pressed", this, "_menu_option", varray(MODE_CREATE));
 	button_create->set_toggle_mode(true);
 
-	button_edit = memnew( ToolButton );
+	button_edit = memnew(ToolButton);
 	add_child(button_edit);
-	button_edit->connect("pressed",this,"_menu_option",varray(MODE_EDIT));
+	button_edit->connect("pressed", this, "_menu_option", varray(MODE_EDIT));
 	button_edit->set_toggle_mode(true);
 
-	//add_constant_override("separation",0);
+//add_constant_override("separation",0);
 
 #if 0
 	options = memnew( MenuButton );
@@ -567,46 +519,40 @@ CollisionPolygonEditor::CollisionPolygonEditor(EditorNode *p_editor) {
 #endif
 
 	mode = MODE_EDIT;
-	wip_active=false;
-	imgeom = memnew( ImmediateGeometry );
-	imgeom->set_transform(Transform(Matrix3(),Vector3(0,0,0.00001)));
-
+	wip_active = false;
+	imgeom = memnew(ImmediateGeometry);
+	imgeom->set_transform(Transform(Basis(), Vector3(0, 0, 0.00001)));
 
-	line_material = Ref<SpatialMaterial>( memnew( SpatialMaterial ));
-	line_material->set_flag(Material::FLAG_UNSHADED, true);
+	line_material = Ref<SpatialMaterial>(memnew(SpatialMaterial));
+	line_material->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
 	line_material->set_line_width(3.0);
-	line_material->set_fixed_flag(SpatialMaterial::FLAG_USE_ALPHA, true);
-	line_material->set_fixed_flag(SpatialMaterial::FLAG_USE_COLOR_ARRAY, true);
-	line_material->set_parameter(SpatialMaterial::PARAM_DIFFUSE,Color(1,1,1));
-
-
-
-
-	handle_material = Ref<SpatialMaterial>( memnew( SpatialMaterial ));
-	handle_material->set_flag(Material::FLAG_UNSHADED, true);
-	handle_material->set_fixed_flag(SpatialMaterial::FLAG_USE_POINT_SIZE, true);
-	handle_material->set_parameter(SpatialMaterial::PARAM_DIFFUSE,Color(1,1,1));
-	handle_material->set_fixed_flag(SpatialMaterial::FLAG_USE_ALPHA, true);
-	handle_material->set_fixed_flag(SpatialMaterial::FLAG_USE_COLOR_ARRAY, false);
-	Ref<Texture> handle=editor->get_gui_base()->get_icon("Editor3DHandle","EditorIcons");
+	line_material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
+	line_material->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	line_material->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
+	line_material->set_albedo(Color(1, 1, 1));
+
+	handle_material = Ref<SpatialMaterial>(memnew(SpatialMaterial));
+	handle_material->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
+	handle_material->set_flag(SpatialMaterial::FLAG_USE_POINT_SIZE, true);
+	handle_material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
+	handle_material->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	handle_material->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
+	Ref<Texture> handle = editor->get_gui_base()->get_icon("Editor3DHandle", "EditorIcons");
 	handle_material->set_point_size(handle->get_width());
-	handle_material->set_texture(SpatialMaterial::PARAM_DIFFUSE,handle);
+	handle_material->set_texture(SpatialMaterial::TEXTURE_ALBEDO, handle);
 
-	pointsm = memnew( MeshInstance );
+	pointsm = memnew(MeshInstance);
 	imgeom->add_child(pointsm);
-	m = Ref<Mesh>( memnew( Mesh ) );
+	m.instance();
 	pointsm->set_mesh(m);
-	pointsm->set_transform(Transform(Matrix3(),Vector3(0,0,0.00001)));
-
-
+	pointsm->set_transform(Transform(Basis(), Vector3(0, 0, 0.00001)));
 }
 
 CollisionPolygonEditor::~CollisionPolygonEditor() {
 
-	memdelete( imgeom );
+	memdelete(imgeom);
 }
 
-
 void CollisionPolygonEditorPlugin::edit(Object *p_object) {
 
 	collision_polygon_editor->edit(p_object->cast_to<Node>());
@@ -614,7 +560,7 @@ void CollisionPolygonEditorPlugin::edit(Object *p_object) {
 
 bool CollisionPolygonEditorPlugin::handles(Object *p_object) const {
 
-	return p_object->is_type("CollisionPolygon");
+	return p_object->is_class("CollisionPolygon");
 }
 
 void CollisionPolygonEditorPlugin::make_visible(bool p_visible) {
@@ -626,24 +572,16 @@ void CollisionPolygonEditorPlugin::make_visible(bool p_visible) {
 		collision_polygon_editor->hide();
 		collision_polygon_editor->edit(NULL);
 	}
-
 }
 
 CollisionPolygonEditorPlugin::CollisionPolygonEditorPlugin(EditorNode *p_node) {
 
-	editor=p_node;
-	collision_polygon_editor = memnew( CollisionPolygonEditor(p_node) );
+	editor = p_node;
+	collision_polygon_editor = memnew(CollisionPolygonEditor(p_node));
 	SpatialEditor::get_singleton()->add_control_to_menu_panel(collision_polygon_editor);
 
 	collision_polygon_editor->hide();
-
-
-
 }
 
-
-CollisionPolygonEditorPlugin::~CollisionPolygonEditorPlugin()
-{
+CollisionPolygonEditorPlugin::~CollisionPolygonEditorPlugin() {
 }
-
-#endif

editor/plugins/collision_polygon_editor_plugin.h

@@ -42,12 +42,11 @@
 	@author Juan Linietsky <[email protected]>
 */
 
-#if 0
 class CanvasItemEditor;
 
 class CollisionPolygonEditor : public HBoxContainer {
 
-	GDCLASS(CollisionPolygonEditor, HBoxContainer );
+	GDCLASS(CollisionPolygonEditor, HBoxContainer);
 
 	UndoRedo *undo_redo;
 	enum Mode {
@@ -62,7 +61,6 @@ class CollisionPolygonEditor : public HBoxContainer {
 	ToolButton *button_create;
 	ToolButton *button_edit;
 
-
 	Ref<SpatialMaterial> line_material;
 	Ref<SpatialMaterial> handle_material;
 
@@ -71,7 +69,7 @@ class CollisionPolygonEditor : public HBoxContainer {
 	CollisionPolygon *node;
 	ImmediateGeometry *imgeom;
 	MeshInstance *pointsm;
-	Ref<Mesh> m;
+	Ref<ArrayMesh> m;
 
 	MenuButton *options;
 
@@ -91,9 +89,9 @@ protected:
 	void _notification(int p_what);
 	void _node_removed(Node *p_node);
 	static void _bind_methods();
-public:
 
-	virtual bool forward_spatial_gui_input(Camera* p_camera,const InputEvent& p_event);
+public:
+	virtual bool forward_spatial_gui_input(Camera *p_camera, const Ref<InputEvent> &p_event);
 	void edit(Node *p_collision_polygon);
 	CollisionPolygonEditor(EditorNode *p_editor);
 	~CollisionPolygonEditor();
@@ -101,14 +99,13 @@ public:
 
 class CollisionPolygonEditorPlugin : public EditorPlugin {
 
-	GDCLASS( CollisionPolygonEditorPlugin, EditorPlugin );
+	GDCLASS(CollisionPolygonEditorPlugin, EditorPlugin);
 
 	CollisionPolygonEditor *collision_polygon_editor;
 	EditorNode *editor;
 
 public:
-
-	virtual bool forward_spatial_gui_input(Camera* p_camera,const InputEvent& p_event) { return collision_polygon_editor->forward_spatial_gui_input(p_camera,p_event); }
+	virtual bool forward_spatial_gui_input(Camera *p_camera, const Ref<InputEvent> &p_event) { return collision_polygon_editor->forward_spatial_gui_input(p_camera, p_event); }
 
 	virtual String get_name() const { return "CollisionPolygon"; }
 	bool has_main_screen() const { return false; }
@@ -118,7 +115,6 @@ public:
 
 	CollisionPolygonEditorPlugin(EditorNode *p_node);
 	~CollisionPolygonEditorPlugin();
-
 };
-#endif
+
 #endif // COLLISION_POLYGON_EDITOR_PLUGIN_H

editor/plugins/mesh_instance_editor_plugin.cpp

@@ -29,7 +29,7 @@
 /*************************************************************************/
 #include "mesh_instance_editor_plugin.h"
 
-#include "scene/3d/body_shape.h"
+#include "scene/3d/collision_shape.h"
 #include "scene/3d/navigation_mesh.h"
 #include "scene/3d/physics_body.h"
 #include "scene/gui/box_container.h"

editor/plugins/spatial_editor_plugin.cpp

@@ -2024,6 +2024,15 @@ void SpatialEditorViewport::_menu_option(int p_option) {
 			viewport->set_as_audio_listener(current);
 			view_menu->get_popup()->set_item_checked(idx, current);
 
+		} break;
+		case VIEW_AUDIO_DOPPLER: {
+
+			int idx = view_menu->get_popup()->get_item_index(VIEW_AUDIO_DOPPLER);
+			bool current = view_menu->get_popup()->is_item_checked(idx);
+			current = !current;
+			camera->set_doppler_tracking(current ? Camera::DOPPLER_TRACKING_IDLE_STEP : Camera::DOPPLER_TRACKING_DISABLED);
+			view_menu->get_popup()->set_item_checked(idx, current);
+
 		} break;
 		case VIEW_GIZMOS: {
 
@@ -2237,6 +2246,13 @@ void SpatialEditorViewport::set_state(const Dictionary &p_state) {
 		viewport->set_as_audio_listener(listener);
 		view_menu->get_popup()->set_item_checked(idx, listener);
 	}
+	if (p_state.has("doppler")) {
+		bool doppler = p_state["doppler"];
+
+		int idx = view_menu->get_popup()->get_item_index(VIEW_AUDIO_DOPPLER);
+		camera->set_doppler_tracking(doppler ? Camera::DOPPLER_TRACKING_IDLE_STEP : Camera::DOPPLER_TRACKING_DISABLED);
+		view_menu->get_popup()->set_item_checked(idx, doppler);
+	}
 
 	if (p_state.has("previewing")) {
 		Node *pv = EditorNode::get_singleton()->get_edited_scene()->get_node(p_state["previewing"]);
@@ -2395,6 +2411,7 @@ SpatialEditorViewport::SpatialEditorViewport(SpatialEditor *p_spatial_editor, Ed
 	view_menu->get_popup()->set_item_checked(view_menu->get_popup()->get_item_index(VIEW_ENVIRONMENT), true);
 	view_menu->get_popup()->add_separator();
 	view_menu->get_popup()->add_check_shortcut(ED_SHORTCUT("spatial_editor/view_audio_listener", TTR("Audio Listener")), VIEW_AUDIO_LISTENER);
+	view_menu->get_popup()->add_check_shortcut(ED_SHORTCUT("spatial_editor/view_audio_doppler", TTR("Doppler Enable")), VIEW_AUDIO_DOPPLER);
 	view_menu->get_popup()->set_item_checked(view_menu->get_popup()->get_item_index(VIEW_GIZMOS), true);
 
 	view_menu->get_popup()->add_separator();

editor/plugins/spatial_editor_plugin.h

@@ -83,6 +83,7 @@ class SpatialEditorViewport : public Control {
 		VIEW_ENVIRONMENT,
 		VIEW_ORTHOGONAL,
 		VIEW_AUDIO_LISTENER,
+		VIEW_AUDIO_DOPPLER,
 		VIEW_GIZMOS,
 		VIEW_INFORMATION,
 		VIEW_DISPLAY_NORMAL,

editor/scene_tree_editor.cpp

@@ -161,7 +161,7 @@ bool SceneTreeEditor::_add_nodes(Node *p_node, TreeItem *p_parent) {
 
 	item->set_selectable(0, true);
 	if (can_rename) {
-#ifdef ENABLE_DEPRECATED
+#ifndef DISABLE_DEPRECATED
 		if (p_node->has_meta("_editor_collapsed")) {
 			//remove previous way of storing folding, which did not get along with scene inheritance and instancing
 			if ((bool)p_node->get_meta("_editor_collapsed"))

editor/spatial_editor_gizmos.cpp

@@ -867,6 +867,127 @@ LightSpatialGizmo::LightSpatialGizmo(Light *p_light) {
 
 //////
 
+//// player gizmo
+
+String AudioStreamPlayer3DSpatialGizmo::get_handle_name(int p_idx) const {
+
+	return "Emission Radius";
+}
+
+Variant AudioStreamPlayer3DSpatialGizmo::get_handle_value(int p_idx) const {
+
+	return player->get_emission_angle();
+}
+
+void AudioStreamPlayer3DSpatialGizmo::set_handle(int p_idx, Camera *p_camera, const Point2 &p_point) {
+
+	Transform gt = player->get_global_transform();
+	gt.orthonormalize();
+	Transform gi = gt.affine_inverse();
+
+	Vector3 ray_from = p_camera->project_ray_origin(p_point);
+	Vector3 ray_dir = p_camera->project_ray_normal(p_point);
+	Vector3 ray_to = ray_from + ray_dir * 4096;
+
+	ray_from = gi.xform(ray_from);
+	ray_to = gi.xform(ray_to);
+
+	float closest_dist = 1e20;
+	float closest_angle = 1e20;
+
+	for (int i = 0; i < 180; i++) {
+
+		float a = i * Math_PI / 180.0;
+		float an = (i + 1) * Math_PI / 180.0;
+
+		Vector3 from(Math::sin(a), 0, -Math::cos(a));
+		Vector3 to(Math::sin(an), 0, -Math::cos(an));
+
+		Vector3 r1, r2;
+		Geometry::get_closest_points_between_segments(from, to, ray_from, ray_to, r1, r2);
+		float d = r1.distance_to(r2);
+		if (d < closest_dist) {
+			closest_dist = d;
+			closest_angle = i;
+		}
+	}
+
+	if (closest_angle < 91) {
+		player->set_emission_angle(closest_angle);
+	}
+}
+
+void AudioStreamPlayer3DSpatialGizmo::commit_handle(int p_idx, const Variant &p_restore, bool p_cancel) {
+
+	if (p_cancel) {
+
+		player->set_emission_angle(p_restore);
+
+	} else {
+
+		UndoRedo *ur = SpatialEditor::get_singleton()->get_undo_redo();
+		ur->create_action(TTR("Change AudioStreamPlayer3D Emission Angle"));
+		ur->add_do_method(player, "set_emission_angle", player->get_emission_angle());
+		ur->add_undo_method(player, "set_emission_angle", p_restore);
+		ur->commit_action();
+	}
+}
+
+void AudioStreamPlayer3DSpatialGizmo::redraw() {
+
+	clear();
+
+	if (player->is_emission_angle_enabled()) {
+		float pc = player->get_emission_angle();
+
+		Vector<Vector3> points;
+		points.resize(208);
+
+		float ofs = -Math::cos(Math::deg2rad(pc));
+		float radius = Math::sin(Math::deg2rad(pc));
+
+		for (int i = 0; i < 100; i++) {
+
+			float a = i * 2.0 * Math_PI / 100.0;
+			float an = (i + 1) * 2.0 * Math_PI / 100.0;
+
+			Vector3 from(Math::sin(a) * radius, Math::cos(a) * radius, ofs);
+			Vector3 to(Math::sin(an) * radius, Math::cos(an) * radius, ofs);
+
+			points[i * 2 + 0] = from;
+			points[i * 2 + 1] = to;
+		}
+
+		for (int i = 0; i < 4; i++) {
+
+			float a = i * 2.0 * Math_PI / 4.0;
+
+			Vector3 from(Math::sin(a) * radius, Math::cos(a) * radius, ofs);
+
+			points[200 + i * 2 + 0] = from;
+			points[200 + i * 2 + 1] = Vector3();
+		}
+
+		add_lines(points, SpatialEditorGizmos::singleton->car_wheel_material);
+		add_collision_segments(points);
+
+		Vector<Vector3> handles;
+		float ha = Math::deg2rad(player->get_emission_angle());
+		handles.push_back(Vector3(Math::sin(ha), 0, -Math::cos(ha)));
+		add_handles(handles);
+	}
+
+	add_unscaled_billboard(SpatialEditorGizmos::singleton->sample_player_icon, 0.05);
+}
+
+AudioStreamPlayer3DSpatialGizmo::AudioStreamPlayer3DSpatialGizmo(AudioStreamPlayer3D *p_player) {
+
+	player = p_player;
+	set_spatial_node(p_player);
+}
+
+//////
+
 String CameraSpatialGizmo::get_handle_name(int p_idx) const {
 
 	if (camera->get_projection() == Camera::PROJECTION_PERSPECTIVE) {
@@ -3101,6 +3222,12 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		return misg;
 	}
 
+	if (p_spatial->cast_to<AudioStreamPlayer3D>()) {
+
+		Ref<AudioStreamPlayer3DSpatialGizmo> misg = memnew(AudioStreamPlayer3DSpatialGizmo(p_spatial->cast_to<AudioStreamPlayer3D>()));
+		return misg;
+	}
+
 	return Ref<SpatialEditorGizmo>();
 }
 

editor/spatial_editor_gizmos.h

@@ -31,9 +31,10 @@
 #define SPATIAL_EDITOR_GIZMOS_H
 
 #include "editor/plugins/spatial_editor_plugin.h"
-#include "scene/3d/body_shape.h"
+#include "scene/3d/audio_stream_player_3d.h"
 #include "scene/3d/camera.h"
 #include "scene/3d/collision_polygon.h"
+#include "scene/3d/collision_shape.h"
 #include "scene/3d/gi_probe.h"
 #include "scene/3d/light.h"
 #include "scene/3d/listener.h"
@@ -137,6 +138,22 @@ public:
 	LightSpatialGizmo(Light *p_light = NULL);
 };
 
+class AudioStreamPlayer3DSpatialGizmo : public EditorSpatialGizmo {
+
+	GDCLASS(AudioStreamPlayer3DSpatialGizmo, EditorSpatialGizmo);
+
+	AudioStreamPlayer3D *player;
+
+public:
+	virtual String get_handle_name(int p_idx) const;
+	virtual Variant get_handle_value(int p_idx) const;
+	virtual void set_handle(int p_idx, Camera *p_camera, const Point2 &p_point);
+	virtual void commit_handle(int p_idx, const Variant &p_restore, bool p_cancel = false);
+
+	void redraw();
+	AudioStreamPlayer3DSpatialGizmo(AudioStreamPlayer3D *p_player = NULL);
+};
+
 class CameraSpatialGizmo : public EditorSpatialGizmo {
 
 	GDCLASS(CameraSpatialGizmo, EditorSpatialGizmo);

main/main.cpp

@@ -1525,11 +1525,15 @@ static uint64_t idle_process_max = 0;
 bool Main::iteration() {
 
 	uint64_t ticks = OS::get_singleton()->get_ticks_usec();
+	Engine::get_singleton()->_frame_ticks = ticks;
+
 	uint64_t ticks_elapsed = ticks - last_ticks;
 
 	double step = (double)ticks_elapsed / 1000000.0;
 	float frame_slice = 1.0 / Engine::get_singleton()->get_iterations_per_second();
 
+	Engine::get_singleton()->_frame_step = step;
+
 	/*
 	if (time_accum+step < frame_slice)
 		return false;

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;
@@ -538,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);
@@ -597,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")));
@@ -620,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()
@@ -640,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() {

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();
 };

scene/3d/audio_stream_player_3d.cpp

@@ -0,0 +1,910 @@
+#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;
+				}
+
+				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()) {
+						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();
+
+						int vol_index_max = AudioServer::get_singleton()->get_speaker_mode() + 1;
+
+						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
+
+							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;
+								}
+							}
+
+							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]);
+
+							for (int i = 0; i < vol_index_max; i++) {
+
+								output.reverb_vol[i] = output.reverb_vol[i].linear_interpolate(center_frame, attenuation);
+								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;
+				}
+
+				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() {
+}

scene/3d/audio_stream_player_3d.h

@@ -0,0 +1,170 @@
+#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; }
+	};
+
+	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

scene/3d/body_shape.cpp

@@ -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

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);
 }
 

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

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 {

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;

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;
 }

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

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;
+}

scene/3d/body_shape.h → 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;
 

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) {

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() {
 }

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();

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;
+}

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

scene/gui/tree.cpp

@@ -1953,6 +1953,14 @@ void Tree::text_editor_enter(String p_text) {
 				c.val = evaluator->eval(p_text);
 			else
 				c.val = p_text.to_double();
+
+			if (c.step > 0)
+				c.val = Math::stepify(c.val, c.step);
+			if (c.val < c.min)
+				c.val = c.min;
+			else if (c.val > c.max)
+				c.val = c.max;
+
 		} break;
 		default: { ERR_FAIL(); }
 	}

scene/register_scene_types.cpp

@@ -164,7 +164,7 @@
 #include "scene/resources/polygon_path_finder.h"
 
 //#include "scene/resources/sample.h"
-//#include "scene/audio/sample_player.h"
+#include "scene/3d/audio_stream_player_3d.h"
 #include "scene/resources/material.h"
 #include "scene/resources/mesh.h"
 #include "scene/resources/room.h"
@@ -212,7 +212,7 @@
 
 #include "scene/3d/area.h"
 
-#include "scene/3d/body_shape.h"
+#include "scene/3d/collision_shape.h"
 #include "scene/3d/immediate_geometry.h"
 #include "scene/3d/multimesh_instance.h"
 #include "scene/3d/physics_joint.h"
@@ -532,8 +532,6 @@ void register_scene_types() {
 	ClassDB::register_class<SphereMesh>();
 	ClassDB::register_virtual_class<Material>();
 	ClassDB::register_class<SpatialMaterial>();
-	ClassDB::add_compatibility_class("FixedSpatialMaterial", "SpatialMaterial");
-	ClassDB::add_compatibility_class("Mesh", "ArrayMesh");
 	SceneTree::add_idle_callback(SpatialMaterial::flush_changes);
 	SpatialMaterial::init_shaders();
 
@@ -562,6 +560,7 @@ void register_scene_types() {
 
 	OS::get_singleton()->yield(); //may take time to init
 
+	ClassDB::register_class<SpatialVelocityTracker>();
 #endif
 	ClassDB::register_class<World>();
 	ClassDB::register_class<Environment>();
@@ -598,6 +597,7 @@ void register_scene_types() {
 
 	ClassDB::register_class<AudioStreamPlayer>();
 	ClassDB::register_class<AudioStreamPlayer2D>();
+	ClassDB::register_class<AudioStreamPlayer3D>();
 	ClassDB::register_virtual_class<VideoStream>();
 	ClassDB::register_class<AudioStreamSample>();
 
@@ -628,6 +628,13 @@ void register_scene_types() {
 	ClassDB::register_class<SceneTree>();
 	ClassDB::register_virtual_class<SceneTreeTimer>(); //sorry, you can't create it
 
+#ifndef DISABLE_DEPRECATED
+	ClassDB::add_compatibility_class("ImageSkyBox", "PanoramaSky");
+	ClassDB::add_compatibility_class("FixedSpatialMaterial", "SpatialMaterial");
+	ClassDB::add_compatibility_class("Mesh", "ArrayMesh");
+
+#endif
+
 	OS::get_singleton()->yield(); //may take time to init
 
 	resource_saver_text = memnew(ResourceFormatSaverText);

scene/resources/audio_stream_sample.cpp

@@ -231,7 +231,7 @@ void AudioStreamPlaybackSample::mix(AudioFrame *p_buffer, float p_rate_scale, in
 
 	/* some 64-bit fixed point precaches */
 
-	int64_t loop_begin_fp = ((int64_t)len << MIX_FRAC_BITS);
+	int64_t loop_begin_fp = ((int64_t)base->loop_begin << MIX_FRAC_BITS);
 	int64_t loop_end_fp = ((int64_t)base->loop_end << MIX_FRAC_BITS);
 	int64_t length_fp = ((int64_t)len << MIX_FRAC_BITS);
 	int64_t begin_limit = (base->loop_mode != AudioStreamSample::LOOP_DISABLED) ? loop_begin_fp : 0;

scene/resources/world.cpp

@@ -299,6 +299,13 @@ PhysicsDirectSpaceState *World::get_direct_space_state() {
 	return PhysicsServer::get_singleton()->space_get_direct_state(space);
 }
 
+void World::get_camera_list(List<Camera *> *r_cameras) {
+
+	for (Map<Camera *, SpatialIndexer::CameraData>::Element *E = indexer->cameras.front(); E; E = E->next()) {
+		r_cameras->push_back(E->key());
+	}
+}
+
 void World::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("get_space"), &World::get_space);

scene/resources/world.h

@@ -76,6 +76,8 @@ public:
 	void set_fallback_environment(const Ref<Environment> &p_environment);
 	Ref<Environment> get_fallback_environment() const;
 
+	void get_camera_list(List<Camera *> *r_cameras);
+
 	PhysicsDirectSpaceState *get_direct_space_state();
 
 	World();

servers/audio/audio_filter_sw.cpp

@@ -242,28 +242,49 @@ AudioFilterSW::Processor::Processor() {
 	set_filter(NULL);
 }
 
-void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter) {
+void AudioFilterSW::Processor::set_filter(AudioFilterSW *p_filter, bool p_clear_history) {
 
-	ha1 = ha2 = hb1 = hb2 = 0;
+	if (p_clear_history) {
+		ha1 = ha2 = hb1 = hb2 = 0;
+	}
 	filter = p_filter;
 }
 
-void AudioFilterSW::Processor::update_coeffs() {
+void AudioFilterSW::Processor::update_coeffs(int p_interp_buffer_len) {
 
 	if (!filter)
 		return;
 
-	filter->prepare_coefficients(&coeffs);
+	if (p_interp_buffer_len) { //interpolate
+		Coeffs old_coeffs = coeffs;
+		filter->prepare_coefficients(&coeffs);
+		incr_coeffs.a1 = (coeffs.a1 - old_coeffs.a1) / p_interp_buffer_len;
+		incr_coeffs.a2 = (coeffs.a2 - old_coeffs.a2) / p_interp_buffer_len;
+		incr_coeffs.b0 = (coeffs.b0 - old_coeffs.b0) / p_interp_buffer_len;
+		incr_coeffs.b1 = (coeffs.b1 - old_coeffs.b1) / p_interp_buffer_len;
+		incr_coeffs.b2 = (coeffs.b2 - old_coeffs.b2) / p_interp_buffer_len;
+		coeffs = old_coeffs;
+	} else {
+		filter->prepare_coefficients(&coeffs);
+	}
 }
 
-void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride) {
+void AudioFilterSW::Processor::process(float *p_samples, int p_amount, int p_stride, bool p_interpolate) {
 
 	if (!filter)
 		return;
 
-	for (int i = 0; i < p_amount; i++) {
+	if (p_interpolate) {
+		for (int i = 0; i < p_amount; i++) {
+
+			process_one_interp(*p_samples);
+			p_samples += p_stride;
+		}
+	} else {
+		for (int i = 0; i < p_amount; i++) {
 
-		process_one(*p_samples);
-		p_samples += p_stride;
+			process_one(*p_samples);
+			p_samples += p_stride;
+		}
 	}
 }

servers/audio/audio_filter_sw.h

@@ -60,11 +60,14 @@ public:
 		AudioFilterSW *filter;
 		Coeffs coeffs;
 		float ha1, ha2, hb1, hb2; //history
+		Coeffs incr_coeffs;
+
 	public:
-		void set_filter(AudioFilterSW *p_filter);
-		void process(float *p_samples, int p_amount, int p_stride = 1);
-		void update_coeffs();
+		void set_filter(AudioFilterSW *p_filter, bool p_clear_history = true);
+		void process(float *p_samples, int p_amount, int p_stride = 1, bool p_interpolate = false);
+		void update_coeffs(int p_interp_buffer_len = 0);
 		_ALWAYS_INLINE_ void process_one(float &p_sample);
+		_ALWAYS_INLINE_ void process_one_interp(float &p_sample);
 
 		Processor();
 	};
@@ -104,4 +107,20 @@ void AudioFilterSW::Processor::process_one(float &p_val) {
 	ha1 = p_val;
 }
 
+void AudioFilterSW::Processor::process_one_interp(float &p_val) {
+
+	float pre = p_val;
+	p_val = (p_val * coeffs.b0 + hb1 * coeffs.b1 + hb2 * coeffs.b2 + ha1 * coeffs.a1 + ha2 * coeffs.a2);
+	ha2 = ha1;
+	hb2 = hb1;
+	hb1 = pre;
+	ha1 = p_val;
+
+	coeffs.b0 += incr_coeffs.b0;
+	coeffs.b1 += incr_coeffs.b1;
+	coeffs.b2 += incr_coeffs.b2;
+	coeffs.a1 += incr_coeffs.a1;
+	coeffs.a2 += incr_coeffs.a2;
+}
+
 #endif // AUDIO_FILTER_SW_H

servers/audio/audio_stream.cpp

@@ -81,3 +81,134 @@ void AudioStreamPlaybackResampled::mix(AudioFrame *p_buffer, float p_rate_scale,
 		}
 	}
 }
+////////////////////////////////
+
+void AudioStreamRandomPitch::set_audio_stream(const Ref<AudioStream> &p_audio_stream) {
+
+	audio_stream = p_audio_stream;
+	if (audio_stream.is_valid()) {
+		for (Set<AudioStreamPlaybackRandomPitch *>::Element *E = playbacks.front(); E; E = E->next()) {
+			E->get()->playback = audio_stream->instance_playback();
+		}
+	}
+}
+
+Ref<AudioStream> AudioStreamRandomPitch::get_audio_stream() const {
+
+	return audio_stream;
+}
+
+void AudioStreamRandomPitch::set_random_pitch(float p_pitch) {
+
+	if (p_pitch < 1)
+		p_pitch = 1;
+	random_pitch = p_pitch;
+}
+
+float AudioStreamRandomPitch::get_random_pitch() const {
+	return random_pitch;
+}
+
+Ref<AudioStreamPlayback> AudioStreamRandomPitch::instance_playback() {
+	Ref<AudioStreamPlaybackRandomPitch> playback;
+	playback.instance();
+	if (audio_stream.is_valid())
+		playback->playback = audio_stream->instance_playback();
+
+	playbacks.insert(playback.ptr());
+	playback->random_pitch = Ref<AudioStreamRandomPitch>((AudioStreamRandomPitch *)this);
+	return playback;
+}
+
+String AudioStreamRandomPitch::get_stream_name() const {
+
+	if (audio_stream.is_valid()) {
+		return "Random: " + audio_stream->get_name();
+	}
+	return "RandomPitch";
+}
+
+void AudioStreamRandomPitch::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_audio_stream", "stream"), &AudioStreamRandomPitch::set_audio_stream);
+	ClassDB::bind_method(D_METHOD("get_audio_stream"), &AudioStreamRandomPitch::get_audio_stream);
+
+	ClassDB::bind_method(D_METHOD("set_random_pitch", "scale"), &AudioStreamRandomPitch::set_random_pitch);
+	ClassDB::bind_method(D_METHOD("get_random_pitch"), &AudioStreamRandomPitch::get_random_pitch);
+
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "audio_stream", PROPERTY_HINT_RESOURCE_TYPE, "AudioStream"), "set_audio_stream", "get_audio_stream");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "random_pitch", PROPERTY_HINT_RANGE, "1,16,0.01"), "set_random_pitch", "get_random_pitch");
+}
+
+AudioStreamRandomPitch::AudioStreamRandomPitch() {
+	random_pitch = 1.1;
+}
+
+void AudioStreamPlaybackRandomPitch::start(float p_from_pos) {
+	playing = playback;
+	float range_from = 1.0 / random_pitch->random_pitch;
+	float range_to = random_pitch->random_pitch;
+
+	pitch_scale = range_from + Math::randf() * (range_to - range_from);
+
+	if (playing.is_valid()) {
+		playing->start(p_from_pos);
+	}
+}
+
+void AudioStreamPlaybackRandomPitch::stop() {
+	if (playing.is_valid()) {
+		playing->stop();
+		;
+	}
+}
+bool AudioStreamPlaybackRandomPitch::is_playing() const {
+	if (playing.is_valid()) {
+		return playing->is_playing();
+	}
+
+	return false;
+}
+
+int AudioStreamPlaybackRandomPitch::get_loop_count() const {
+	if (playing.is_valid()) {
+		return playing->get_loop_count();
+	}
+
+	return 0;
+}
+
+float AudioStreamPlaybackRandomPitch::get_pos() const {
+	if (playing.is_valid()) {
+		return playing->get_pos();
+	}
+
+	return 0;
+}
+void AudioStreamPlaybackRandomPitch::seek_pos(float p_time) {
+	if (playing.is_valid()) {
+		playing->seek_pos(p_time);
+	}
+}
+
+void AudioStreamPlaybackRandomPitch::mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames) {
+	if (playing.is_valid()) {
+		playing->mix(p_bufer, p_rate_scale * pitch_scale, p_frames);
+	} else {
+		for (int i = 0; i < p_frames; i++) {
+			p_bufer[i] = AudioFrame(0, 0);
+		}
+	}
+}
+
+float AudioStreamPlaybackRandomPitch::get_length() const {
+	if (playing.is_valid()) {
+		return playing->get_length();
+	}
+
+	return 0;
+}
+
+AudioStreamPlaybackRandomPitch::~AudioStreamPlaybackRandomPitch() {
+	random_pitch->playbacks.erase(this);
+}

servers/audio/audio_stream.h

@@ -31,6 +31,7 @@
 #define AUDIO_STREAM_H
 
 #include "resource.h"
+#include "servers/audio/audio_filter_sw.h"
 #include "servers/audio_server.h"
 
 class AudioStreamPlayback : public Reference {
@@ -88,4 +89,58 @@ public:
 	virtual String get_stream_name() const = 0;
 };
 
+class AudioStreamPlaybackRandomPitch;
+
+class AudioStreamRandomPitch : public AudioStream {
+
+	GDCLASS(AudioStreamRandomPitch, AudioStream)
+	friend class AudioStreamPlaybackRandomPitch;
+
+	Set<AudioStreamPlaybackRandomPitch *> playbacks;
+	Ref<AudioStream> audio_stream;
+	float random_pitch;
+
+protected:
+	static void _bind_methods();
+
+public:
+	void set_audio_stream(const Ref<AudioStream> &audio_stream);
+	Ref<AudioStream> get_audio_stream() const;
+
+	void set_random_pitch(float p_pitch);
+	float get_random_pitch() const;
+
+	virtual Ref<AudioStreamPlayback> instance_playback();
+	virtual String get_stream_name() const;
+
+	AudioStreamRandomPitch();
+};
+
+class AudioStreamPlaybackRandomPitch : public AudioStreamPlayback {
+
+	GDCLASS(AudioStreamPlaybackRandomPitch, AudioStreamPlayback)
+	friend class AudioStreamRandomPitch;
+
+	Ref<AudioStreamRandomPitch> random_pitch;
+	Ref<AudioStreamPlayback> playback;
+	Ref<AudioStreamPlayback> playing;
+	float pitch_scale;
+
+public:
+	virtual void start(float p_from_pos = 0.0);
+	virtual void stop();
+	virtual bool is_playing() const;
+
+	virtual int get_loop_count() const; //times it looped
+
+	virtual float get_pos() const;
+	virtual void seek_pos(float p_time);
+
+	virtual void mix(AudioFrame *p_bufer, float p_rate_scale, int p_frames);
+
+	virtual float get_length() const; //if supported, otherwise return 0
+
+	~AudioStreamPlaybackRandomPitch();
+};
+
 #endif // AUDIO_STREAM_H

servers/physics/area_pair_sw.cpp

@@ -32,12 +32,13 @@
 
 bool AreaPairSW::setup(real_t p_step) {
 
-	if (!area->test_collision_mask(body)) {
-		colliding = false;
-		return false;
-	}
+	bool result = false;
 
-	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this);
+	if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
+		result = false;
+	} else if (area->test_collision_mask(body) && CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this)) {
+		result = true;
+	}
 
 	if (result != colliding) {
 
@@ -95,14 +96,13 @@ AreaPairSW::~AreaPairSW() {
 
 bool Area2PairSW::setup(real_t p_step) {
 
-	if (!area_a->test_collision_mask(area_b)) {
-		colliding = false;
-		return false;
+	bool result = false;
+	if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
+		result = false;
+	} else if (area_a->test_collision_mask(area_b) && CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this)) {
+		result = true;
 	}
 
-	//bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
-	bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this);
-
 	if (result != colliding) {
 
 		if (result) {

servers/physics/body_pair_sw.cpp

@@ -214,6 +214,11 @@ bool BodyPairSW::setup(real_t p_step) {
 		return false;
 	}
 
+	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
+		collided = false;
+		return false;
+	}
+
 	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
 
 	validate_contacts();
@@ -313,12 +318,6 @@ bool BodyPairSW::setup(real_t p_step) {
 			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
 		}
 
-		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
-			c.active = false;
-			collided = false;
-			continue;
-		}
-
 		c.active = true;
 
 		// Precompute normal mass, tangent mass, and bias.

servers/physics/broad_phase_basic.cpp

@@ -105,6 +105,26 @@ int BroadPhaseBasic::get_subindex(ID p_id) const {
 	return E->get().subindex;
 }
 
+int BroadPhaseBasic::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
+
+	int rc = 0;
+
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
+
+		const Rect3 aabb = E->get().aabb;
+		if (aabb.has_point(p_point)) {
+
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
+			rc++;
+			if (rc >= p_max_results)
+				break;
+		}
+	}
+
+	return rc;
+}
+
 int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
 	int rc = 0;

servers/physics/broad_phase_basic.h

@@ -91,6 +91,7 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 

servers/physics/broad_phase_octree.cpp

@@ -66,6 +66,11 @@ int BroadPhaseOctree::get_subindex(ID p_id) const {
 	return octree.get_subindex(p_id);
 }
 
+int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
+
+	return octree.cull_point(p_point, p_results, p_max_results, p_result_indices);
+}
+
 int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
 	return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);

servers/physics/broad_phase_octree.h

@@ -56,6 +56,7 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 

servers/physics/broad_phase_sw.h

@@ -57,6 +57,7 @@ public:
 	virtual bool is_static(ID p_id) const = 0;
 	virtual int get_subindex(ID p_id) const = 0;
 
+	virtual int cull_point(const Vector3 &p_point, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 	virtual int cull_aabb(const Rect3 &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 

servers/physics/collision_object_sw.h

@@ -64,9 +64,9 @@ private:
 		Rect3 aabb_cache; //for rayqueries
 		real_t area_cache;
 		ShapeSW *shape;
-		bool trigger;
+		bool disabled;
 
-		Shape() { trigger = false; }
+		Shape() { disabled = false; }
 	};
 
 	Vector<Shape> shapes;
@@ -131,8 +131,8 @@ public:
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
-	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx, bool p_enable) { shapes[p_idx].trigger = p_enable; }
-	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
+	_FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes[p_idx].disabled = p_enable; }
+	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { return shapes[p_idx].disabled; }
 
 	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }

servers/physics/physics_server_sw.cpp

@@ -330,6 +330,14 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) {
 		area->remove_shape(0);
 }
 
+void PhysicsServerSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
+	area->set_shape_as_disabled(p_shape_idx, p_disabled);
+}
+
 void PhysicsServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) {
 
 	if (space_owner.owns(p_area)) {
@@ -551,21 +559,12 @@ RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
 	return shape->get_self();
 }
 
-void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) {
+void PhysicsServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
-	body->set_shape_as_trigger(p_shape_idx, p_enable);
-}
-
-bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const {
-
-	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false);
-
-	return body->is_shape_set_as_trigger(p_shape_idx);
+	body->set_shape_as_disabled(p_shape_idx, p_disabled);
 }
 
 Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
@@ -875,6 +874,16 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const {
 	return body->is_ray_pickable();
 }
 
+bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin, MotionResult *r_result) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
+}
+
 /* JOINT API */
 
 RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
@@ -1530,8 +1539,9 @@ void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_
 	}
 }
 
+PhysicsServerSW *PhysicsServerSW::singleton = NULL;
 PhysicsServerSW::PhysicsServerSW() {
-
+	singleton = this;
 	BroadPhaseSW::create_func = BroadPhaseOctree::_create;
 	island_count = 0;
 	active_objects = 0;

servers/physics/physics_server_sw.h

@@ -63,6 +63,8 @@ class PhysicsServerSW : public PhysicsServer {
 
 	//void _clear_query(QuerySW *p_query);
 public:
+	static PhysicsServerSW *singleton;
+
 	struct CollCbkData {
 
 		int max;
@@ -117,6 +119,8 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx);
 	virtual void area_clear_shapes(RID p_area);
 
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled);
+
 	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID);
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
 
@@ -156,8 +160,7 @@ public:
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
 	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable);
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
 
 	virtual void body_remove_shape(RID p_body, int p_shape_idx);
 	virtual void body_clear_shapes(RID p_body);
@@ -214,6 +217,8 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool body_is_ray_pickable(RID p_body) const;
 
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+
 	/* JOINT API */
 
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);

servers/physics/shape_sw.cpp

@@ -117,6 +117,20 @@ bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en
 	return inters;
 }
 
+bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return plane.distance_to(p_point) < 0;
+}
+
+Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	if (plane.is_point_over(p_point)) {
+		return plane.project(p_point);
+	} else {
+		return p_point;
+	}
+}
+
 Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3(); //wtf
@@ -184,6 +198,21 @@ bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end,
 	return false; //simply not possible
 }
 
+bool RayShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //simply not possible
+}
+
+Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 s[2] = {
+		Vector3(0, 0, 0),
+		Vector3(0, 0, length)
+	};
+
+	return Geometry::get_closest_point_to_segment(p_point, s);
+}
+
 Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3();
@@ -245,6 +274,20 @@ bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e
 	return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
 }
 
+bool SphereShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return p_point.length() < radius;
+}
+
+Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 p = p_point;
+	float l = p.length();
+	if (l < radius)
+		return p_point;
+	return (p / l) * radius;
+}
+
 Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	real_t s = 0.4 * p_mass * radius * radius;
@@ -390,6 +433,62 @@ bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end,
 	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
 }
 
+bool BoxShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z);
+}
+
+Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	int outside = 0;
+	Vector3 min_point;
+
+	for (int i = 0; i < 3; i++) {
+
+		if (Math::abs(p_point[i]) > half_extents[i]) {
+			outside++;
+			if (outside == 1) {
+				//use plane if only one side matches
+				Vector3 n;
+				n[i] = SGN(p_point[i]);
+
+				Plane p(n, half_extents[i]);
+				min_point = p.project(p_point);
+			}
+		}
+	}
+
+	if (!outside)
+		return p_point; //it's inside, don't do anything else
+
+	if (outside == 1) //if only above one plane, this plane clearly wins
+		return min_point;
+
+	//check segments
+	float min_distance = 1e20;
+	Vector3 closest_vertex = half_extents * p_point.sign();
+	Vector3 s[2] = {
+		closest_vertex,
+		closest_vertex
+	};
+
+	for (int i = 0; i < 3; i++) {
+
+		s[1] = closest_vertex;
+		s[1][i] = -s[1][i]; //edge
+
+		Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s);
+
+		float d = p_point.distance_to(closest_edge);
+		if (d < min_distance) {
+			min_point = closest_edge;
+			min_distance = d;
+		}
+	}
+
+	return min_point;
+}
+
 Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	real_t lx = half_extents.x;
@@ -542,6 +641,32 @@ bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_
 	return collision;
 }
 
+bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	if (Math::abs(p_point.z) < height * 0.5) {
+		return Vector3(p_point.x, p_point.y, 0).length() < radius;
+	} else {
+		Vector3 p = p_point;
+		p.z = Math::abs(p.z) - height * 0.5;
+		return p.length() < radius;
+	}
+}
+
+Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	Vector3 s[2] = {
+		Vector3(0, 0, -height * 0.5),
+		Vector3(0, 0, height * 0.5),
+	};
+
+	Vector3 p = Geometry::get_closest_point_to_segment(p_point, s);
+
+	if (p.distance_to(p_point) < radius)
+		return p_point;
+
+	return p + (p_point - p).normalized() * radius;
+}
+
 Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	// use crappy AABB approximation
@@ -738,6 +863,81 @@ bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vecto
 	return col;
 }
 
+bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+
+	for (int i = 0; i < fc; i++) {
+
+		if (faces[i].plane.distance_to(p_point) >= 0)
+			return false;
+	}
+
+	return true;
+}
+
+Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+	int fc = mesh.faces.size();
+	const Vector3 *vertices = mesh.vertices.ptr();
+
+	bool all_inside = true;
+	for (int i = 0; i < fc; i++) {
+
+		if (!faces[i].plane.is_point_over(p_point))
+			continue;
+
+		all_inside = false;
+		bool is_inside = true;
+		int ic = faces[i].indices.size();
+		const int *indices = faces[i].indices.ptr();
+
+		for (int j = 0; j < ic; j++) {
+
+			Vector3 a = vertices[indices[j]];
+			Vector3 b = vertices[indices[(j + 1) % ic]];
+			Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
+			if (Plane(a, n).is_point_over(p_point)) {
+				is_inside = false;
+				break;
+			}
+		}
+
+		if (is_inside) {
+			return faces[i].plane.project(p_point);
+		}
+	}
+
+	if (all_inside) {
+		return p_point;
+	}
+
+	float min_distance = 1e20;
+	Vector3 min_point;
+
+	//check edges
+	const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+	int ec = mesh.edges.size();
+	for (int i = 0; i < ec; i++) {
+
+		Vector3 s[2] = {
+			vertices[edges[i].a],
+			vertices[edges[i].b]
+		};
+
+		Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s);
+		float d = closest.distance_to(p_point);
+		if (d < min_distance) {
+			min_distance = d;
+			min_point = closest;
+		}
+	}
+
+	return min_point;
+}
+
 Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	// use crappy AABB approximation
@@ -880,6 +1080,16 @@ bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end
 	return c;
 }
 
+bool FaceShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //face is flat
+}
+
+Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
+}
+
 Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
@@ -1046,6 +1256,16 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vect
 	}
 }
 
+bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const {
+
+	return false; //face is flat
+}
+
+Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Vector3();
+}
+
 void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
 
 	const BVH *bvh = &p_params->bvh[p_idx];
@@ -1471,6 +1691,15 @@ bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 	return false;
 }
 
+bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const {
+	return false;
+}
+
+Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const {
+
+	return Vector3();
+}
+
 void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const {
 }
 

servers/physics/shape_sw.h

@@ -87,8 +87,9 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0;
-
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0;
+	virtual bool intersect_point(const Vector3 &p_point) const = 0;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0;
 
 	virtual void set_data(const Variant &p_data) = 0;
@@ -134,7 +135,8 @@ public:
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
-
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
 	virtual void set_data(const Variant &p_data);
@@ -159,6 +161,8 @@ public:
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -185,6 +189,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -209,6 +215,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -237,6 +245,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -261,6 +271,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -338,6 +350,8 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
@@ -372,7 +386,9 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
 
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
@@ -397,6 +413,8 @@ struct FaceShapeSW : public ShapeSW {
 	Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
 
 	Vector3 get_moment_of_inertia(real_t p_mass) const;
 
@@ -436,6 +454,8 @@ struct MotionShapeSW : public ShapeSW {
 	}
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; }
+	virtual bool intersect_point(const Vector3 &p_point) const { return false; }
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; }
 
 	Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); }
 

servers/physics/space_sw.cpp

@@ -45,6 +45,50 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object,
 	return (1 << body->get_mode()) & p_type_mask;
 }
 
+int PhysicsDirectSpaceStateSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask) {
+
+	ERR_FAIL_COND_V(space->locked, false);
+	int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int cc = 0;
+
+	//Transform ai = p_xform.affine_inverse();
+
+	for (int i = 0; i < amount; i++) {
+
+		if (cc >= p_result_max)
+			break;
+
+		if (!_match_object_type_query(space->intersection_query_results[i], p_collision_layer, p_object_type_mask))
+			continue;
+
+		//area can't be picked by ray (default)
+
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
+			continue;
+
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		Transform inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+		inv_xform.affine_invert();
+
+		if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point)))
+			continue;
+
+		r_results[cc].collider_id = col_obj->get_instance_id();
+		if (r_results[cc].collider_id != 0)
+			r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
+		else
+			r_results[cc].collider = NULL;
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+
+		cc++;
+	}
+
+	return cc;
+}
+
 bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_layer, uint32_t p_object_type_mask, bool p_pick_ray) {
 
 	ERR_FAIL_COND_V(space->locked, false);
@@ -428,6 +472,48 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
 	return true;
 }
 
+Vector3 PhysicsDirectSpaceStateSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
+
+	CollisionObjectSW *obj = NULL;
+	obj = PhysicsServerSW::singleton->area_owner.getornull(p_object);
+	if (!obj) {
+		obj = PhysicsServerSW::singleton->body_owner.getornull(p_object);
+	}
+	ERR_FAIL_COND_V(!obj, Vector3());
+
+	ERR_FAIL_COND_V(obj->get_space() != space, Vector3());
+
+	float min_distance = 1e20;
+	Vector3 min_point;
+
+	bool shapes_found = false;
+
+	for (int i = 0; i < obj->get_shape_count(); i++) {
+
+		if (obj->is_shape_set_as_disabled(i))
+			continue;
+
+		Transform shape_xform = obj->get_transform() * obj->get_shape_transform(i);
+		ShapeSW *shape = obj->get_shape(i);
+
+		Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point));
+		point = shape_xform.xform(point);
+
+		float dist = point.distance_to(p_point);
+		if (dist < min_distance) {
+			min_distance = dist;
+			min_point = point;
+		}
+		shapes_found = true;
+	}
+
+	if (!shapes_found) {
+		return obj->get_transform().origin; //no shapes found, use distance to origin.
+	} else {
+		return min_point;
+	}
+}
+
 PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
 	space = NULL;
@@ -435,6 +521,337 @@ PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+int SpaceSW::_cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb) {
+
+	int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
+
+	for (int i = 0; i < amount; i++) {
+
+		bool keep = true;
+
+		if (intersection_query_results[i] == p_body)
+			keep = false;
+		else if (intersection_query_results[i]->get_type() == CollisionObjectSW::TYPE_AREA)
+			keep = false;
+		else if ((static_cast<BodySW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0)
+			keep = false;
+		else if (static_cast<BodySW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
+			keep = false;
+		else if (static_cast<BodySW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i]))
+			keep = false;
+
+		if (!keep) {
+
+			if (i < amount - 1) {
+				SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
+				SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
+			}
+
+			amount--;
+			i--;
+		}
+	}
+
+	return amount;
+}
+
+bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result) {
+
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	if (r_result) {
+		r_result->collider_id = 0;
+		r_result->collider_shape = 0;
+	}
+	Rect3 body_aabb;
+
+	for (int i = 0; i < p_body->get_shape_count(); i++) {
+
+		if (i == 0)
+			body_aabb = p_body->get_shape_aabb(i);
+		else
+			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
+	}
+
+	// Undo the currently transform the physics server is aware of and apply the provided one
+	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
+	body_aabb = body_aabb.grow(p_margin);
+
+	Transform body_transform = p_from;
+
+	{
+		//STEP 1, FREE BODY IF STUCK
+
+		const int max_results = 32;
+		int recover_attempts = 4;
+		Vector3 sr[max_results * 2];
+
+		do {
+
+			PhysicsServerSW::CollCbkData cbk;
+			cbk.max = max_results;
+			cbk.amount = 0;
+			cbk.ptr = sr;
+
+			CollisionSolverSW::CallbackResult cbkres = NULL;
+
+			PhysicsServerSW::CollCbkData *cbkptr = NULL;
+			cbkptr = &cbk;
+			cbkres = PhysicsServerSW::_shape_col_cbk;
+
+			bool collided = false;
+
+			int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+			for (int j = 0; j < p_body->get_shape_count(); j++) {
+				if (p_body->is_shape_set_as_disabled(j))
+					continue;
+
+				Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
+				ShapeSW *body_shape = p_body->get_shape(j);
+				for (int i = 0; i < amount; i++) {
+
+					const CollisionObjectSW *col_obj = intersection_query_results[i];
+					int shape_idx = intersection_query_subindex_results[i];
+
+					if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
+						collided = cbk.amount > 0;
+					}
+				}
+			}
+
+			if (!collided) {
+				break;
+			}
+
+			Vector3 recover_motion;
+
+			for (int i = 0; i < cbk.amount; i++) {
+
+				Vector3 a = sr[i * 2 + 0];
+				Vector3 b = sr[i * 2 + 1];
+
+#if 0
+				Vector3 rel = b-a;
+				real_t d = rel.length();
+				if (d==0)
+					continue;
+
+				Vector3 n = rel/d;
+				real_t traveled = n.dot(recover_motion);
+				a+=n*traveled;
+
+				real_t d = a.distance_to(b);
+				if (d<margin)
+					continue;
+#endif
+				recover_motion += (b - a) * 0.4;
+			}
+
+			if (recover_motion == Vector3()) {
+				collided = false;
+				break;
+			}
+
+			body_transform.origin += recover_motion;
+			body_aabb.position += recover_motion;
+
+			recover_attempts--;
+
+		} while (recover_attempts);
+	}
+
+	real_t safe = 1.0;
+	real_t unsafe = 1.0;
+	int best_shape = -1;
+
+	{
+		// STEP 2 ATTEMPT MOTION
+
+		Rect3 motion_aabb = body_aabb;
+		motion_aabb.position += p_motion;
+		motion_aabb = motion_aabb.merge(body_aabb);
+
+		int amount = _cull_aabb_for_body(p_body, motion_aabb);
+
+		for (int j = 0; j < p_body->get_shape_count(); j++) {
+
+			if (p_body->is_shape_set_as_disabled(j))
+				continue;
+
+			Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
+			ShapeSW *body_shape = p_body->get_shape(j);
+
+			Transform body_shape_xform_inv = body_shape_xform.affine_inverse();
+			MotionShapeSW mshape;
+			mshape.shape = body_shape;
+			mshape.motion = body_shape_xform_inv.basis.xform(p_motion);
+
+			bool stuck = false;
+
+			real_t best_safe = 1;
+			real_t best_unsafe = 1;
+
+			for (int i = 0; i < amount; i++) {
+
+				const CollisionObjectSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
+
+				//test initial overlap, does it collide if going all the way?
+				Vector3 point_A, point_B;
+				Vector3 sep_axis = p_motion.normalized();
+
+				Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+				//test initial overlap, does it collide if going all the way?
+				if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+					//print_line("failed motion cast (no collision)");
+					continue;
+				}
+				sep_axis = p_motion.normalized();
+
+				if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+					//print_line("failed motion cast (no collision)");
+					stuck = true;
+					break;
+				}
+
+				//just do kinematic solving
+				real_t low = 0;
+				real_t hi = 1;
+				Vector3 mnormal = p_motion.normalized();
+
+				for (int i = 0; i < 8; i++) { //steps should be customizable..
+
+					real_t ofs = (low + hi) * 0.5;
+
+					Vector3 sep = mnormal; //important optimization for this to work fast enough
+
+					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
+
+					Vector3 lA, lB;
+
+					bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
+
+					if (collided) {
+
+						//print_line(itos(i)+": "+rtos(ofs));
+						hi = ofs;
+					} else {
+
+						point_A = lA;
+						point_B = lB;
+						low = ofs;
+					}
+				}
+
+				if (low < best_safe) {
+					best_safe = low;
+					best_unsafe = hi;
+				}
+			}
+
+			if (stuck) {
+
+				safe = 0;
+				unsafe = 0;
+				best_shape = j; //sadly it's the best
+				break;
+			}
+			if (best_safe == 1.0) {
+				continue;
+			}
+			if (best_safe < safe) {
+
+				safe = best_safe;
+				unsafe = best_unsafe;
+				best_shape = j;
+			}
+		}
+	}
+
+	bool collided = false;
+	if (safe >= 1) {
+		//not collided
+		collided = false;
+		if (r_result) {
+
+			r_result->motion = p_motion;
+			r_result->remainder = Vector3();
+			r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+		}
+
+	} else {
+
+		//it collided, let's get the rest info in unsafe advance
+		Transform ugt = body_transform;
+		ugt.origin += p_motion * unsafe;
+
+		_RestCallbackData rcd;
+		rcd.best_len = 0;
+		rcd.best_object = NULL;
+		rcd.best_shape = 0;
+
+		Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
+		ShapeSW *body_shape = p_body->get_shape(best_shape);
+
+		body_aabb.position += p_motion * unsafe;
+
+		int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+		for (int i = 0; i < amount; i++) {
+
+			const CollisionObjectSW *col_obj = intersection_query_results[i];
+			int shape_idx = intersection_query_subindex_results[i];
+
+			rcd.object = col_obj;
+			rcd.shape = shape_idx;
+			bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin);
+			if (!sc)
+				continue;
+		}
+
+		if (rcd.best_len != 0) {
+
+			if (r_result) {
+				r_result->collider = rcd.best_object->get_self();
+				r_result->collider_id = rcd.best_object->get_instance_id();
+				r_result->collider_shape = rcd.best_shape;
+				r_result->collision_local_shape = best_shape;
+				r_result->collision_normal = rcd.best_normal;
+				r_result->collision_point = rcd.best_contact;
+				//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
+
+				const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
+				//Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin();
+				//				r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
+				r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
+
+				r_result->motion = safe * p_motion;
+				r_result->remainder = p_motion - safe * p_motion;
+				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+			}
+
+			collided = true;
+		} else {
+			if (r_result) {
+
+				r_result->motion = p_motion;
+				r_result->remainder = Vector3();
+				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+			}
+
+			collided = false;
+		}
+	}
+
+	return collided;
+}
+
 void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
 
 	CollisionObjectSW::Type type_A = A->get_type();

servers/physics/space_sw.h

@@ -47,11 +47,13 @@ class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
 public:
 	SpaceSW *space;
 
+	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false);
 	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL);
 	virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const;
 
 	PhysicsDirectSpaceStateSW();
 };
@@ -120,6 +122,8 @@ private:
 
 	friend class PhysicsDirectSpaceStateSW;
 
+	int _cull_aabb_for_body(BodySW *p_body, const Rect3 &p_aabb);
+
 public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
@@ -192,6 +196,8 @@ public:
 	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
 	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
 
+	bool test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer::MotionResult *r_result);
+
 	SpaceSW();
 	~SpaceSW();
 };

servers/physics_2d/space_2d_sw.cpp

@@ -809,162 +809,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	}
 
 	return collided;
-
-#if 0
-	//give me back regular physics engine logic
-	//this is madness
-	//and most people using this function will think
-	//what it does is simpler than using physics
-	//this took about a week to get right..
-	//but is it right? who knows at this point..
-
-
-	colliding=false;
-	ERR_FAIL_COND_V(!is_inside_tree(),Vector2());
-	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
-	ERR_FAIL_COND_V(!dss,Vector2());
-	const int max_shapes=32;
-	Vector2 sr[max_shapes*2];
-	int res_shapes;
-
-	Set<RID> exclude;
-	exclude.insert(get_rid());
-
-
-	//recover first
-	int recover_attempts=4;
-
-	bool collided=false;
-	uint32_t mask=0;
-	if (collide_static)
-		mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
-	if (collide_kinematic)
-		mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
-	if (collide_rigid)
-		mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
-	if (collide_character)
-		mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
-
-	//print_line("motion: "+p_motion+" margin: "+rtos(margin));
-
-	//print_line("margin: "+rtos(margin));
-	do {
-
-		//motion recover
-		for(int i=0;i<get_shape_count();i++) {
-
-			if (is_shape_set_as_trigger(i))
-				continue;
-			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),Vector2(),margin,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask))
-				collided=true;
-
-		}
-
-		if (!collided)
-			break;
-
-		Vector2 recover_motion;
-
-		for(int i=0;i<res_shapes;i++) {
-
-			Vector2 a = sr[i*2+0];
-			Vector2 b = sr[i*2+1];
-
-			real_t d = a.distance_to(b);
-
-			/*
-			if (d<margin)
-				continue;
-			*/
-			recover_motion+=(b-a)*0.4;
-		}
-
-		if (recover_motion==Vector2()) {
-			collided=false;
-			break;
-		}
-
-		Matrix32 gt = get_global_transform();
-		gt.elements[2]+=recover_motion;
-		set_global_transform(gt);
-
-		recover_attempts--;
-
-	} while (recover_attempts);
-
-
-	//move second
-	real_t safe = 1.0;
-	real_t unsafe = 1.0;
-	int best_shape=-1;
-
-	for(int i=0;i<get_shape_count();i++) {
-
-		if (is_shape_set_as_trigger(i))
-			continue;
-
-		real_t lsafe,lunsafe;
-		bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion, 0,lsafe,lunsafe,exclude,get_layer_mask(),mask);
-		//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
-		if (!valid) {
-
-			safe=0;
-			unsafe=0;
-			best_shape=i; //sadly it's the best
-			break;
-		}
-		if (lsafe==1.0) {
-			continue;
-		}
-		if (lsafe < safe) {
-
-			safe=lsafe;
-			unsafe=lunsafe;
-			best_shape=i;
-		}
-	}
-
-
-	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
-
-	if (safe>=1) {
-		//not collided
-		colliding=false;
-	} else {
-
-		//it collided, let's get the rest info in unsafe advance
-		Matrix32 ugt = get_global_transform();
-		ugt.elements[2]+=p_motion*unsafe;
-		Physics2DDirectSpaceState::ShapeRestInfo rest_info;
-		bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), Vector2(), margin,&rest_info,exclude,get_layer_mask(),mask);
-		if (!c2) {
-			//should not happen, but floating point precision is so weird..
-
-			colliding=false;
-		} else {
-
-
-			//print_line("Travel: "+rtos(travel));
-			colliding=true;
-			collision=rest_info.point;
-			normal=rest_info.normal;
-			collider=rest_info.collider_id;
-			collider_vel=rest_info.linear_velocity;
-			collider_shape=rest_info.shape;
-			collider_metadata=rest_info.metadata;
-		}
-
-	}
-
-	Vector2 motion=p_motion*safe;
-	Matrix32 gt = get_global_transform();
-	gt.elements[2]+=motion;
-	set_global_transform(gt);
-
-	return p_motion-motion;
-
-#endif
-	return false;
 }
 
 void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {

servers/physics_server.h

@@ -156,26 +156,28 @@ protected:
 	static void _bind_methods();
 
 public:
-	struct RayResult {
+	struct ShapeResult {
 
-		Vector3 position;
-		Vector3 normal;
 		RID rid;
 		ObjectID collider_id;
 		Object *collider;
 		int shape;
 	};
 
-	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0;
+	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
-	struct ShapeResult {
+	struct RayResult {
 
+		Vector3 position;
+		Vector3 normal;
 		RID rid;
 		ObjectID collider_id;
 		Object *collider;
 		int shape;
 	};
 
+	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0;
+
 	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	struct ShapeRestInfo {
@@ -194,6 +196,8 @@ public:
 
 	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_layer = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const = 0;
+
 	PhysicsDirectSpaceState();
 };
 
@@ -322,6 +326,8 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0;
 	virtual void area_clear_shapes(RID p_area) = 0;
 
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) = 0;
+
 	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0;
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0;
 
@@ -370,12 +376,11 @@ public:
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0;
 	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0;
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0;
-
 	virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0;
 	virtual void body_clear_shapes(RID p_body) = 0;
 
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) = 0;
+
 	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0;
 	virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0;
 
@@ -458,6 +463,23 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;
 	virtual bool body_is_ray_pickable(RID p_body) const = 0;
 
+	struct MotionResult {
+
+		Vector3 motion;
+		Vector3 remainder;
+
+		Vector3 collision_point;
+		Vector3 collision_normal;
+		Vector3 collider_velocity;
+		int collision_local_shape;
+		ObjectID collider_id;
+		RID collider;
+		int collider_shape;
+		Variant collider_metadata;
+	};
+
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0;
+
 	/* JOINT API */
 
 	enum JointType {

servers/register_server_types.cpp

@@ -84,6 +84,7 @@ void register_server_types() {
 
 	ClassDB::register_virtual_class<AudioStream>();
 	ClassDB::register_virtual_class<AudioStreamPlayback>();
+	ClassDB::register_class<AudioStreamRandomPitch>();
 	ClassDB::register_virtual_class<AudioEffect>();
 	ClassDB::register_class<AudioBusLayout>();
 

servers/visual/visual_server_scene.cpp

@@ -1482,6 +1482,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 
 				// a pre pass will need to be needed to determine the actual z-near to be used
 
+				Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2));
+
 				for (int j = 0; j < cull_count; j++) {
 
 					float min, max;
@@ -1494,6 +1496,8 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 					}
 
 					instance->transformed_aabb.project_range_in_plane(Plane(z_vec, 0), min, max);
+					instance->depth = near_plane.distance_to(instance->transform.origin);
+					instance->depth_layer = 0;
 					if (max > z_max)
 						z_max = max;
 				}
@@ -1539,6 +1543,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 						planes[4] = p_instance->transform.xform(Plane(Vector3(0, -1, z).normalized(), radius));
 
 						int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
+						Plane near_plane(p_instance->transform.origin, p_instance->transform.basis.get_axis(2) * z);
 
 						for (int j = 0; j < cull_count; j++) {
 
@@ -1547,6 +1552,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 								cull_count--;
 								SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 								j--;
+							} else {
+								instance->depth = near_plane.distance_to(instance->transform.origin);
+								instance->depth_layer = 0;
 							}
 						}
 
@@ -1587,6 +1595,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 
 						int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
 
+						Plane near_plane(xform.origin, -xform.basis.get_axis(2));
 						for (int j = 0; j < cull_count; j++) {
 
 							Instance *instance = instance_shadow_cull_result[j];
@@ -1594,6 +1603,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 								cull_count--;
 								SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 								j--;
+							} else {
+								instance->depth = near_plane.distance_to(instance->transform.origin);
+								instance->depth_layer = 0;
 							}
 						}
 
@@ -1619,6 +1631,7 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 			Vector<Plane> planes = cm.get_projection_planes(p_instance->transform);
 			int cull_count = p_scenario->octree.cull_convex(planes, instance_shadow_cull_result, MAX_INSTANCE_CULL, VS::INSTANCE_GEOMETRY_MASK);
 
+			Plane near_plane(p_instance->transform.origin, -p_instance->transform.basis.get_axis(2));
 			for (int j = 0; j < cull_count; j++) {
 
 				Instance *instance = instance_shadow_cull_result[j];
@@ -1626,6 +1639,9 @@ void VisualServerScene::_light_instance_update_shadow(Instance *p_instance, cons
 					cull_count--;
 					SWAP(instance_shadow_cull_result[j], instance_shadow_cull_result[cull_count]);
 					j--;
+				} else {
+					instance->depth = near_plane.distance_to(instance->transform.origin);
+					instance->depth_layer = 0;
 				}
 			}
 
@@ -3344,6 +3360,8 @@ void VisualServerScene::_update_dirty_instance(Instance *p_instance) {
 					for (int i = 0; i < dp; i++) {
 
 						RID mesh = VSG::storage->particles_get_draw_pass_mesh(p_instance->base, i);
+						if (!mesh.is_valid())
+							continue;
 
 						int sc = VSG::storage->mesh_get_surface_count(mesh);
 						for (int j = 0; j < sc; j++) {