3D Physics and Other Stuff

-=-=-=-=-=-=-=-=-=-=-=-=-= -New Vehicle (Based on Bullet's RaycastVehicle) - Vehiclebody/VehicleWheel. Demo will come soon, old vehicle (CarBody) will go away soon too. -A lot of fixes to the 3D physics engine -Added KinematicBody with demo -Fixed the space query API for 2D (demo will come soon). 3D is WIP. -Fixed long-standing bug with body_enter/body_exit for Area and Area2D -Performance variables now includes physics (active bodies, collision pairs and islands) -Ability to see what's inside of instanced scenes! -Fixed Blend Shapes (no bs+skeleton yet) -Added an Android JavaClassWrapper singleton for using Android native classes directly from GDScript. This is very Alpha!
author: Juan Linietsky <reduzio@gmail.com> 2014-09-02 23:13:40 -0300
committer: Juan Linietsky <reduzio@gmail.com> 2014-09-02 23:13:40 -0300
commit: 1a2cb755e2d8b9d59178f36702f6dff7235b9088 (patch)
tree: 4a88f47c8b984522e36ac973accb34bdcb00363b
parent: 89fa70706f9166765c3ac3f799225a467800f065 (diff)
82 files changed, 5153 insertions, 848 deletions
diff --git a/SConstruct b/SConstruct
index 4ec27b7ea4..de7256f6bc 100644
--- a/SConstruct
+++ b/SConstruct
@@ -205,8 +205,9 @@ for p in platform_list:
 
 	flag_list = platform_flags[p]
 	for f in flag_list:
-		env[f[0]] = f[1]
-		print(f[0]+":"+f[1])
+		if not (f[0] in ARGUMENTS): # allow command line to override platform flags
+			env[f[0]] = f[1]
+			print(f[0]+":"+f[1])
 
 	env.module_list=[]
 
diff --git a/core/global_constants.cpp b/core/global_constants.cpp
index efa72b6547..ae4abc627d 100644
--- a/core/global_constants.cpp
+++ b/core/global_constants.cpp
@@ -445,15 +445,26 @@ static _GlobalConstant _global_constants[]={
 	BIND_GLOBAL_CONSTANT( ERR_BUG ), ///< a bug in the software certainly happened ), due to a double check failing or unexpected behavior.
 	BIND_GLOBAL_CONSTANT( ERR_WTF  ),
 
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_NONE  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RANGE  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_RANGE  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_ENUM  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_LENGTH  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FLAGS  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FILE  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_DIR  ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RESOURCE_TYPE  ),
+
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_NONE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RANGE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_RANGE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_ENUM ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_EASING ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_LENGTH ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_KEY_ACCEL ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FLAGS ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_ALL_FLAGS ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FILE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_DIR ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_GLOBAL_FILE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_GLOBAL_DIR ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RESOURCE_TYPE ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_MULTILINE_TEXT ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_COLOR_NO_ALPHA ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_IMAGE_COMPRESS_LOSSY ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_IMAGE_COMPRESS_LOSSLESS  ),
+
 
 	BIND_GLOBAL_CONSTANT( PROPERTY_USAGE_STORAGE  ),
 	BIND_GLOBAL_CONSTANT( PROPERTY_USAGE_STORAGE  ),
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 959f7cd0a8..d2f2408829 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -111,6 +111,12 @@ struct Vector3 {
         _FORCE_INLINE_ real_t distance_to(const Vector3& p_b) const;
         _FORCE_INLINE_ real_t distance_squared_to(const Vector3& p_b) const;
 
+
+
+	_FORCE_INLINE_ Vector3 slide(const Vector3& p_vec) const;
+	_FORCE_INLINE_ Vector3 reflect(const Vector3& p_vec) const;
+
+
         /* Operators */
 
         _FORCE_INLINE_ Vector3& operator+=(const Vector3& p_v);
@@ -368,6 +374,16 @@ void Vector3::zero() {
 	x=y=z=0;
 }
 
+Vector3 Vector3::slide(const Vector3& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec);
+}
+Vector3 Vector3::reflect(const Vector3& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec) * 2.0;
+
+}
+
 #endif
 
 #endif // VECTOR3_H
diff --git a/core/variant_call.cpp b/core/variant_call.cpp
index 2697e6f7a7..8fbccc87ae 100644
--- a/core/variant_call.cpp
+++ b/core/variant_call.cpp
@@ -331,6 +331,9 @@ static void _call_##m_type##_##m_method(Variant& r_ret,Variant& p_self,const Var
 	VCALL_LOCALMEM0R(Vector3, abs);
 	VCALL_LOCALMEM1R(Vector3, distance_to);
 	VCALL_LOCALMEM1R(Vector3, distance_squared_to);
+	VCALL_LOCALMEM1R(Vector3, slide);
+	VCALL_LOCALMEM1R(Vector3, reflect);
+
 
 	VCALL_LOCALMEM0R(Plane,normalized);
 	VCALL_LOCALMEM0R(Plane,center);
@@ -1236,6 +1239,8 @@ _VariantCall::addfunc(Variant::m_vtype,Variant::m_ret,_SCS(#m_method),VCALL(m_cl
 	ADDFUNC0(VECTOR3,VECTOR3,Vector3,abs,varray());
 	ADDFUNC1(VECTOR3,REAL,Vector3,distance_to,VECTOR3,"b",varray());
 	ADDFUNC1(VECTOR3,REAL,Vector3,distance_squared_to,VECTOR3,"b",varray());
+	ADDFUNC1(VECTOR3,VECTOR3,Vector3,slide,VECTOR3,"by",varray());
+	ADDFUNC1(VECTOR3,VECTOR3,Vector3,reflect,VECTOR3,"by",varray());
 
 	ADDFUNC0(PLANE,PLANE,Plane,normalized,varray());
 	ADDFUNC0(PLANE,VECTOR3,Plane,center,varray());
diff --git a/core/variant_op.cpp b/core/variant_op.cpp
index 6c2667c7e9..9c489c5ef2 100644
--- a/core/variant_op.cpp
+++ b/core/variant_op.cpp
@@ -1145,6 +1145,7 @@ void Variant::set(const Variant& p_index, const Variant& p_value, bool *r_valid)
 			if (p_value.type!=Variant::VECTOR3)
 				return;
 
+
 			if (p_index.get_type()==Variant::STRING) {
 				//scalar name
 
@@ -1181,6 +1182,24 @@ void Variant::set(const Variant& p_index, const Variant& p_value, bool *r_valid)
 					v->set_axis(index,p_value);
 					return;
 				}
+			} else if (p_index.get_type()==Variant::STRING) {
+
+				const String *str=reinterpret_cast<const String*>(p_index._data._mem);
+				Matrix3 *v=_data._matrix3;
+
+				if (*str=="x") {
+					valid=true;
+					v->set_axis(0,p_value);
+					return;
+				} else if (*str=="y" ) {
+					valid=true;
+					v->set_axis(1,p_value);
+					return;
+				} else if (*str=="z" ) {
+					valid=true;
+					v->set_axis(2,p_value);
+					return;
+				}
 			}
 
 		} break;
@@ -2021,6 +2040,21 @@ Variant Variant::get(const Variant& p_index, bool *r_valid) const {
 					valid=true;
 					return v->get_axis(index);
 				}
+			} else if (p_index.get_type()==Variant::STRING) {
+
+				const String *str=reinterpret_cast<const String*>(p_index._data._mem);
+				const Matrix3 *v=_data._matrix3;
+
+				if (*str=="x") {
+					valid=true;
+					return v->get_axis(0);
+				} else if (*str=="y" ) {
+					valid=true;
+					return v->get_axis(1);
+				} else if (*str=="z" ) {
+					valid=true;
+					return v->get_axis(2);
+				}
 			}
 
 		} break;
diff --git a/demos/2d/platformer/stage.xml b/demos/2d/platformer/stage.xml
index 6a112e02aa..78d0f9ae2c 100644
--- a/demos/2d/platformer/stage.xml
+++ b/demos/2d/platformer/stage.xml
@@ -1,19 +1,20 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <resource_file type="PackedScene" subresource_count="9" version="1.0" version_name="Godot Engine v1.0.3917-beta1">
-	<ext_resource path="res://music.ogg" type="AudioStream"></ext_resource>
 	<ext_resource path="res://tileset.xml" type="TileSet"></ext_resource>
+	<ext_resource path="res://music.ogg" type="AudioStream"></ext_resource>
 	<ext_resource path="res://coin.xml" type="PackedScene"></ext_resource>
 	<ext_resource path="res://player.xml" type="PackedScene"></ext_resource>
-	<ext_resource path="res://moving_platform.xml" type="PackedScene"></ext_resource>
 	<ext_resource path="res://seesaw.xml" type="PackedScene"></ext_resource>
+	<ext_resource path="res://moving_platform.xml" type="PackedScene"></ext_resource>
 	<ext_resource path="res://enemy.xml" type="PackedScene"></ext_resource>
 	<ext_resource path="res://parallax_bg.xml" type="PackedScene"></ext_resource>
 	<main_resource>
 		<dictionary name="_bundled" shared="false">
 			<string> "names" </string>
-			<string_array  len="119">
+			<string_array  len="122">
 				<string> "stage" </string>
 				<string> "Node" </string>
+				<string> "_import_path" </string>
 				<string> "__meta__" </string>
 				<string> "tile_map" </string>
 				<string> "TileMap" </string>
@@ -28,7 +29,9 @@
 				<string> "quadrant_size" </string>
 				<string> "tile_set" </string>
 				<string> "tile_data" </string>
-				<string> "collision_layers" </string>
+				<string> "collision/friction" </string>
+				<string> "collision/bounce" </string>
+				<string> "collision/layers" </string>
 				<string> "coins" </string>
 				<string> "coin" </string>
 				<string> "Area2D" </string>
@@ -140,6 +143,7 @@
 			<int> 66 </int>
 			<string> "variants" </string>
 			<array  len="96" shared="false">
+				<node_path> "" </node_path>
 				<dictionary  shared="false">
 					<string> "__editor_plugin_states__" </string>
 					<dictionary  shared="false">
@@ -164,7 +168,7 @@
 							<string> "use_snap" </string>
 							<bool> False </bool>
 							<string> "ofs" </string>
-							<vector2> 418.81, 615.088 </vector2>
+							<vector2> -177.089, 415.221 </vector2>
 							<string> "snap" </string>
 							<int> 10 </int>
 						</dictionary>
@@ -318,7 +322,7 @@
 				<vector2> 4236.75, 541.058 </vector2>
 				<vector2> 4172.75, 541.058 </vector2>
 				<resource  resource_type="PackedScene" path="res://player.xml">  </resource>
-				<vector2> 236.879, 1051.15 </vector2>
+				<vector2> 251.684, 1045.6 </vector2>
 				<resource  resource_type="PackedScene" path="res://moving_platform.xml">  </resource>
 				<vector2> 1451.86, 742.969 </vector2>
 				<vector2> 0, 140 </vector2>
@@ -349,16 +353,15 @@
 				<real> -202 </real>
 				<real> 358 </real>
 				<real> -10 </real>
-				<node_path> "" </node_path>
 				<int> 2 </int>
-				<real> 14 </real>
+				<real> 7 </real>
 				<real> 14.769231 </real>
-				<string> "This is a simple demo on how to make a platformer game with Godot.&#10;This version uses physics and the 2D physics engine for motion and collision.&#10;&#10;The demo also shows the benefits of using the scene system, where coins,&#10;enemies and the player are edited separatedly and instanced in the stage.&#10;&#10;To edit the base tiles for the  tileset, open the tileset_edit.xml file and follow &#10;instructions.&#10;" </string>
+				<string> "This is a simple demo on how to make a platformer game with Godot.&#22;This version uses physics and the 2D physics engine for motion and collision.&#22;&#22;The demo also shows the benefits of using the scene system, where coins,&#22;enemies and the player are edited separatedly and instanced in the stage.&#22;&#22;To edit the base tiles for the  tileset, open the tileset_edit.xml file and follow &#22;instructions.&#22;" </string>
 				<int> 0 </int>
 				<real> -1 </real>
 			</array>
 			<string> "nodes" </string>
-			<int_array  len="690"> 				-1, -1, 1, 0, -1, 1, 2, 0, 0, 0, 0, 4, 3, -1, 13, 5, 1, 6, 2, 7, 2, 8, 3, 9, 4, 10, 5, 11, 6, 12, 7, 13, 8, 14, 9, 15, 10, 16, 11, 2, 12, 0, 0, 0, 1, 17, -1, 1, 2, 13, 0, 2, 0, 19, 18, 14, 1, 9, 15, 0, 2, 0, 19, 20, 14, 1, 9, 16, 0, 2, 0, 19, 21, 14, 1, 9, 17, 0, 2, 0, 19, 22, 14, 1, 9, 18, 0, 2, 0, 19, 23, 14, 1, 9, 19, 0, 2, 0, 19, 24, 14, 1, 9, 20, 0, 2, 0, 19, 25, 14, 1, 9, 21, 0, 2, 0, 19, 26, 14, 1, 9, 22, 0, 2, 0, 19, 27, 14, 1, 9, 23, 0, 2, 0, 19, 28, 14, 1, 9, 24, 0, 2, 0, 19, 29, 14, 1, 9, 25, 0, 2, 0, 19, 30, 14, 1, 9, 26, 0, 2, 0, 19, 31, 14, 1, 9, 27, 0, 2, 0, 19, 32, 14, 1, 9, 28, 0, 2, 0, 19, 33, 14, 1, 9, 29, 0, 2, 0, 19, 34, 14, 1, 9, 30, 0, 2, 0, 19, 35, 14, 1, 9, 31, 0, 2, 0, 19, 36, 14, 1, 9, 32, 0, 2, 0, 19, 37, 14, 1, 9, 33, 0, 2, 0, 19, 38, 14, 1, 9, 34, 0, 2, 0, 19, 39, 14, 1, 9, 35, 0, 2, 0, 19, 40, 14, 1, 9, 36, 0, 2, 0, 19, 41, 14, 1, 9, 37, 0, 2, 0, 19, 42, 14, 1, 9, 38, 0, 2, 0, 19, 43, 14, 1, 9, 39, 0, 2, 0, 19, 44, 14, 1, 9, 40, 0, 2, 0, 19, 45, 14, 1, 9, 41, 0, 2, 0, 19, 46, 14, 1, 9, 42, 0, 2, 0, 19, 47, 14, 1, 9, 43, 0, 2, 0, 19, 48, 14, 1, 9, 44, 0, 2, 0, 19, 49, 14, 1, 9, 45, 0, 2, 0, 19, 50, 14, 1, 9, 46, 0, 2, 0, 19, 51, 14, 1, 9, 47, 0, 2, 0, 19, 52, 14, 1, 9, 48, 0, 2, 0, 19, 53, 14, 1, 9, 49, 0, 2, 0, 19, 54, 14, 1, 9, 50, 0, 2, 0, 19, 55, 14, 1, 9, 51, 0, 2, 0, 19, 56, 14, 1, 9, 52, 0, 2, 0, 19, 57, 14, 1, 9, 53, 0, 2, 0, 19, 58, 14, 1, 9, 54, 0, 2, 0, 19, 59, 14, 1, 9, 55, 0, 2, 0, 19, 60, 14, 1, 9, 56, 0, 0, 0, 62, 61, 57, 1, 9, 58, 0, 0, 0, 1, 63, -1, 0, 0, 46, 0, 65, 64, 59, 3, 9, 60, 66, 61, 67, 62, 0, 46, 0, 65, 68, 59, 3, 9, 63, 66, 64, 67, 65, 0, 46, 0, 65, 69, 59, 3, 9, 66, 66, 67, 67, 65, 0, 46, 0, 65, 70, 68, 1, 9, 69, 0, 0, 0, 72, 71, -1, 6, 73, 70, 74, 3, 75, 1, 76, 71, 77, 1, 78, 3, 0, 0, 0, 1, 79, -1, 0, 0, 52, 0, 62, 80, 72, 1, 9, 73, 0, 52, 0, 62, 81, 72, 1, 9, 74, 0, 52, 0, 62, 82, 72, 1, 9, 75, 0, 52, 0, 62, 83, 72, 1, 9, 76, 0, 52, 0, 62, 84, 72, 1, 9, 77, 0, 52, 0, 62, 85, 72, 1, 9, 78, 0, 52, 0, 62, 86, 72, 1, 9, 79, 0, 52, 0, 62, 87, 72, 1, 9, 80, 0, 52, 0, 62, 88, 72, 1, 9, 81, 0, 52, 0, 62, 89, 72, 1, 9, 82, 0, 52, 0, 62, 90, 72, 1, 9, 83, 0, 0, 0, 92, 91, 84, 0, 0, 0, 0, 93, 93, -1, 29, 5, 1, 6, 2, 7, 2, 8, 3, 94, 85, 95, 86, 96, 87, 97, 88, 98, 89, 99, 89, 100, 89, 101, 89, 102, 1, 103, 1, 104, 90, 105, 2, 106, 5, 107, 91, 108, 2, 109, 92, 110, 5, 111, 3, 112, 3, 113, 93, 114, 94, 115, 94, 116, 1, 117, 3, 118, 95, 0 </int_array>
+			<int_array  len="708"> 				-1, -1, 1, 0, -1, 2, 2, 0, 3, 1, 0, 0, 0, 5, 4, -1, 16, 2, 0, 6, 2, 7, 3, 8, 3, 9, 4, 10, 5, 11, 6, 12, 7, 13, 8, 14, 9, 15, 10, 16, 11, 17, 3, 18, 6, 19, 12, 3, 13, 0, 0, 0, 1, 20, -1, 2, 2, 0, 3, 14, 0, 2, 0, 22, 21, 15, 1, 10, 16, 0, 2, 0, 22, 23, 15, 1, 10, 17, 0, 2, 0, 22, 24, 15, 1, 10, 18, 0, 2, 0, 22, 25, 15, 1, 10, 19, 0, 2, 0, 22, 26, 15, 1, 10, 20, 0, 2, 0, 22, 27, 15, 1, 10, 21, 0, 2, 0, 22, 28, 15, 1, 10, 22, 0, 2, 0, 22, 29, 15, 1, 10, 23, 0, 2, 0, 22, 30, 15, 1, 10, 24, 0, 2, 0, 22, 31, 15, 1, 10, 25, 0, 2, 0, 22, 32, 15, 1, 10, 26, 0, 2, 0, 22, 33, 15, 1, 10, 27, 0, 2, 0, 22, 34, 15, 1, 10, 28, 0, 2, 0, 22, 35, 15, 1, 10, 29, 0, 2, 0, 22, 36, 15, 1, 10, 30, 0, 2, 0, 22, 37, 15, 1, 10, 31, 0, 2, 0, 22, 38, 15, 1, 10, 32, 0, 2, 0, 22, 39, 15, 1, 10, 33, 0, 2, 0, 22, 40, 15, 1, 10, 34, 0, 2, 0, 22, 41, 15, 1, 10, 35, 0, 2, 0, 22, 42, 15, 1, 10, 36, 0, 2, 0, 22, 43, 15, 1, 10, 37, 0, 2, 0, 22, 44, 15, 1, 10, 38, 0, 2, 0, 22, 45, 15, 1, 10, 39, 0, 2, 0, 22, 46, 15, 1, 10, 40, 0, 2, 0, 22, 47, 15, 1, 10, 41, 0, 2, 0, 22, 48, 15, 1, 10, 42, 0, 2, 0, 22, 49, 15, 1, 10, 43, 0, 2, 0, 22, 50, 15, 1, 10, 44, 0, 2, 0, 22, 51, 15, 1, 10, 45, 0, 2, 0, 22, 52, 15, 1, 10, 46, 0, 2, 0, 22, 53, 15, 1, 10, 47, 0, 2, 0, 22, 54, 15, 1, 10, 48, 0, 2, 0, 22, 55, 15, 1, 10, 49, 0, 2, 0, 22, 56, 15, 1, 10, 50, 0, 2, 0, 22, 57, 15, 1, 10, 51, 0, 2, 0, 22, 58, 15, 1, 10, 52, 0, 2, 0, 22, 59, 15, 1, 10, 53, 0, 2, 0, 22, 60, 15, 1, 10, 54, 0, 2, 0, 22, 61, 15, 1, 10, 55, 0, 2, 0, 22, 62, 15, 1, 10, 56, 0, 2, 0, 22, 63, 15, 1, 10, 57, 0, 0, 0, 65, 64, 58, 1, 10, 59, 0, 0, 0, 1, 66, -1, 1, 2, 0, 0, 46, 0, 68, 67, 60, 3, 10, 61, 69, 62, 70, 63, 0, 46, 0, 68, 71, 60, 3, 10, 64, 69, 65, 70, 66, 0, 46, 0, 68, 72, 60, 3, 10, 67, 69, 68, 70, 66, 0, 46, 0, 68, 73, 69, 1, 10, 70, 0, 0, 0, 75, 74, -1, 7, 2, 0, 76, 71, 77, 4, 78, 2, 79, 72, 80, 2, 81, 4, 0, 0, 0, 1, 82, -1, 1, 2, 0, 0, 52, 0, 65, 83, 73, 1, 10, 74, 0, 52, 0, 65, 84, 73, 1, 10, 75, 0, 52, 0, 65, 85, 73, 1, 10, 76, 0, 52, 0, 65, 86, 73, 1, 10, 77, 0, 52, 0, 65, 87, 73, 1, 10, 78, 0, 52, 0, 65, 88, 73, 1, 10, 79, 0, 52, 0, 65, 89, 73, 1, 10, 80, 0, 52, 0, 65, 90, 73, 1, 10, 81, 0, 52, 0, 65, 91, 73, 1, 10, 82, 0, 52, 0, 65, 92, 73, 1, 10, 83, 0, 52, 0, 65, 93, 73, 1, 10, 84, 0, 0, 0, 95, 94, 85, 0, 0, 0, 0, 96, 96, -1, 30, 2, 0, 6, 2, 7, 3, 8, 3, 9, 4, 97, 86, 98, 87, 99, 88, 100, 89, 101, 0, 102, 0, 103, 0, 104, 0, 105, 2, 106, 2, 107, 90, 108, 3, 109, 6, 110, 91, 111, 3, 112, 92, 113, 6, 114, 4, 115, 4, 116, 93, 117, 94, 118, 94, 119, 2, 120, 4, 121, 95, 0 </int_array>
 			<string> "conns" </string>
 			<int_array  len="0"> 				 </int_array>
 		</dictionary>
diff --git a/demos/3d/kinematic_char/cubelib.res b/demos/3d/kinematic_char/cubelib.res
new file mode 100644
index 0000000000..66b999d78d
--- /dev/null
+++ b/demos/3d/kinematic_char/cubelib.res
diff --git a/demos/3d/kinematic_char/cubio.gd b/demos/3d/kinematic_char/cubio.gd
new file mode 100644
index 0000000000..6f12e39db7
--- /dev/null
+++ b/demos/3d/kinematic_char/cubio.gd
@@ -0,0 +1,96 @@
+
+extends KinematicBody
+
+# member variables here, example:
+# var a=2
+# var b="textvar"
+
+var g = -9.8
+var vel = Vector3()
+const MAX_SPEED = 5
+const JUMP_SPEED = 7
+const ACCEL= 2
+const DEACCEL= 4
+const MAX_SLOPE_ANGLE = 30
+
+func _fixed_process(delta):
+
+	var dir = Vector3() #where does the player intend to walk to
+	var cam_xform = get_node("target/camera").get_global_transform()
+	
+	if (Input.is_action_pressed("move_forward")):
+		dir+=-cam_xform.basis[2] 
+	if (Input.is_action_pressed("move_backwards")):
+		dir+=cam_xform.basis[2] 
+	if (Input.is_action_pressed("move_left")):
+		dir+=-cam_xform.basis[0] 
+	if (Input.is_action_pressed("move_right")):
+		dir+=cam_xform.basis[0] 
+
+	dir.y=0
+	dir=dir.normalized()
+
+	vel.y+=delta*g
+	
+	var hvel = vel
+	hvel.y=0	
+	
+	var target = dir*MAX_SPEED
+	var accel
+	if (dir.dot(hvel) >0):
+		accel=ACCEL
+	else:
+		accel=DEACCEL
+		
+	hvel = hvel.linear_interpolate(target,accel*delta)
+	
+	vel.x=hvel.x;
+	vel.z=hvel.z	
+		
+	var motion = vel*delta
+	motion=move(vel*delta)
+
+	var on_floor = false
+	var original_vel = vel
+
+
+	var floor_velocity=Vector2()
+
+	var attempts=4
+	
+	while(is_colliding() and attempts):
+		var n=get_collision_normal()
+
+		if ( rad2deg(acos(n.dot( Vector3(0,1,0)))) < MAX_SLOPE_ANGLE ):
+				#if angle to the "up" vectors is < angle tolerance
+				#char is on floor
+				floor_velocity=get_collider_velocity()
+				on_floor=true			
+			
+		motion = n.slide(motion)
+		vel = n.slide(vel)
+		if (original_vel.dot(vel) > 0):
+			#do not allow to slide towads the opposite direction we were coming from
+			motion=move(motion)
+			if (motion.length()<0.001):
+				break
+		attempts-=1
+
+	if (on_floor and floor_velocity!=Vector3()):
+			move(floor_velocity*delta)
+	
+	if (on_floor and Input.is_action_pressed("jump")):
+		vel.y=JUMP_SPEED
+		
+	var crid = get_node("../elevator1").get_rid()
+#	print(crid," : ",PS.body_get_state(crid,PS.BODY_STATE_TRANSFORM))
+
+func _ready():
+	# Initalization here
+	set_fixed_process(true)
+	pass
+
+
+func _on_tcube_body_enter( body ):
+	get_node("../ty").show()
+	pass # replace with function body
diff --git a/demos/3d/kinematic_char/engine.cfg b/demos/3d/kinematic_char/engine.cfg
new file mode 100644
index 0000000000..b3060b65e0
--- /dev/null
+++ b/demos/3d/kinematic_char/engine.cfg
@@ -0,0 +1,17 @@
+[application]
+
+name="Kinematic Character 3D"
+main_scene="res://level.scn"
+icon="res://kinebody3d.png"
+
+[input]
+
+move_forward=[key(Up)]
+move_left=[key(Left)]
+move_right=[key(Right)]
+move_backwards=[key(Down)]
+jump=[key(Space)]
+
+[rasterizer]
+
+shadow_filter=3
diff --git a/demos/3d/kinematic_char/follow_camera.gd b/demos/3d/kinematic_char/follow_camera.gd
new file mode 100644
index 0000000000..0b9ff9bbb2
--- /dev/null
+++ b/demos/3d/kinematic_char/follow_camera.gd
@@ -0,0 +1,92 @@
+
+extends Camera
+
+# member variables here, example:
+# var a=2
+# var b="textvar"
+
+var collision_exception=[]
+export var min_distance=0.5
+export var max_distance=4.0
+export var angle_v_adjust=0.0
+export var autoturn_ray_aperture=25
+export var autoturn_speed=50
+var max_height = 2.0
+var min_height = 0
+
+func _fixed_process(dt):
+	var target  = get_parent().get_global_transform().origin
+	var pos = get_global_transform().origin
+	var up = Vector3(0,1,0)
+	
+	var delta = pos - target
+	
+	#regular delta follow
+	
+	#check ranges
+	
+	if (delta.length() < min_distance):
+		delta = delta.normalized() * min_distance
+	elif (delta.length() > max_distance):
+		delta = delta.normalized() * max_distance
+	
+	#check upper and lower height
+	if ( delta.y > max_height):
+		delta.y = max_height
+	if ( delta.y < min_height):
+		delta.y = min_height
+		
+	#check autoturn
+	
+	var ds = PhysicsServer.space_get_direct_state( get_world().get_space() )
+	
+	
+	var col_left = ds.intersect_ray(target,target+Matrix3(up,deg2rad(autoturn_ray_aperture)).xform(delta),collision_exception)
+	var col = ds.intersect_ray(target,target,collision_exception)
+	var col_right = ds.intersect_ray(target,target+Matrix3(up,deg2rad(-autoturn_ray_aperture)).xform(delta),collision_exception)
+	
+	if (col!=null):
+		#if main ray was occluded, get camera closer, this is the worst case scenario
+		delta = col.position - target	
+	elif (col_left!=null and col_right==null):
+		#if only left ray is occluded, turn the camera around to the right
+		delta = Matrix3(up,deg2rad(-dt*autoturn_speed)).xform(delta)
+	elif (col_left==null and col_right!=null):
+		#if only right ray is occluded, turn the camera around to the left
+		delta = Matrix3(up,deg2rad(dt*autoturn_speed)).xform(delta)
+	else:
+		#do nothing otherwise, left and right are occluded but center is not, so do not autoturn
+		pass
+	
+	#apply lookat
+	pos = target + delta
+	
+	look_at_from_pos(pos,target,up)
+	
+	#turn a little up or down
+	var t = get_transform()
+	t.basis = Matrix3(t.basis[0],deg2rad(angle_v_adjust)) * t.basis
+	set_transform(t)
+	
+	
+
+func _ready():
+
+#find collision exceptions for ray
+	var node = self
+	while(node):
+		if (node extends RigidBody):
+			collision_exception.append(node.get_rid())
+			break
+		else:
+			node=node.get_parent()
+	# Initalization here
+	set_fixed_process(true)
+	#this detaches the camera transform from the parent spatial node
+	set_as_toplevel(true)
+
+	
+	
+
+
+
diff --git a/demos/3d/kinematic_char/kinebody3d.png b/demos/3d/kinematic_char/kinebody3d.png
new file mode 100644
index 0000000000..41f0edb246
--- /dev/null
+++ b/demos/3d/kinematic_char/kinebody3d.png
diff --git a/demos/3d/kinematic_char/level.scn b/demos/3d/kinematic_char/level.scn
new file mode 100644
index 0000000000..785db19adc
--- /dev/null
+++ b/demos/3d/kinematic_char/level.scn
diff --git a/demos/3d/kinematic_char/purple_wood.tex b/demos/3d/kinematic_char/purple_wood.tex
new file mode 100644
index 0000000000..cdf0f810f1
--- /dev/null
+++ b/demos/3d/kinematic_char/purple_wood.tex
diff --git a/demos/3d/kinematic_char/purplecube.scn b/demos/3d/kinematic_char/purplecube.scn
new file mode 100644
index 0000000000..ab758366fd
--- /dev/null
+++ b/demos/3d/kinematic_char/purplecube.scn
diff --git a/demos/3d/kinematic_char/twood.tex b/demos/3d/kinematic_char/twood.tex
new file mode 100644
index 0000000000..65c1bd043c
--- /dev/null
+++ b/demos/3d/kinematic_char/twood.tex
diff --git a/demos/3d/kinematic_char/white_wood.tex b/demos/3d/kinematic_char/white_wood.tex
new file mode 100644
index 0000000000..e003442e70
--- /dev/null
+++ b/demos/3d/kinematic_char/white_wood.tex
diff --git a/drivers/gles2/rasterizer_gles2.cpp b/drivers/gles2/rasterizer_gles2.cpp
index d55557bdbc..4a1362f9f8 100644
--- a/drivers/gles2/rasterizer_gles2.cpp
+++ b/drivers/gles2/rasterizer_gles2.cpp
@@ -1504,6 +1504,23 @@ void RasterizerGLES2::mesh_add_surface(RID p_mesh,VS::PrimitiveType p_primitive,
 
 	ERR_FAIL_COND((format&VS::ARRAY_FORMAT_VERTEX)==0); // mandatory
 
+	ERR_FAIL_COND( mesh->morph_target_count!=p_blend_shapes.size() );
+	if (mesh->morph_target_count) {
+		//validate format for morphs
+		for(int i=0;i<p_blend_shapes.size();i++) {
+
+			uint32_t bsformat=0;
+			Array arr = p_blend_shapes[i];
+			for(int j=0;j<arr.size();j++) {
+
+
+				if (arr[j].get_type()!=Variant::NIL)
+					bsformat|=(1<<j);
+			}
+
+			ERR_FAIL_COND( (bsformat)!=(format&(VS::ARRAY_FORMAT_BONES-1)));
+		}
+	}
 
 	Surface *surface = memnew( Surface );
 	ERR_FAIL_COND( !surface );
@@ -1701,7 +1718,9 @@ void RasterizerGLES2::mesh_add_surface(RID p_mesh,VS::PrimitiveType p_primitive,
 	surface->array_len=array_len;
 	surface->format=format;
 	surface->primitive=p_primitive;
+	surface->morph_target_count=mesh->morph_target_count;
 	surface->configured_format=0;
+	surface->mesh=mesh;
 	if (keep_copies) {
 		surface->data=p_arrays;
 		surface->morph_data=p_blend_shapes;
@@ -1735,6 +1754,17 @@ void RasterizerGLES2::mesh_add_surface(RID p_mesh,VS::PrimitiveType p_primitive,
 			surface->index_array_local = (uint8_t*)memalloc(index_array_len*surface->array[VS::ARRAY_INDEX].size);
 			index_array_ptr=(uint8_t*)surface->index_array_local;
 		}
+
+		if (mesh->morph_target_count) {
+
+			surface->morph_targets_local = memnew_arr(Surface::MorphTarget,mesh->morph_target_count);
+			for(int i=0;i<mesh->morph_target_count;i++) {
+
+				surface->morph_targets_local[i].array=memnew_arr(uint8_t,surface->local_stride*surface->array_len);
+				surface->morph_targets_local[i].configured_format=surface->morph_format;
+				_surface_set_arrays(surface,surface->morph_targets_local[i].array,NULL,p_blend_shapes[i],false);
+			}
+		}
 	}
 
 
@@ -4946,8 +4976,11 @@ Error RasterizerGLES2::_setup_geometry(const Geometry *p_geometry, const Materia
 
 				/* compute morphs */
 
+
 				if (p_morphs && surf->morph_target_count && can_copy_to_local) {
 
+
+
 					base = skinned_buffer;
 					stride=surf->local_stride;
 
@@ -7773,9 +7806,9 @@ void RasterizerGLES2::free(const RID& p_rid) {
 
 				for(int i=0;i<mesh->morph_target_count;i++) {
 
-					memfree(surface->morph_targets_local[i].array);
+					memdelete_arr(surface->morph_targets_local[i].array);
 				}
-				memfree(surface->morph_targets_local);
+				memdelete_arr(surface->morph_targets_local);
 				surface->morph_targets_local=NULL;
 			}
 
diff --git a/main/performance.cpp b/main/performance.cpp
index 81db7ae1fa..9999cc0ae0 100644
--- a/main/performance.cpp
+++ b/main/performance.cpp
@@ -29,6 +29,8 @@
 #include "performance.h"
 #include "os/os.h"
 #include "servers/visual_server.h"
+#include "servers/physics_2d_server.h"
+#include "servers/physics_server.h"
 #include "message_queue.h"
 #include "scene/main/scene_main_loop.h"
 Performance *Performance::singleton=NULL;
@@ -61,6 +63,13 @@ void Performance::_bind_methods() {
 	BIND_CONSTANT( RENDER_VIDEO_MEM_USED );
 	BIND_CONSTANT( RENDER_TEXTURE_MEM_USED );
 	BIND_CONSTANT( RENDER_VERTEX_MEM_USED );
+	BIND_CONSTANT( PHYSICS_2D_ACTIVE_OBJECTS );
+	BIND_CONSTANT( PHYSICS_2D_COLLISION_PAIRS );
+	BIND_CONSTANT( PHYSICS_2D_ISLAND_COUNT );
+	BIND_CONSTANT( PHYSICS_3D_ACTIVE_OBJECTS );
+	BIND_CONSTANT( PHYSICS_3D_COLLISION_PAIRS );
+	BIND_CONSTANT( PHYSICS_3D_ISLAND_COUNT );
+
 	BIND_CONSTANT( MONITOR_MAX );
 
 }
@@ -90,7 +99,14 @@ String Performance::get_monitor_name(Monitor p_monitor) const {
 		"video/video_mem",
 		"video/texure_mem",
 		"video/vertex_mem",
-		"render/mem_max"
+		"video/video_mem_max",
+		"physics_2d/active_objects",
+		"physics_2d/collision_pairs",
+		"physics_2d/islands",
+		"physics_3d/active_objects",
+		"physics_3d/collision_pairs",
+		"physics_3d/islands",
+
 	};
 
 	return names[p_monitor];
@@ -133,6 +149,13 @@ float Performance::get_monitor(Monitor p_monitor) const {
 		case RENDER_TEXTURE_MEM_USED: return VS::get_singleton()->get_render_info(VS::INFO_TEXTURE_MEM_USED);
 		case RENDER_VERTEX_MEM_USED: return VS::get_singleton()->get_render_info(VS::INFO_VERTEX_MEM_USED);
 		case RENDER_USAGE_VIDEO_MEM_TOTAL: return VS::get_singleton()->get_render_info(VS::INFO_USAGE_VIDEO_MEM_TOTAL);
+		case PHYSICS_2D_ACTIVE_OBJECTS: return Physics2DServer::get_singleton()->get_process_info(Physics2DServer::INFO_ACTIVE_OBJECTS);
+		case PHYSICS_2D_COLLISION_PAIRS: return Physics2DServer::get_singleton()->get_process_info(Physics2DServer::INFO_COLLISION_PAIRS);
+		case PHYSICS_2D_ISLAND_COUNT: return Physics2DServer::get_singleton()->get_process_info(Physics2DServer::INFO_ISLAND_COUNT);
+		case PHYSICS_3D_ACTIVE_OBJECTS: return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_ACTIVE_OBJECTS);
+		case PHYSICS_3D_COLLISION_PAIRS: return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_COLLISION_PAIRS);
+		case PHYSICS_3D_ISLAND_COUNT: return PhysicsServer::get_singleton()->get_process_info(PhysicsServer::INFO_ISLAND_COUNT);
+
 		default: {}
 	}
 
diff --git a/main/performance.h b/main/performance.h
index db453d0156..1879ba39eb 100644
--- a/main/performance.h
+++ b/main/performance.h
@@ -69,6 +69,12 @@ public:
 		RENDER_TEXTURE_MEM_USED,
 		RENDER_VERTEX_MEM_USED,
 		RENDER_USAGE_VIDEO_MEM_TOTAL,
+		PHYSICS_2D_ACTIVE_OBJECTS,
+		PHYSICS_2D_COLLISION_PAIRS,
+		PHYSICS_2D_ISLAND_COUNT,
+		PHYSICS_3D_ACTIVE_OBJECTS,
+		PHYSICS_3D_COLLISION_PAIRS,
+		PHYSICS_3D_ISLAND_COUNT,
 		//physics
 		MONITOR_MAX
 	};
diff --git a/platform/android/SCsub b/platform/android/SCsub
index 8e61b7d8e0..4d6f14dbde 100644
--- a/platform/android/SCsub
+++ b/platform/android/SCsub
@@ -15,7 +15,9 @@ android_files = [
 	'audio_driver_jandroid.cpp',
 	'ifaddrs_android.cpp',
 	'android_native_app_glue.c',
-	'java_glue.cpp'
+	'java_glue.cpp',
+	'java_class_wrapper.cpp'
+	
 ]
 
 #env.Depends('#core/math/vector3.h', 'vector3_psp.h')
diff --git a/platform/android/java/src/com/android/godot/GodotPaymentV3.java b/platform/android/java/src/com/android/godot/GodotPaymentV3.java
index a459f8e15c..0fd102ac55 100644
--- a/platform/android/java/src/com/android/godot/GodotPaymentV3.java
+++ b/platform/android/java/src/com/android/godot/GodotPaymentV3.java
@@ -64,15 +64,15 @@ public class GodotPaymentV3 extends Godot.SingletonBase {
 	
 	
 	public void callbackSuccess(String ticket, String signature){
-        Log.d(this.getClass().getName(), "PRE-Send callback to purchase success");
+//        Log.d(this.getClass().getName(), "PRE-Send callback to purchase success");
         GodotLib.callobject(purchaseCallbackId, "purchase_success", new Object[]{ticket, signature});
-    	Log.d(this.getClass().getName(), "POST-Send callback to purchase success");
+//    	Log.d(this.getClass().getName(), "POST-Send callback to purchase success");
 }
 
 	public void callbackSuccessProductMassConsumed(String ticket, String signature, String sku){
-        Log.d(this.getClass().getName(), "PRE-Send callback to consume success");
+//        Log.d(this.getClass().getName(), "PRE-Send callback to consume success");
         GodotLib.calldeferred(purchaseCallbackId, "consume_success", new Object[]{ticket, signature, sku});
-    	Log.d(this.getClass().getName(), "POST-Send callback to consume success");
+//    	Log.d(this.getClass().getName(), "POST-Send callback to consume success");
 	}
 
 	public void callbackSuccessNoUnconsumedPurchases(){
diff --git a/platform/android/java_class_wrapper.cpp b/platform/android/java_class_wrapper.cpp
new file mode 100644
index 0000000000..d4cf848484
--- /dev/null
+++ b/platform/android/java_class_wrapper.cpp
@@ -0,0 +1,1332 @@
+#include "java_class_wrapper.h"
+#include "thread_jandroid.h"
+
+
+bool JavaClass::_call_method(JavaObject* p_instance,const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error,Variant& ret) {
+
+	Map<StringName,List<MethodInfo> >::Element *M=methods.find(p_method);
+	if (!M)
+		return false;
+
+	JNIEnv *env = ThreadAndroid::get_env();
+
+	MethodInfo *method=NULL;
+	for (List<MethodInfo>::Element *E=M->get().front();E;E=E->next()) {
+
+		if (!p_instance && !E->get()._static) {
+			r_error.error=Variant::CallError::CALL_ERROR_INSTANCE_IS_NULL;
+			continue;
+		}
+
+		int pc = E->get().param_types.size();
+		if (pc>p_argcount) {
+
+			r_error.error=Variant::CallError::CALL_ERROR_TOO_FEW_ARGUMENTS;
+			r_error.argument=pc;
+			continue;
+		}
+		if (pc<p_argcount) {
+
+			r_error.error=Variant::CallError::CALL_ERROR_TOO_MANY_ARGUMENTS;
+			r_error.argument=pc;
+			continue;
+		}
+		uint32_t *ptypes=E->get().param_types.ptr();
+		bool valid=true;
+
+		for(int i=0;i<pc;i++) {
+
+			Variant::Type arg_expected=Variant::NIL;
+			switch(ptypes[i]) {
+
+				case ARG_TYPE_VOID: {
+					//bug?
+				} break;
+				case ARG_TYPE_BOOLEAN: {
+					if (p_args[i]->get_type()!=Variant::BOOL)
+						arg_expected=Variant::BOOL;
+				} break;
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_BYTE:
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_CHAR:
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_SHORT:
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_INT:
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_LONG:
+				case ARG_TYPE_BYTE:
+				case ARG_TYPE_CHAR:
+				case ARG_TYPE_SHORT:
+				case ARG_TYPE_INT:
+				case ARG_TYPE_LONG: {
+
+					if (!p_args[i]->is_num())
+						arg_expected=Variant::INT;
+
+				} break;
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_FLOAT:
+				case ARG_NUMBER_CLASS_BIT|ARG_TYPE_DOUBLE:
+				case ARG_TYPE_FLOAT:
+				case ARG_TYPE_DOUBLE: {
+
+					if (!p_args[i]->is_num())
+						arg_expected=Variant::REAL;
+
+				} break;
+				case ARG_TYPE_STRING: {
+
+					if (p_args[i]->get_type()!=Variant::STRING)
+						arg_expected=Variant::STRING;
+
+				} break;
+				case ARG_TYPE_CLASS: {
+
+					if (p_args[i]->get_type()!=Variant::OBJECT)
+						arg_expected=Variant::OBJECT;
+					else {
+
+						Ref<Reference> ref = *p_args[i];
+						if (!ref.is_null()) {
+							if (ref->cast_to<JavaObject>() ) {
+
+								Ref<JavaObject> jo=ref;
+								//could be faster
+								jclass c = env->FindClass(E->get().param_sigs[i].operator String().utf8().get_data());
+								if (!c || !env->IsInstanceOf(jo->instance,c)) {
+
+									arg_expected=Variant::OBJECT;
+								} else {
+									//ok
+								}
+							} else {
+								arg_expected=Variant::OBJECT;
+							}
+
+						}
+					}
+
+				} break;
+				default: {
+
+					if (p_args[i]->get_type()!=Variant::ARRAY)
+						arg_expected=Variant::ARRAY;
+
+				} break;
+
+			}
+
+			if (arg_expected!=Variant::NIL) {
+				r_error.error=Variant::CallError::CALL_ERROR_INVALID_ARGUMENT;
+				r_error.argument=i;
+				r_error.expected=arg_expected;
+				valid=false;
+				break;
+
+			}
+
+		}
+		if (!valid)
+			continue;
+
+
+		method=&E->get();
+		break;
+
+	}
+
+	if (!method)
+		return true; //no version convinces
+
+
+
+	r_error.error=Variant::CallError::CALL_OK;
+
+	jvalue *argv=NULL;
+
+	if (method->param_types.size()) {
+
+		argv=(jvalue*)alloca( sizeof(jvalue)*method->param_types.size() );
+	}
+
+	List<jobject> to_free;
+	for(int i=0;i<method->param_types.size();i++) {
+
+		switch(method->param_types[i]) {
+			case ARG_TYPE_VOID:  {
+				//can't happen
+				argv[i].l=NULL; //I hope this works
+			} break;
+
+			case ARG_TYPE_BOOLEAN: {
+				argv[i].z=*p_args[i];
+			} break;
+			case ARG_TYPE_BYTE: {
+				argv[i].b=*p_args[i];
+			} break;
+			case ARG_TYPE_CHAR: {
+				argv[i].c=*p_args[i];
+			} break;
+			case ARG_TYPE_SHORT: {
+				argv[i].s=*p_args[i];
+			} break;
+			case ARG_TYPE_INT: {
+				argv[i].i=*p_args[i];
+			} break;
+			case ARG_TYPE_LONG: {
+				argv[i].j=*p_args[i];
+			} break;
+			case ARG_TYPE_FLOAT: {
+				argv[i].f=*p_args[i];
+			} break;
+			case ARG_TYPE_DOUBLE: {
+				argv[i].d=*p_args[i];
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_BOOLEAN: {
+				jclass bclass = env->FindClass("java/lang/Boolean");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(Z)V");
+				jvalue val;
+				val.z = (bool)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_BYTE: {
+				jclass bclass = env->FindClass("java/lang/Byte");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(B)V");
+				jvalue val;
+				val.b = (int)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_CHAR: {
+				jclass bclass = env->FindClass("java/lang/Character");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(C)V");
+				jvalue val;
+				val.c = (int)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_SHORT: {
+				jclass bclass = env->FindClass("java/lang/Short");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(S)V");
+				jvalue val;
+				val.s = (int)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_INT: {
+				jclass bclass = env->FindClass("java/lang/Integer");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(I)V");
+				jvalue val;
+				val.i = (int)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_LONG: {
+				jclass bclass = env->FindClass("java/lang/Long");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(J)V");
+				jvalue val;
+				val.j = (int64_t)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_FLOAT: {
+				jclass bclass = env->FindClass("java/lang/Float");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(F)V");
+				jvalue val;
+				val.f = (float)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_NUMBER_CLASS_BIT|ARG_TYPE_DOUBLE: {
+				jclass bclass = env->FindClass("java/lang/Double");
+				jmethodID ctor = env->GetMethodID(bclass, "<init>", "(D)V");
+				jvalue val;
+				val.d = (double)(*p_args[i]);
+				jobject obj = env->NewObjectA(bclass, ctor, &val);
+				argv[i].l = obj;
+				to_free.push_back(obj);
+			} break;
+			case ARG_TYPE_STRING: {
+				String s = *p_args[i];
+				jstring jStr = env->NewStringUTF(s.utf8().get_data());
+				argv[i].l=jStr;
+				to_free.push_back(jStr);
+			} break;
+			case ARG_TYPE_CLASS: {
+
+				Ref<JavaObject> jo=*p_args[i];
+				if (jo.is_valid()) {
+
+					argv[i].l=jo->instance;
+				} else {
+					argv[i].l=NULL; //I hope this works
+				}
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_BOOLEAN: {
+
+				Array arr = *p_args[i];
+				jbooleanArray a = env->NewBooleanArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jboolean val = arr[j];
+					env->SetBooleanArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_BYTE: {
+
+				Array arr = *p_args[i];
+				jbyteArray a = env->NewByteArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jbyte val = arr[j];
+					env->SetByteArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_CHAR: {
+
+				Array arr = *p_args[i];
+				jcharArray a = env->NewCharArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jchar val = arr[j];
+					env->SetCharArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_SHORT: {
+
+				Array arr = *p_args[i];
+				jshortArray a = env->NewShortArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jshort val = arr[j];
+					env->SetShortArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_INT: {
+
+				Array arr = *p_args[i];
+				jintArray a = env->NewIntArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jint val = arr[j];
+					env->SetIntArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_LONG: {
+				Array arr = *p_args[i];
+				jlongArray a = env->NewLongArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jlong val = arr[j];
+					env->SetLongArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_FLOAT: {
+
+				Array arr = *p_args[i];
+				jfloatArray a = env->NewFloatArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jfloat val = arr[j];
+					env->SetFloatArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_DOUBLE: {
+
+				Array arr = *p_args[i];
+				jdoubleArray a = env->NewDoubleArray(arr.size());
+				for(int j=0;j<arr.size();j++) {
+					jdouble val = arr[j];
+					env->SetDoubleArrayRegion(a,j,1,&val);
+				}
+				argv[i].l=a;
+				to_free.push_back(a);
+
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_STRING: {
+
+				Array arr = *p_args[i];
+				jobjectArray a = env->NewObjectArray(arr.size(),env->FindClass("java/lang/String"),NULL);
+				for(int j=0;j<arr.size();j++) {
+
+					String s = arr[j];
+					jstring jStr = env->NewStringUTF(s.utf8().get_data());
+					env->SetObjectArrayElement(a,j,jStr);
+					to_free.push_back(jStr);
+				}
+
+				argv[i].l=a;
+				to_free.push_back(a);
+			} break;
+			case ARG_ARRAY_BIT|ARG_TYPE_CLASS: {
+
+				argv[i].l=NULL;
+			} break;
+		}
+	}
+
+	r_error.error=Variant::CallError::CALL_OK;
+	bool success=true;
+
+	switch(method->return_type) {
+
+
+		case ARG_TYPE_VOID: {
+			if (method->_static) {
+				env->CallStaticVoidMethodA(_class,method->method,argv);
+			} else {
+				env->CallVoidMethodA(p_instance->instance,method->method,argv);
+			}
+			ret=Variant();
+
+		} break;
+		case ARG_TYPE_BOOLEAN: {
+			if (method->_static) {
+				ret=env->CallStaticBooleanMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallBooleanMethodA(p_instance->instance,method->method,argv);
+			}
+		} break;
+		case ARG_TYPE_BYTE: {
+			if (method->_static) {
+				ret=env->CallStaticByteMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallByteMethodA(p_instance->instance,method->method,argv);
+			}
+		} break;
+		case ARG_TYPE_CHAR: {
+
+			if (method->_static) {
+				ret=env->CallStaticCharMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallCharMethodA(p_instance->instance,method->method,argv);
+			}
+		} break;
+		case ARG_TYPE_SHORT: {
+
+			if (method->_static) {
+				ret=env->CallStaticShortMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallShortMethodA(p_instance->instance,method->method,argv);
+			}
+
+		} break;
+		case ARG_TYPE_INT: {
+
+			if (method->_static) {
+				ret=env->CallStaticIntMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallIntMethodA(p_instance->instance,method->method,argv);
+			}
+
+		} break;
+		case ARG_TYPE_LONG: {
+
+			if (method->_static) {
+				ret=env->CallStaticLongMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallLongMethodA(p_instance->instance,method->method,argv);
+			}
+
+		} break;
+		case ARG_TYPE_FLOAT: {
+
+			if (method->_static) {
+				ret=env->CallStaticFloatMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallFloatMethodA(p_instance->instance,method->method,argv);
+			}
+
+		} break;
+		case ARG_TYPE_DOUBLE: {
+
+			if (method->_static) {
+				ret=env->CallStaticDoubleMethodA(_class,method->method,argv);
+			} else {
+				ret=env->CallDoubleMethodA(p_instance->instance,method->method,argv);
+			}
+
+		} break;
+		default: {
+
+			jobject obj;
+			if (method->_static) {
+				obj=env->CallStaticObjectMethodA(_class,method->method,argv);
+			} else {
+				obj=env->CallObjectMethodA(p_instance->instance,method->method,argv);
+			}
+
+			if (!obj) {
+				ret=Variant();
+			} else {
+
+				if (!_convert_object_to_variant(env, obj, ret,method->return_type)) {
+					ret=Variant();
+					r_error.error=Variant::CallError::CALL_ERROR_INVALID_METHOD;
+					success=false;
+				}
+				env->DeleteLocalRef(obj);
+			}
+
+		} break;
+
+	}
+
+	for(List<jobject>::Element *E=to_free.front();E;E=E->next()) {
+		env->DeleteLocalRef(E->get());
+	}
+
+	return success;
+}
+
+Variant JavaClass::call(const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error) {
+
+	Variant ret;
+	bool found = _call_method(NULL,p_method,p_args,p_argcount,r_error,ret);
+	if (found) {
+		return ret;
+	}
+
+	return Reference::call(p_method,p_args,p_argcount,r_error);
+}
+
+JavaClass::JavaClass() {
+
+
+}
+
+/////////////////////
+
+Variant JavaObject::call(const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error){
+
+
+	return Variant();
+}
+
+JavaObject::JavaObject(const Ref<JavaClass>& p_base,jobject *p_instance) {
+
+
+}
+
+JavaObject::~JavaObject(){
+
+
+}
+
+
+////////////////////
+
+void JavaClassWrapper::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("wrap:JavaClass","name"),&JavaClassWrapper::wrap);
+}
+
+
+bool JavaClassWrapper::_get_type_sig(JNIEnv *env,jobject obj,uint32_t& sig,String&strsig) {
+
+	jstring name2 = (jstring)env->CallObjectMethod(obj, Class_getName);
+	String str_type = env->GetStringUTFChars( name2, NULL );
+	print_line("name: "+str_type);
+	env->DeleteLocalRef(name2);
+	uint32_t t=0;
+
+	if (str_type.begins_with("[")) {
+
+		t=JavaClass::ARG_ARRAY_BIT;
+		strsig="[";
+		str_type=str_type.substr(1,str_type.length()-1);
+		if (str_type.begins_with("[")) {
+			print_line("Nested arrays not supported for type: "+str_type);
+			return false;
+		}
+		if (str_type.begins_with("L")) {
+			str_type=str_type.substr(1,str_type.length()-2); //ok it's a class
+		}
+	}
+
+	if (str_type=="void" || str_type=="V") {
+		t|=JavaClass::ARG_TYPE_VOID;
+		strsig+="V";
+	} else if (str_type=="boolean" || str_type=="Z") {
+		t|=JavaClass::ARG_TYPE_BOOLEAN;
+		strsig+="Z";
+	} else if (str_type=="byte" || str_type=="B") {
+		t|=JavaClass::ARG_TYPE_BYTE;
+		strsig+="B";
+	} else if (str_type=="char" || str_type=="C") {
+		t|=JavaClass::ARG_TYPE_CHAR;
+		strsig+="C";
+	} else if (str_type=="short" || str_type=="S") {
+		t|=JavaClass::ARG_TYPE_SHORT;
+		strsig+="S";
+	} else if (str_type=="int" || str_type=="I") {
+		t|=JavaClass::ARG_TYPE_INT;
+		strsig+="I";
+	} else if (str_type=="long" || str_type=="J") {
+		t|=JavaClass::ARG_TYPE_LONG;
+		strsig+="J";
+	} else if (str_type=="float" || str_type=="F") {
+		t|=JavaClass::ARG_TYPE_FLOAT;
+		strsig+="F";
+	} else if (str_type=="double" || str_type=="D") {
+		t|=JavaClass::ARG_TYPE_DOUBLE;
+		strsig+="D";
+	} else if (str_type=="java.lang.String") {
+		t|=JavaClass::ARG_TYPE_STRING;
+		strsig+="Ljava/lang/String;";
+	} else if (str_type=="java.lang.Boolean") {
+		t|=JavaClass::ARG_TYPE_BOOLEAN|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Boolean;";
+	} else if (str_type=="java.lang.Byte") {
+		t|=JavaClass::ARG_TYPE_BYTE|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Byte;";
+	} else if (str_type=="java.lang.Character") {
+		t|=JavaClass::ARG_TYPE_CHAR|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Character;";
+	} else if (str_type=="java.lang.Short") {
+		t|=JavaClass::ARG_TYPE_SHORT|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Short;";
+	} else if (str_type=="java.lang.Integer") {
+		t|=JavaClass::ARG_TYPE_INT|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Integer;";
+	} else if (str_type=="java.lang.Long") {
+		t|=JavaClass::ARG_TYPE_LONG|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Long;";
+	} else if (str_type=="java.lang.Float") {
+		t|=JavaClass::ARG_TYPE_FLOAT|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Float;";
+	} else if (str_type=="java.lang.Double") {
+		t|=JavaClass::ARG_TYPE_DOUBLE|JavaClass::ARG_NUMBER_CLASS_BIT;
+		strsig+="Ljava/lang/Double;";
+	} else {
+		//a class likely
+		strsig+="L"+str_type.replace(".","/")+";";
+		t|=JavaClass::ARG_TYPE_CLASS;
+	}
+
+	sig=t;
+
+
+	return true;
+
+}
+
+bool JavaClass::_convert_object_to_variant(JNIEnv * env, jobject obj, Variant& var,uint32_t p_sig) {
+
+	if (!obj) {
+		var=Variant(); //seems null is just null...
+		return true;
+	}
+
+
+	switch(p_sig) {
+
+		case ARG_TYPE_VOID: {
+
+			return Variant();
+		} break;
+		case ARG_TYPE_BOOLEAN|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallBooleanMethod(obj, JavaClassWrapper::singleton->Boolean_booleanValue);
+			return true;
+		} break;
+		case ARG_TYPE_BYTE|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallByteMethod(obj, JavaClassWrapper::singleton->Byte_byteValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_CHAR|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallCharMethod(obj, JavaClassWrapper::singleton->Character_characterValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_SHORT|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallShortMethod(obj, JavaClassWrapper::singleton->Short_shortValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_INT|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallIntMethod(obj, JavaClassWrapper::singleton->Integer_integerValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_LONG|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallLongMethod(obj, JavaClassWrapper::singleton->Long_longValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_FLOAT|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallFloatMethod(obj, JavaClassWrapper::singleton->Float_floatValue);
+			return true;
+
+		} break;
+		case ARG_TYPE_DOUBLE|ARG_NUMBER_CLASS_BIT: {
+
+			var = env->CallDoubleMethod(obj, JavaClassWrapper::singleton->Double_doubleValue);
+			return true;
+		} break;
+		case ARG_TYPE_STRING: {
+
+			var = String::utf8(env->GetStringUTFChars( (jstring)obj, NULL ));
+			return true;
+		} break;
+		case ARG_TYPE_CLASS: {
+
+			return false;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_VOID: {
+
+			var = Array(); // ?
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_BOOLEAN: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jboolean val;
+				env->GetBooleanArrayRegion((jbooleanArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_BYTE: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jbyte val;
+				env->GetByteArrayRegion((jbyteArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_CHAR: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jchar val;
+				env->GetCharArrayRegion((jcharArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_SHORT: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jshort val;
+				env->GetShortArrayRegion((jshortArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_INT: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jint val;
+				env->GetIntArrayRegion((jintArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_LONG: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jlong val;
+				env->GetLongArrayRegion((jlongArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_FLOAT: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jfloat val;
+				env->GetFloatArrayRegion((jfloatArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_DOUBLE: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jdouble val;
+				env->GetDoubleArrayRegion((jdoubleArray)arr,0,1,&val);
+				ret.push_back(val);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_BOOLEAN: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					bool val = env->CallBooleanMethod(o, JavaClassWrapper::singleton->Boolean_booleanValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_BYTE: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					int val = env->CallByteMethod(o, JavaClassWrapper::singleton->Byte_byteValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_CHAR: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					int val = env->CallCharMethod(o, JavaClassWrapper::singleton->Character_characterValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_SHORT: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					int val = env->CallShortMethod(o, JavaClassWrapper::singleton->Short_shortValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_INT: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					int val = env->CallIntMethod(o, JavaClassWrapper::singleton->Integer_integerValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_LONG: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					int64_t val = env->CallLongMethod(o, JavaClassWrapper::singleton->Long_longValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_FLOAT: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					float val = env->CallFloatMethod(o, JavaClassWrapper::singleton->Float_floatValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_NUMBER_CLASS_BIT|ARG_ARRAY_BIT|ARG_TYPE_DOUBLE: {
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					double val = env->CallDoubleMethod(o, JavaClassWrapper::singleton->Double_doubleValue);
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+
+		case ARG_ARRAY_BIT|ARG_TYPE_STRING: {
+
+			Array ret;
+			jobjectArray arr = (jobjectArray)obj;
+
+			int count = env->GetArrayLength(arr);
+
+			for (int i=0; i<count; i++) {
+
+				jobject o = env->GetObjectArrayElement(arr, i);
+				if (!o)
+					ret.push_back(Variant());
+				else {
+					String val = String::utf8(env->GetStringUTFChars( (jstring)o, NULL ));
+					ret.push_back(val);
+
+				}
+				env->DeleteLocalRef(o);
+			}
+
+			var=ret;
+			return true;
+		} break;
+		case ARG_ARRAY_BIT|ARG_TYPE_CLASS: {
+
+		} break;
+	}
+
+	return false;
+
+}
+
+
+Ref<JavaClass> JavaClassWrapper::wrap(const String& p_class) {
+
+	if (class_cache.has(p_class))
+		return class_cache[p_class];
+
+
+	JNIEnv *env = ThreadAndroid::get_env();
+
+	jclass bclass = env->FindClass(p_class.utf8().get_data());
+	ERR_FAIL_COND_V(!bclass,Ref<JavaClass>());
+
+	//jmethodID getDeclaredMethods = env->GetMethodID(bclass,"getDeclaredMethods", "()[Ljava/lang/reflect/Method;");
+
+	//ERR_FAIL_COND_V(!getDeclaredMethods,Ref<JavaClass>());
+
+	jobjectArray methods = (jobjectArray)env->CallObjectMethod(bclass, getDeclaredMethods);
+
+	ERR_FAIL_COND_V(!methods,Ref<JavaClass>());
+
+
+	Ref<JavaClass> java_class = memnew( JavaClass );
+
+	int count = env->GetArrayLength(methods);
+
+	for (int i=0; i<count; i++) {
+
+		jobject obj = env->GetObjectArrayElement(methods, i);
+		ERR_CONTINUE(!obj);
+
+
+		jstring name = (jstring)env->CallObjectMethod(obj, getName);
+		String str_method = env->GetStringUTFChars( name, NULL );
+		env->DeleteLocalRef(name);
+
+		Vector<String> params;
+
+		jint mods = env->CallIntMethod(obj,getModifiers);
+
+		if (!(mods&0x0001)) {
+			env->DeleteLocalRef(obj);
+			continue; //not public bye
+		}
+
+
+
+		jobjectArray param_types = (jobjectArray)env->CallObjectMethod(obj, getParameterTypes);
+		int count2=env->GetArrayLength(param_types);
+
+		if (!java_class->methods.has(str_method)) {
+			java_class->methods[str_method]=List<JavaClass::MethodInfo>();
+		}
+
+		JavaClass::MethodInfo mi;
+		mi._static = (mods&0x8)!=0;
+		bool valid=true;
+		String signature="(";
+
+		for(int j=0;j<count2;j++) {
+
+			jobject obj2 = env->GetObjectArrayElement(param_types, j);
+			String strsig;
+			uint32_t sig=0;
+			if (!_get_type_sig(env,obj2,sig,strsig)) {
+				valid=false;
+				env->DeleteLocalRef(obj2);
+				break;
+			}
+			signature+=strsig;
+			mi.param_types.push_back(sig);
+			mi.param_sigs.push_back(strsig);
+			env->DeleteLocalRef(obj2);
+
+		}
+
+		if (!valid) {
+			print_line("Method Can't be bound (unsupported arguments): "+p_class+"::"+str_method);
+			env->DeleteLocalRef(obj);
+			env->DeleteLocalRef(param_types);
+			continue;
+		}
+
+		signature+=")";
+
+		jobject return_type = (jobject)env->CallObjectMethod(obj, getReturnType);
+
+
+		String strsig;
+		uint32_t sig=0;
+		if (!_get_type_sig(env,return_type,sig,strsig)) {
+			print_line("Method Can't be bound (unsupported return type): "+p_class+"::"+str_method);
+			env->DeleteLocalRef(obj);
+			env->DeleteLocalRef(param_types);
+			env->DeleteLocalRef(return_type);
+			continue;
+		}
+
+		signature+=strsig;
+		mi.return_type=sig;
+
+		print_line("METHOD: "+str_method+" SIG: "+signature+" static: "+itos(mi._static));
+
+		bool discard=false;
+
+		for(List<JavaClass::MethodInfo>::Element *E=java_class->methods[str_method].front();E;E=E->next()) {
+
+			float new_likeliness=0;
+			float existing_likeliness=0;
+
+			if (E->get().param_types.size()!=mi.param_types.size())
+				continue;
+			bool valid=true;
+			for(int j=0;j<E->get().param_types.size();j++) {
+
+				Variant::Type _new;
+				float new_l;
+				Variant::Type existing;
+				float existing_l;
+				JavaClass::_convert_to_variant_type(E->get().param_types[j],existing,existing_l);
+				JavaClass::_convert_to_variant_type(mi.param_types[j],_new,new_l);
+				if (_new!=existing) {
+					valid=false;
+					break;
+				}
+				new_likeliness+=new_l;
+				existing_likeliness=existing_l;
+
+			}
+
+			if (!valid)
+				continue;
+
+			if (new_likeliness>existing_likeliness) {
+				java_class->methods[str_method].erase(E);
+				print_line("replace old");
+				break;
+			} else {
+				discard=true;
+				print_line("old is better");
+			}
+
+
+		}
+
+		if (!discard) {
+			if (mi._static)
+				mi.method = env->GetStaticMethodID(bclass, str_method.utf8().get_data(), signature.utf8().get_data());
+			else
+				mi.method = env->GetMethodID(bclass, str_method.utf8().get_data(), signature.utf8().get_data());
+
+			ERR_CONTINUE(!mi.method);
+
+			java_class->methods[str_method].push_back(mi);
+		}
+
+		env->DeleteLocalRef(obj);
+		env->DeleteLocalRef(param_types);
+		env->DeleteLocalRef(return_type);
+
+
+
+
+		//args[i] = _jobject_to_variant(env, obj);
+//		print_line("\targ"+itos(i)+": "+Variant::get_type_name(args[i].get_type()));
+
+	};
+
+	env->DeleteLocalRef(methods);
+
+	jobjectArray fields = (jobjectArray)env->CallObjectMethod(bclass, getFields);
+
+	count = env->GetArrayLength(fields);
+
+	for (int i=0; i<count; i++) {
+
+		jobject obj = env->GetObjectArrayElement(fields, i);
+		ERR_CONTINUE(!obj);
+
+		jstring name = (jstring)env->CallObjectMethod(obj, Field_getName);
+		String str_field = env->GetStringUTFChars( name, NULL );
+		env->DeleteLocalRef(name);
+		print_line("FIELD: "+str_field);
+		int mods = env->CallIntMethod(obj,Field_getModifiers);
+		if ((mods&0x8) && (mods&0x10) && (mods&0x1)) { //static final public!
+
+			jobject objc = env->CallObjectMethod(obj, Field_get,NULL);
+			if (objc) {
+
+
+				uint32_t sig;
+				String strsig;
+				jclass cl = env->GetObjectClass(objc);
+				if (JavaClassWrapper::_get_type_sig(env,cl,sig,strsig)) {
+
+					if ((sig&JavaClass::ARG_TYPE_MASK)<=JavaClass::ARG_TYPE_STRING) {
+
+						Variant value;
+						if (JavaClass::_convert_object_to_variant(env,objc,value,sig)) {
+
+							java_class->constant_map[str_field]=value;
+						}
+					}
+				}
+
+				env->DeleteLocalRef(cl);
+			}
+
+
+			env->DeleteLocalRef(objc);
+
+		}
+		env->DeleteLocalRef(obj);
+	}
+
+	env->DeleteLocalRef(fields);
+
+
+	return Ref<JavaClass>();
+}
+
+JavaClassWrapper *JavaClassWrapper::singleton=NULL;
+
+JavaClassWrapper::JavaClassWrapper(jobject p_activity) {
+
+	singleton=this;
+
+	JNIEnv *env = ThreadAndroid::get_env();
+
+	jclass activityClass = env->FindClass("com/android/godot/Godot");
+	jmethodID getClassLoader = env->GetMethodID(activityClass,"getClassLoader", "()Ljava/lang/ClassLoader;");
+	classLoader = env->CallObjectMethod(p_activity, getClassLoader);
+	classLoader=(jclass)env->NewGlobalRef(classLoader);
+	jclass classLoaderClass = env->FindClass("java/lang/ClassLoader");
+	findClass = env->GetMethodID(classLoaderClass, "loadClass", "(Ljava/lang/String;)Ljava/lang/Class;");
+
+	jclass bclass = env->FindClass("java/lang/Class");
+	getDeclaredMethods = env->GetMethodID(bclass,"getDeclaredMethods", "()[Ljava/lang/reflect/Method;");
+	getFields = env->GetMethodID(bclass,"getFields", "()[Ljava/lang/reflect/Field;");
+	Class_getName = env->GetMethodID(bclass,"getName", "()Ljava/lang/String;");
+	//
+	bclass = env->FindClass("java/lang/reflect/Method");
+	getParameterTypes = env->GetMethodID(bclass,"getParameterTypes", "()[Ljava/lang/Class;");
+	getReturnType = env->GetMethodID(bclass,"getReturnType", "()Ljava/lang/Class;");
+	getName = env->GetMethodID(bclass,"getName", "()Ljava/lang/String;");
+	getModifiers = env->GetMethodID(bclass,"getModifiers", "()I");
+	///
+	bclass = env->FindClass("java/lang/reflect/Field");
+	Field_getName = env->GetMethodID(bclass,"getName", "()Ljava/lang/String;");
+	Field_getModifiers = env->GetMethodID(bclass,"getModifiers", "()I");
+	Field_get = env->GetMethodID(bclass,"get", "(Ljava/lang/Object;)Ljava/lang/Object;");
+	// each
+	bclass = env->FindClass("java/lang/Boolean");
+	Boolean_booleanValue = env->GetMethodID(bclass, "booleanValue", "()Z");
+
+	bclass = env->FindClass("java/lang/Byte");
+	Byte_byteValue = env->GetMethodID(bclass, "byteValue", "()B");
+
+	bclass = env->FindClass("java/lang/Character");
+	Character_characterValue = env->GetMethodID(bclass, "charValue", "()C");
+
+	bclass = env->FindClass("java/lang/Short");
+	Short_shortValue = env->GetMethodID(bclass, "shortValue", "()S");
+
+	bclass = env->FindClass("java/lang/Integer");
+	Integer_integerValue = env->GetMethodID(bclass, "intValue", "()I");
+
+	bclass = env->FindClass("java/lang/Long");
+	Long_longValue = env->GetMethodID(bclass, "longValue", "()J");
+
+	bclass = env->FindClass("java/lang/Float");
+	Float_floatValue = env->GetMethodID(bclass, "floatValue", "()F");
+
+	bclass = env->FindClass("java/lang/Double");
+	Double_doubleValue = env->GetMethodID(bclass, "doubleValue", "()D");
+
+
+}
diff --git a/platform/android/java_class_wrapper.h b/platform/android/java_class_wrapper.h
new file mode 100644
index 0000000000..d5d8bd5be8
--- /dev/null
+++ b/platform/android/java_class_wrapper.h
@@ -0,0 +1,168 @@
+#ifndef JAVA_CLASS_WRAPPER_H
+#define JAVA_CLASS_WRAPPER_H
+
+#include "reference.h"
+#include <jni.h>
+#include <android/log.h>
+
+class JavaObject;
+
+class JavaClass : public Reference {
+
+	OBJ_TYPE(JavaClass,Reference);
+
+	enum ArgumentType {
+
+		ARG_TYPE_VOID,
+		ARG_TYPE_BOOLEAN,
+		ARG_TYPE_BYTE,
+		ARG_TYPE_CHAR,
+		ARG_TYPE_SHORT,
+		ARG_TYPE_INT,
+		ARG_TYPE_LONG,
+		ARG_TYPE_FLOAT,
+		ARG_TYPE_DOUBLE,
+		ARG_TYPE_STRING, //special case
+		ARG_TYPE_CLASS,
+		ARG_ARRAY_BIT=1<<16,
+		ARG_NUMBER_CLASS_BIT=1<<17,
+		ARG_TYPE_MASK=(1<<16)-1
+	};
+
+
+	Map<StringName,Variant> constant_map;
+
+	struct MethodInfo {
+
+		bool _static;
+		Vector<uint32_t> param_types;
+		Vector<StringName> param_sigs;
+		uint32_t return_type;
+		jmethodID method;
+
+	};
+
+	_FORCE_INLINE_ static void _convert_to_variant_type(int p_sig, Variant::Type& r_type, float& likelyhood)  {
+
+		likelyhood=1.0;
+		r_type=Variant::NIL;
+
+		switch(p_sig) {
+
+			case ARG_TYPE_VOID: r_type=Variant::NIL; break;
+			case ARG_TYPE_BOOLEAN|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_BOOLEAN: r_type=Variant::BOOL; break;
+			case ARG_TYPE_BYTE|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_BYTE: r_type=Variant::INT; likelyhood=0.1; break;
+			case ARG_TYPE_CHAR|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_CHAR: r_type=Variant::INT; likelyhood=0.2; break;
+			case ARG_TYPE_SHORT|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_SHORT: r_type=Variant::INT; likelyhood=0.3; break;
+			case ARG_TYPE_INT|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_INT: r_type=Variant::INT; likelyhood=1.0; break;
+			case ARG_TYPE_LONG|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_LONG: r_type=Variant::INT; likelyhood=0.5; break;
+			case ARG_TYPE_FLOAT|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_FLOAT: r_type=Variant::REAL; likelyhood=1.0; break;
+			case ARG_TYPE_DOUBLE|ARG_NUMBER_CLASS_BIT:
+			case ARG_TYPE_DOUBLE: r_type=Variant::REAL; likelyhood=0.5; break;
+			case ARG_TYPE_STRING: r_type=Variant::STRING;  break;
+			case ARG_TYPE_CLASS: r_type=Variant::OBJECT;  break;
+			case ARG_ARRAY_BIT|ARG_TYPE_VOID: r_type=Variant::NIL; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_BOOLEAN: r_type=Variant::ARRAY; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_BYTE: r_type=Variant::RAW_ARRAY; likelyhood=1.0; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_CHAR: r_type=Variant::RAW_ARRAY; likelyhood=0.5; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_SHORT: r_type=Variant::INT_ARRAY; likelyhood=0.3; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_INT: r_type=Variant::INT_ARRAY; likelyhood=1.0; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_LONG: r_type=Variant::INT_ARRAY; likelyhood=0.5; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_FLOAT: r_type=Variant::REAL_ARRAY; likelyhood=1.0; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_DOUBLE: r_type=Variant::REAL_ARRAY; likelyhood=0.5; break;
+			case ARG_ARRAY_BIT|ARG_TYPE_STRING: r_type=Variant::STRING_ARRAY;  break;
+			case ARG_ARRAY_BIT|ARG_TYPE_CLASS: r_type=Variant::ARRAY;  break;
+		}
+	}
+
+	_FORCE_INLINE_ static bool _convert_object_to_variant(JNIEnv * env, jobject obj, Variant& var,uint32_t p_sig);
+
+
+
+	bool _call_method(JavaObject* p_instance,const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error,Variant& ret);
+
+friend class JavaClassWrapper;
+	Map<StringName,List<MethodInfo> > methods;
+	jclass _class;
+
+public:
+
+	virtual Variant call(const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error);
+
+	JavaClass();
+
+};
+
+
+class JavaObject : public Reference {
+
+	OBJ_TYPE(JavaObject,Reference);
+
+	Ref<JavaClass> base_class;
+friend class JavaClass;
+
+	jobject instance;
+
+public:
+
+	virtual Variant call(const StringName& p_method,const Variant** p_args,int p_argcount,Variant::CallError &r_error);
+
+	JavaObject(const Ref<JavaClass>& p_base,jobject *p_instance);
+	~JavaObject();
+
+};
+
+
+class JavaClassWrapper : public Object {
+
+	OBJ_TYPE(JavaClassWrapper,Object);
+
+
+	Map<String,Ref<JavaClass> > class_cache;
+friend class JavaClass;
+	jclass activityClass;
+	jmethodID findClass;
+	jmethodID getDeclaredMethods;
+	jmethodID getFields;
+	jmethodID getParameterTypes;
+	jmethodID getReturnType;
+	jmethodID getModifiers;
+	jmethodID getName;
+	jmethodID Class_getName;
+	jmethodID Field_getName;
+	jmethodID Field_getModifiers;
+	jmethodID Field_get;
+	jmethodID Boolean_booleanValue;
+	jmethodID Byte_byteValue;
+	jmethodID Character_characterValue;
+	jmethodID Short_shortValue;
+	jmethodID Integer_integerValue;
+	jmethodID Long_longValue;
+	jmethodID Float_floatValue;
+	jmethodID Double_doubleValue;
+	jobject classLoader;
+
+	bool _get_type_sig(JNIEnv *env, jobject obj, uint32_t& sig, String&strsig);
+
+	static JavaClassWrapper *singleton;
+
+protected:
+
+	static void _bind_methods();
+public:
+
+	static JavaClassWrapper *get_singleton() { return singleton; }
+
+	Ref<JavaClass> wrap(const String& p_class);
+
+	JavaClassWrapper(jobject p_activity=NULL);
+};
+
+#endif // JAVA_CLASS_WRAPPER_H
diff --git a/platform/android/java_glue.cpp b/platform/android/java_glue.cpp
index ae8174c35a..fdc6f1207d 100644
--- a/platform/android/java_glue.cpp
+++ b/platform/android/java_glue.cpp
@@ -38,7 +38,10 @@
 #include "globals.h"
 #include "thread_jandroid.h"
 #include "core/os/keyboard.h"
+#include "java_class_wrapper.h"
 
+
+static JavaClassWrapper *java_class_wrapper=NULL;
 static OS_Android *os_android=NULL;
 
 
@@ -934,6 +937,8 @@ JNIEXPORT void JNICALL Java_com_android_godot_GodotLib_step(JNIEnv * env, jobjec
 		// ugly hack to initialize the rest of the engine
 		// because of the way android forces you to do everything with threads
 
+		java_class_wrapper = memnew( JavaClassWrapper(_godot_instance ));
+		Globals::get_singleton()->add_singleton(Globals::Singleton("JavaClassWrapper",java_class_wrapper));
 		_initialize_java_modules();
 
 		Main::setup2();
diff --git a/platform/iphone/app_delegate.mm b/platform/iphone/app_delegate.mm
index 9ba95ff0c5..9877e09ade 100644
--- a/platform/iphone/app_delegate.mm
+++ b/platform/iphone/app_delegate.mm
@@ -257,18 +257,21 @@ static int frame_count = 0;
 - (void)applicationDidEnterBackground:(UIApplication *)application
 {
 	printf("********************* did enter background\n");
+	OS::get_singleton()->get_main_loop()->notification(MainLoop::NOTIFICATION_WM_FOCUS_OUT);
 	[view_controller.view stopAnimation];
 }
 
 - (void)applicationWillEnterForeground:(UIApplication *)application
 {
 	printf("********************* did enter foreground\n");
+	//OS::get_singleton()->get_main_loop()->notification(MainLoop::NOTIFICATION_WM_FOCUS_IN);
 	[view_controller.view startAnimation];
 }
 
 - (void) applicationWillResignActive:(UIApplication *)application
 {
 	printf("********************* will resign active\n");
+	//OS::get_singleton()->get_main_loop()->notification(MainLoop::NOTIFICATION_WM_FOCUS_OUT);
 	[view_controller.view stopAnimation]; // FIXME: pause seems to be recommended elsewhere
 }
 
@@ -279,6 +282,7 @@ static int frame_count = 0;
     printf("********************* mobile app tracker found\n");
 	[MobileAppTracker measureSession];
 #endif
+	OS::get_singleton()->get_main_loop()->notification(MainLoop::NOTIFICATION_WM_FOCUS_IN);
 	[view_controller.view startAnimation]; // FIXME: resume seems to be recommended elsewhere
 }
 
diff --git a/platform/iphone/in_app_store.h b/platform/iphone/in_app_store.h
index dba1a1a5a1..656d126ead 100644
--- a/platform/iphone/in_app_store.h
+++ b/platform/iphone/in_app_store.h
@@ -49,6 +49,8 @@ public:
 
 	int get_pending_event_count();
 	Variant pop_pending_event();
+	void finish_transaction(String product_id);
+    void set_auto_finish_transaction(bool b);
 
 	void _post_event(Variant p_event);
 	void _record_purchase(String product_id);
diff --git a/platform/iphone/in_app_store.mm b/platform/iphone/in_app_store.mm
index 9b932d147b..f3640c3076 100644
--- a/platform/iphone/in_app_store.mm
+++ b/platform/iphone/in_app_store.mm
@@ -32,8 +32,12 @@
 
 extern "C" {
 #import <StoreKit/StoreKit.h>
+#import <Foundation/Foundation.h>
 };
 
+bool auto_finish_transactions = true;
+NSMutableDictionary* pending_transactions = [NSMutableDictionary dictionary];
+
 @interface SKProduct (LocalizedPrice)
 @property (nonatomic, readonly) NSString *localizedPrice;
 @end
@@ -63,6 +67,8 @@ void InAppStore::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("get_pending_event_count"),&InAppStore::get_pending_event_count);
 	ObjectTypeDB::bind_method(_MD("pop_pending_event"),&InAppStore::pop_pending_event);
+	ObjectTypeDB::bind_method(_MD("finish_transaction"),&InAppStore::finish_transaction);
+	ObjectTypeDB::bind_method(_MD("set_auto_finish_transaction"),&InAppStore::set_auto_finish_transaction);
 };
 
 @interface ProductsDelegate : NSObject<SKProductsRequestDelegate> {
@@ -162,11 +168,13 @@ Error InAppStore::request_product_info(Variant p_params) {
 		case SKPaymentTransactionStatePurchased: {
             printf("status purchased!\n");
 			String pid = String::utf8([transaction.payment.productIdentifier UTF8String]);
+            String transactionId = String::utf8([transaction.transactionIdentifier UTF8String]);
 			InAppStore::get_singleton()->_record_purchase(pid);
 			Dictionary ret;
 			ret["type"] = "purchase";
 			ret["result"] = "ok";
 			ret["product_id"] = pid;
+            ret["transaction_id"] = transactionId;
             
             NSData* receipt = nil;
             int sdk_version = 6;
@@ -207,7 +215,13 @@ Error InAppStore::request_product_info(Variant p_params) {
             ret["receipt"] = receipt_ret;
             
 			InAppStore::get_singleton()->_post_event(ret);
-			[[SKPaymentQueue defaultQueue] finishTransaction:transaction];
+            
+            if (auto_finish_transactions){
+                [[SKPaymentQueue defaultQueue] finishTransaction:transaction];
+            }
+            else{
+                [pending_transactions setObject:transaction forKey:transaction.payment.productIdentifier];
+            }
 		} break;
 		case SKPaymentTransactionStateFailed: {
             printf("status transaction failed!\n");
@@ -290,11 +304,26 @@ InAppStore* InAppStore::get_singleton() {
 InAppStore::InAppStore() {
 	ERR_FAIL_COND(instance != NULL);
 	instance = this;
+	auto_finish_transactions = false;
 
 	TransObserver* observer = [[TransObserver alloc] init];
 	[[SKPaymentQueue defaultQueue] addTransactionObserver:observer];
+    //pending_transactions = [NSMutableDictionary dictionary];
 };
 
+void InAppStore::finish_transaction(String product_id){
+    NSString* prod_id = [NSString stringWithCString:product_id.utf8().get_data() encoding:NSUTF8StringEncoding];
+    
+	if ([pending_transactions objectForKey:prod_id]){
+		[[SKPaymentQueue defaultQueue] finishTransaction:[pending_transactions objectForKey:prod_id]];
+        [pending_transactions removeObjectForKey:prod_id];
+	}
+};
+
+void InAppStore::set_auto_finish_transaction(bool b){
+    auto_finish_transactions = b;
+}
+
 InAppStore::~InAppStore() {
 
 };
diff --git a/platform/windows/detect.py b/platform/windows/detect.py
index 0e21540e13..b866dd2ec3 100644
--- a/platform/windows/detect.py
+++ b/platform/windows/detect.py
@@ -51,7 +51,7 @@ def get_flags():
 
 	return [
 		('freetype','builtin'), #use builtin freetype
-                ('openssl','builtin'), #use builtin openssl
+		('openssl','builtin'), #use builtin openssl
         ]
 			
 
@@ -166,6 +166,7 @@ def configure(env):
 			env.Append(CCFLAGS=['-O3','-ffast-math','-fomit-frame-pointer','-msse2'])
 			env['OBJSUFFIX'] = "_opt"+env['OBJSUFFIX']
 			env['LIBSUFFIX'] = "_opt"+env['LIBSUFFIX']
+			env.Append(LINKFLAGS=['-Wl,--subsystem,windows'])
 		elif (env["target"]=="release_debug"):
 
 			env.Append(CCFLAGS=['-O2','-DDEBUG_ENABLED'])
diff --git a/platform/windows/os_windows.cpp b/platform/windows/os_windows.cpp
index 778609950e..5e57827c68 100644
--- a/platform/windows/os_windows.cpp
+++ b/platform/windows/os_windows.cpp
@@ -206,6 +206,54 @@ bool OS_Windows::can_draw() const {
 	return !minimized;
 };
 
+#define MI_WP_SIGNATURE 0xFF515700
+#define SIGNATURE_MASK 0xFFFFFF00
+#define IsPenEvent(dw) (((dw) & SIGNATURE_MASK) == MI_WP_SIGNATURE)
+
+
+void OS_Windows::_touch_event(int idx, UINT uMsg, WPARAM	wParam,	LPARAM	lParam) {
+
+	InputEvent event;
+	event.type = InputEvent::SCREEN_TOUCH;
+	event.ID=++last_id;
+	event.screen_touch.index = idx;
+
+	switch (uMsg) {
+		case WM_LBUTTONDOWN:
+		case WM_MBUTTONDOWN:
+		case WM_RBUTTONDOWN: {
+
+			event.screen_touch.pressed = true;
+		} break;
+
+		case WM_LBUTTONUP:
+		case WM_MBUTTONUP:
+		case WM_RBUTTONUP: {
+			event.screen_touch.pressed = false;
+		} break;
+	};
+
+	event.screen_touch.x=GET_X_LPARAM(lParam);
+	event.screen_touch.y=GET_Y_LPARAM(lParam);
+
+	if (main_loop) {
+		input->parse_input_event(event);
+	}
+};
+
+void OS_Windows::_drag_event(int idx,UINT uMsg, WPARAM	wParam,	LPARAM	lParam) {
+
+	InputEvent event;
+	event.type = InputEvent::SCREEN_DRAG;
+	event.ID=++last_id;
+	event.screen_drag.index = idx;
+
+	event.screen_drag.x=GET_X_LPARAM(lParam);
+	event.screen_drag.y=GET_Y_LPARAM(lParam);
+
+	if (main_loop)
+		input->parse_input_event(event);
+};
 
 LRESULT OS_Windows::WndProc(HWND hWnd,UINT uMsg, WPARAM	wParam,	LPARAM	lParam) {
 
@@ -270,28 +318,41 @@ LRESULT OS_Windows::WndProc(HWND hWnd,UINT uMsg, WPARAM	wParam,	LPARAM	lParam) {
 		}
 		case WM_MOUSELEAVE: {
 
-                    old_invalid=true;
-                    outside=true;
+			old_invalid=true;
+			outside=true;
 
 		} break;
 		case WM_MOUSEMOVE: {
 
-                    if (outside) {
+			if (outside) {
+
+				CursorShape c=cursor_shape;
+				cursor_shape=CURSOR_MAX;
+				set_cursor_shape(c);
+				outside=false;
 
-                        CursorShape c=cursor_shape;
-                        cursor_shape=CURSOR_MAX;
-                        set_cursor_shape(c);
-                        outside=false;
+				//Once-Off notification, must call again....
+				TRACKMOUSEEVENT tme;
+				tme.cbSize=sizeof(TRACKMOUSEEVENT);
+				tme.dwFlags=TME_LEAVE;
+				tme.hwndTrack=hWnd;
+				tme.dwHoverTime=HOVER_DEFAULT;
+				TrackMouseEvent(&tme);
+
+			}
+
+			LPARAM extra = GetMessageExtraInfo();
+			if (IsPenEvent(extra)) {
+
+				int idx = extra & 0x7f;
+				_drag_event(idx, uMsg, wParam, lParam);
+				if (idx != 0) {
+					return 0;
+				};
+				// fallthrough for mouse event
+			};
 
-			//Once-Off notification, must call again....
-			TRACKMOUSEEVENT tme;
-			tme.cbSize=sizeof(TRACKMOUSEEVENT);
-			tme.dwFlags=TME_LEAVE;
-			tme.hwndTrack=hWnd;
-			tme.dwHoverTime=HOVER_DEFAULT;
-			TrackMouseEvent(&tme);
 
-                    }
 			InputEvent event;
 			event.type=InputEvent::MOUSE_MOTION;
 			event.ID=++last_id;
@@ -360,6 +421,17 @@ LRESULT OS_Windows::WndProc(HWND hWnd,UINT uMsg, WPARAM	wParam,	LPARAM	lParam) {
 		/*case WM_XBUTTONDOWN:
 		case WM_XBUTTONUP: */{
 
+			LPARAM extra = GetMessageExtraInfo();
+			if (IsPenEvent(extra)) {
+
+				int idx = extra & 0x7f;
+				_touch_event(idx, uMsg, wParam, lParam);
+				if (idx != 0) {
+					return 0;
+				};
+				// fallthrough for mouse event
+			};
+
 			InputEvent event;
 			event.type=InputEvent::MOUSE_BUTTON;
 			event.ID=++last_id;
diff --git a/platform/windows/os_windows.h b/platform/windows/os_windows.h
index c9eb475e1a..1a41b9d77d 100644
--- a/platform/windows/os_windows.h
+++ b/platform/windows/os_windows.h
@@ -160,6 +160,10 @@ class OS_Windows : public OS {
 
 	void _post_dpad(DWORD p_dpad, int p_device, bool p_pressed);
 
+	void _drag_event(int idx,UINT uMsg, WPARAM	wParam,	LPARAM	lParam);
+	void _touch_event(int idx, UINT uMsg, WPARAM	wParam,	LPARAM	lParam);
+
+
 	// functions used by main to initialize/deintialize the OS
 protected:	
 	virtual int get_video_driver_count() const;
diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index ecd147afde..47d78399b6 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -803,7 +803,7 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	//print_line("margin: "+rtos(margin));
 	do {
 
-		//fill exclude list..
+		//motion recover
 		for(int i=0;i<get_shape_count();i++) {
 
 
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 721fd368e1..f5e3ad66ee 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -57,160 +57,111 @@ float PhysicsBody::get_inverse_mass() const {
 	return 0;
 }
 
-PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) {
-
 
+void PhysicsBody::set_layer_mask(uint32_t p_mask) {
 
+	layer_mask=p_mask;
+	PhysicsServer::get_singleton()->body_set_layer_mask(get_rid(),p_mask);
 }
 
-void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) {
-
-	constant_linear_velocity=p_vel;
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity);
+uint32_t PhysicsBody::get_layer_mask() const {
 
+	return layer_mask;
 }
 
-void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) {
-
-	constant_angular_velocity=p_vel;
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity);
+void PhysicsBody::_bind_methods() {
+	ObjectTypeDB::bind_method(_MD("set_layer_mask","mask"),&PhysicsBody::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsBody::get_layer_mask);
+	ADD_PROPERTY(PropertyInfo(Variant::INT,"layers",PROPERTY_HINT_ALL_FLAGS),_SCS("set_layer_mask"),_SCS("get_layer_mask"));
 }
 
-Vector3 StaticBody::get_constant_linear_velocity() const {
-
-	return constant_linear_velocity;
-}
-Vector3 StaticBody::get_constant_angular_velocity() const {
-
-	return constant_angular_velocity;
-}
 
+PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) {
 
-void StaticBody::_state_notify(Object *p_object) {
+	layer_mask=1;
 
-	if (!pre_xform)
-		return;
+}
 
-	PhysicsDirectBodyState *p2d = (PhysicsDirectBodyState*)p_object;
-	setting=true;
 
-	Transform new_xform = p2d->get_transform();
-	*pre_xform=new_xform;
-	set_ignore_transform_notification(true);
-	set_global_transform(new_xform);
-	set_ignore_transform_notification(false);
+void StaticBody::set_friction(real_t p_friction){
 
-	setting=false;
+	ERR_FAIL_COND(p_friction<0 || p_friction>1);
 
+	friction=p_friction;
+	PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction);
 
 }
+real_t StaticBody::get_friction() const{
 
-void StaticBody::_update_xform() {
-
-	if (!pre_xform || !pending)
-		return;
-
-	setting=true;
-
-
-	Transform new_xform = get_global_transform(); //obtain the new one
+	return friction;
+}
 
-	//set_block_transform_notify(true);
-	set_ignore_transform_notification(true);
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_TRANSFORM,*pre_xform); //then simulate motion!
-	set_global_transform(*pre_xform); //but restore state to previous one in both visual and physics
-	set_ignore_transform_notification(false);
+void StaticBody::set_bounce(real_t p_bounce){
 
-	PhysicsServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion!
+	ERR_FAIL_COND(p_bounce<0 || p_bounce>1);
 
-	setting=false;
-	pending=false;
+	bounce=p_bounce;
+	PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_BOUNCE,bounce);
 
 }
+real_t StaticBody::get_bounce() const{
 
-void StaticBody::_notification(int p_what) {
-
-	switch(p_what) {
-
-		case NOTIFICATION_ENTER_SCENE: {
-
-			if (pre_xform)
-				*pre_xform = get_global_transform();
-			pending=false;
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
+	return bounce;
+}
 
-			if (simulating_motion && !pending && is_inside_scene() && !setting && !get_scene()->is_editor_hint()) {
+void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) {
 
-				call_deferred(SceneStringNames::get_singleton()->_update_xform);
-				pending=true;
-			}
+	constant_linear_velocity=p_vel;
+	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity);
 
-		} break;
-	}
+}
 
+void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) {
 
+	constant_angular_velocity=p_vel;
+	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity);
 }
 
-void StaticBody::set_simulate_motion(bool p_enable) {
-
-	if (p_enable==simulating_motion)
-		return;
-	simulating_motion=p_enable;
+Vector3 StaticBody::get_constant_linear_velocity() const {
 
-	if (p_enable) {
-		pre_xform = memnew( Transform );
-		if (is_inside_scene())
-			*pre_xform=get_transform();
-//		query = PhysicsServer::get_singleton()->query_create(this,"_state_notify",Variant());
-	//	PhysicsServer::get_singleton()->query_body_direct_state(query,get_rid());
-		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_state_notify");
+	return constant_linear_velocity;
+}
+Vector3 StaticBody::get_constant_angular_velocity() const {
 
-	} else {
-		memdelete( pre_xform );
-		pre_xform=NULL;
-		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName());
-		pending=false;
-	}
+	return constant_angular_velocity;
 }
 
-bool StaticBody::is_simulating_motion() const {
 
-	return simulating_motion;
-}
 
 void StaticBody::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("set_simulate_motion","enabled"),&StaticBody::set_simulate_motion);
-	ObjectTypeDB::bind_method(_MD("is_simulating_motion"),&StaticBody::is_simulating_motion);
-	ObjectTypeDB::bind_method(_MD("_update_xform"),&StaticBody::_update_xform);
-	ObjectTypeDB::bind_method(_MD("_state_notify"),&StaticBody::_state_notify);
 	ObjectTypeDB::bind_method(_MD("set_constant_linear_velocity","vel"),&StaticBody::set_constant_linear_velocity);
 	ObjectTypeDB::bind_method(_MD("set_constant_angular_velocity","vel"),&StaticBody::set_constant_angular_velocity);
 	ObjectTypeDB::bind_method(_MD("get_constant_linear_velocity"),&StaticBody::get_constant_linear_velocity);
 	ObjectTypeDB::bind_method(_MD("get_constant_angular_velocity"),&StaticBody::get_constant_angular_velocity);
 
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"simulate_motion"),_SCS("set_simulate_motion"),_SCS("is_simulating_motion"));
+	ObjectTypeDB::bind_method(_MD("set_friction","friction"),&StaticBody::set_friction);
+	ObjectTypeDB::bind_method(_MD("get_friction"),&StaticBody::get_friction);
+
+	ObjectTypeDB::bind_method(_MD("set_bounce","bounce"),&StaticBody::set_bounce);
+	ObjectTypeDB::bind_method(_MD("get_bounce"),&StaticBody::get_bounce);
+
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"bounce",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_bounce"),_SCS("get_bounce"));
+
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_linear_velocity"),_SCS("set_constant_linear_velocity"),_SCS("get_constant_linear_velocity"));
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_angular_velocity"),_SCS("set_constant_angular_velocity"),_SCS("get_constant_angular_velocity"));
 }
 
 StaticBody::StaticBody() : PhysicsBody(PhysicsServer::BODY_MODE_STATIC) {
 
-	simulating_motion=false;
-	pre_xform=NULL;
-	setting=false;
-	pending=false;
-	//constant_angular_velocity=0;
-
+	bounce=0;
+	friction=1;
 }
 
 StaticBody::~StaticBody() {
 
-	if (pre_xform)
-		memdelete(pre_xform);
-	//if (query.is_valid())
-	//	PhysicsServer::get_singleton()->free(query);
+
 }
 
 
@@ -768,4 +719,418 @@ 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);
+	}
+
+	return obj;
+}
+
+
+bool KinematicBody::_ignores_mode(PhysicsServer::BodyMode p_mode) const {
+
+	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;
+	}
+
+	return true;
+}
+
+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_scene(),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 (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),m,sr,max_shapes,res_shapes,exclude,get_layer_mask(),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++) {
+
+
+
+		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_layer_mask(),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_layer_mask(),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;
+		}
+	}
+
+	Vector3 motion=p_motion*safe;
+	//if (colliding)
+	//	motion+=normal*0.001;
+	Transform gt = get_global_transform();
+	gt.origin+=motion;
+	set_global_transform(gt);
+
+	return p_motion-motion;
+
+}
+
+Vector3 KinematicBody::move_to(const Vector3& p_position) {
+
+	return move(p_position-get_global_transform().origin);
+}
+
+bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),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;
+
+	Vector3 motion = p_position-get_global_transform().origin;
+	Transform xform=get_global_transform();
+
+	if (true || p_discrete) {
+
+		xform.origin+=motion;
+		motion=Vector3();
+	}
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+	//fill exclude list..
+	for(int i=0;i<get_shape_count();i++) {
+
+
+		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask);
+		if (col)
+			return false;
+	}
+
+	return true;
+}
+
+bool KinematicBody::is_colliding() const {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+
+	return colliding;
+}
+Vector3 KinematicBody::get_collision_pos() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector3());
+	return collision;
+
+}
+Vector3 KinematicBody::get_collision_normal() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector3());
+	return normal;
+
+}
+
+Vector3 KinematicBody::get_collider_velocity() const {
+
+	return collider_vel;
+}
+
+ObjectID KinematicBody::get_collider() const {
+
+	ERR_FAIL_COND_V(!colliding,0);
+	return collider;
+}
+
+void KinematicBody::set_collide_with_static_bodies(bool p_enable) {
+
+	collide_static=p_enable;
+}
+bool KinematicBody::can_collide_with_static_bodies() const {
+
+	return collide_static;
+}
+
+void KinematicBody::set_collide_with_rigid_bodies(bool p_enable) {
+
+	collide_rigid=p_enable;
+
+}
+bool KinematicBody::can_collide_with_rigid_bodies() const {
+
+
+	return collide_rigid;
+}
+
+void KinematicBody::set_collide_with_kinematic_bodies(bool p_enable) {
+
+	collide_kinematic=p_enable;
+
+}
+bool KinematicBody::can_collide_with_kinematic_bodies() const {
+
+	return collide_kinematic;
+}
+
+void KinematicBody::set_collide_with_character_bodies(bool p_enable) {
+
+	collide_character=p_enable;
+}
+bool KinematicBody::can_collide_with_character_bodies() const {
+
+	return collide_character;
+}
+
+void KinematicBody::set_collision_margin(float p_margin) {
+
+	margin=p_margin;
+}
+
+float KinematicBody::get_collision_margin() const{
+
+	return margin;
+}
+
+void KinematicBody::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody::move);
+	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody::move_to);
+
+	ObjectTypeDB::bind_method(_MD("can_move_to","position"),&KinematicBody::can_move_to);
+
+	ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody::is_colliding);
+
+	ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody::get_collision_pos);
+	ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody::get_collision_normal);
+	ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&KinematicBody::get_collider_velocity);
+	ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody::_get_collider);
+
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_static_bodies","enable"),&KinematicBody::set_collide_with_static_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_static_bodies"),&KinematicBody::can_collide_with_static_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_kinematic_bodies","enable"),&KinematicBody::set_collide_with_kinematic_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_kinematic_bodies"),&KinematicBody::can_collide_with_kinematic_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_rigid_bodies","enable"),&KinematicBody::set_collide_with_rigid_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_rigid_bodies"),&KinematicBody::can_collide_with_rigid_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_character_bodies","enable"),&KinematicBody::set_collide_with_character_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_character_bodies"),&KinematicBody::can_collide_with_character_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody::set_collision_margin);
+	ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody::get_collision_margin);
+
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/static"),_SCS("set_collide_with_static_bodies"),_SCS("can_collide_with_static_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_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;
+}
+KinematicBody::~KinematicBody()  {
+
+
+}
+
 
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 616288e1f6..0b7a389449 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -38,8 +38,10 @@ class PhysicsBody : public CollisionObject {
 
 	OBJ_TYPE(PhysicsBody,CollisionObject);
 
+	uint32_t layer_mask;
 protected:
 
+	static void _bind_methods();
 	void _notification(int p_what);
 	PhysicsBody(PhysicsServer::BodyMode p_mode);
 public:
@@ -48,6 +50,10 @@ public:
 	virtual Vector3 get_angular_velocity() const;
 	virtual float get_inverse_mass() const;
 
+	void set_layer_mask(uint32_t p_mask);
+	uint32_t get_layer_mask() const;
+
+
 	PhysicsBody();
 
 };
@@ -56,25 +62,26 @@ class StaticBody : public PhysicsBody {
 
 	OBJ_TYPE(StaticBody,PhysicsBody);
 
-	Transform *pre_xform;
-	//RID query;
-	bool setting;
-	bool pending;
-	bool simulating_motion;
 	Vector3 constant_linear_velocity;
 	Vector3 constant_angular_velocity;
-	void _update_xform();
-	void _state_notify(Object *p_object);
+
+	real_t bounce;
+	real_t friction;
+
 
 protected:
 
-	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
 
-	void set_simulate_motion(bool p_enable);
-	bool is_simulating_motion() const;
+
+	void set_friction(real_t p_friction);
+	real_t get_friction() const;
+
+	void set_bounce(real_t p_bounce);
+	real_t get_bounce() const;
+
 
 	void set_constant_linear_velocity(const Vector3& p_vel);
 	void set_constant_angular_velocity(const Vector3& p_vel);
@@ -237,4 +244,70 @@ public:
 
 VARIANT_ENUM_CAST(RigidBody::Mode);
 VARIANT_ENUM_CAST(RigidBody::AxisLock);
+
+
+
+
+
+class KinematicBody : public PhysicsBody {
+
+	OBJ_TYPE(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;
+
+
+	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
+	};
+
+	Vector3 move(const Vector3& p_motion);
+	Vector3 move_to(const Vector3& p_position);
+
+	bool can_move_to(const Vector3& p_position,bool p_discrete=false);
+	bool is_colliding() const;
+	Vector3 get_collision_pos() const;
+	Vector3 get_collision_normal() const;
+	Vector3 get_collider_velocity() const;
+	ObjectID get_collider() const;
+
+	void set_collide_with_static_bodies(bool p_enable);
+	bool can_collide_with_static_bodies() const;
+
+	void set_collide_with_rigid_bodies(bool p_enable);
+	bool can_collide_with_rigid_bodies() const;
+
+	void set_collide_with_kinematic_bodies(bool p_enable);
+	bool can_collide_with_kinematic_bodies() const;
+
+	void set_collide_with_character_bodies(bool p_enable);
+	bool can_collide_with_character_bodies() const;
+
+	void set_collision_margin(float p_margin);
+	float get_collision_margin() const;
+
+	KinematicBody();
+	~KinematicBody();
+
+};
+
 #endif // PHYSICS_BODY__H
diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp
index 7680c1d56c..07abd1dcd2 100644
--- a/scene/3d/vehicle_body.cpp
+++ b/scene/3d/vehicle_body.cpp
@@ -125,19 +125,161 @@ void VehicleWheel::_update(PhysicsDirectBodyState *s) {
 	}
 }
 
+void VehicleWheel::set_radius(float p_radius) {
+
+	m_wheelRadius=p_radius;
+	update_gizmo();
+}
+
+float VehicleWheel::get_radius() const{
+
+	return m_wheelRadius;
+}
+
+void VehicleWheel::set_suspension_rest_length(float p_length){
+
+	m_suspensionRestLength=p_length;
+	update_gizmo();
+}
+float VehicleWheel::get_suspension_rest_length() const{
+
+	return m_suspensionRestLength;
+}
+
+void VehicleWheel::set_suspension_travel(float p_length){
+
+	m_maxSuspensionTravelCm=p_length/0.01;
+}
+float VehicleWheel::get_suspension_travel() const{
+
+	return m_maxSuspensionTravelCm*0.01;
+}
+
+void VehicleWheel::set_suspension_stiffness(float p_value){
+
+	m_suspensionStiffness=p_value;
+}
+float VehicleWheel::get_suspension_stiffness() const{
+
+	return m_suspensionStiffness;
+}
+
+void VehicleWheel::set_suspension_max_force(float p_value){
+
+	m_maxSuspensionForce=p_value;
+}
+float VehicleWheel::get_suspension_max_force() const{
+
+	return m_maxSuspensionForce;
+}
+
+void VehicleWheel::set_damping_compression(float p_value){
+
+	m_wheelsDampingCompression=p_value;
+}
+float VehicleWheel::get_damping_compression() const{
+
+	return m_wheelsDampingRelaxation;
+}
+
+void VehicleWheel::set_damping_relaxation(float p_value){
+
+	m_wheelsDampingRelaxation=p_value;
+}
+float VehicleWheel::get_damping_relaxation() const{
+
+	return m_wheelsDampingRelaxation;
+}
+
+void VehicleWheel::set_friction_slip(float p_value) {
+
+	m_frictionSlip=p_value;
+}
+float VehicleWheel::get_friction_slip() const{
+
+	return m_frictionSlip;
+}
+
+
 void VehicleWheel::_bind_methods() {
 
 
+	ObjectTypeDB::bind_method(_MD("set_radius","length"),&VehicleWheel::set_radius);
+	ObjectTypeDB::bind_method(_MD("get_radius"),&VehicleWheel::get_radius);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_rest_length","length"),&VehicleWheel::set_suspension_rest_length);
+	ObjectTypeDB::bind_method(_MD("get_suspension_rest_length"),&VehicleWheel::get_suspension_rest_length);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_travel","length"),&VehicleWheel::set_suspension_travel);
+	ObjectTypeDB::bind_method(_MD("get_suspension_travel"),&VehicleWheel::get_suspension_travel);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_stiffness","length"),&VehicleWheel::set_suspension_stiffness);
+	ObjectTypeDB::bind_method(_MD("get_suspension_stiffness"),&VehicleWheel::get_suspension_stiffness);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_max_force","length"),&VehicleWheel::set_suspension_max_force);
+	ObjectTypeDB::bind_method(_MD("get_suspension_max_force"),&VehicleWheel::get_suspension_max_force);
+
 
+	ObjectTypeDB::bind_method(_MD("set_damping_compression","length"),&VehicleWheel::set_damping_compression);
+	ObjectTypeDB::bind_method(_MD("get_damping_compression"),&VehicleWheel::get_damping_compression);
+
+	ObjectTypeDB::bind_method(_MD("set_damping_relaxation","length"),&VehicleWheel::set_damping_relaxation);
+	ObjectTypeDB::bind_method(_MD("get_damping_relaxation"),&VehicleWheel::get_damping_relaxation);
+
+	ObjectTypeDB::bind_method(_MD("set_use_as_traction","enable"),&VehicleWheel::set_use_as_traction);
+	ObjectTypeDB::bind_method(_MD("is_used_as_traction"),&VehicleWheel::is_used_as_traction);
+
+	ObjectTypeDB::bind_method(_MD("set_use_as_steering","enable"),&VehicleWheel::set_use_as_steering);
+	ObjectTypeDB::bind_method(_MD("is_used_as_steering"),&VehicleWheel::is_used_as_steering);
+
+	ObjectTypeDB::bind_method(_MD("set_friction_slip","length"),&VehicleWheel::set_friction_slip);
+	ObjectTypeDB::bind_method(_MD("get_friction_slip"),&VehicleWheel::get_friction_slip);
+
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/traction"),_SCS("set_use_as_traction"),_SCS("is_used_as_traction"));
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/steering"),_SCS("set_use_as_steering"),_SCS("is_used_as_steering"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/radius"),_SCS("set_radius"),_SCS("get_radius"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/rest_length"),_SCS("set_suspension_rest_length"),_SCS("get_suspension_rest_length"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/friction_slip"),_SCS("set_friction_slip"),_SCS("get_friction_slip"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/travel"),_SCS("set_suspension_travel"),_SCS("get_suspension_travel"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/stiffness"),_SCS("set_suspension_stiffness"),_SCS("get_suspension_stiffness"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/max_force"),_SCS("set_suspension_max_force"),_SCS("get_suspension_max_force"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/compression"),_SCS("set_damping_compression"),_SCS("get_damping_compression"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/relaxation"),_SCS("set_damping_relaxation"),_SCS("get_damping_relaxation"));
+
+}
+
+
+void VehicleWheel::set_use_as_traction(bool p_enable) {
+
+	engine_traction=p_enable;
+}
+
+bool VehicleWheel::is_used_as_traction() const{
+
+	return engine_traction;
+}
+
+
+void VehicleWheel::set_use_as_steering(bool p_enabled){
+
+	steers=p_enabled;
+}
+
+bool VehicleWheel::is_used_as_steering() const{
+
+	return steers;
 }
 
 
 VehicleWheel::VehicleWheel() {
 
 
+	steers=false;
+	engine_traction=false;
 
 	m_steering = real_t(0.);
-	m_engineForce = real_t(0.);
+	//m_engineForce = real_t(0.);
 	m_rotation = real_t(0.);
 	m_deltaRotation = real_t(0.);
 	m_brake = real_t(0.);
@@ -172,6 +314,7 @@ void VehicleBody::_update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBody
 	//}
 
 	wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform( wheel.m_chassisConnectionPointCS );
+	//wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step();
 	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform( wheel.m_wheelDirectionCS).normalized();
 	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform( wheel.m_wheelAxleCS ).normalized();
 }
@@ -189,12 +332,16 @@ void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) {
 //	up.normalize();
 
 	//rotate around steering over de wheelAxleWS
-	real_t steering = wheel.m_steering;
+	real_t steering = wheel.steers?m_steeringValue:0.0;
+	//print_line(itos(p_idx)+": "+rtos(steering));
 
 	Matrix3 steeringMat(up,steering);
 
 	Matrix3 rotatingMat(right,-wheel.m_rotation);
 
+//	if (p_idx==1)
+//		print_line("steeringMat " +steeringMat);
+
 	Matrix3 basis2(
 		right[0],up[0],fwd[0],
 		right[1],up[1],fwd[1],
@@ -202,9 +349,11 @@ void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) {
 	);
 
 	wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2);
+	//wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat));
 	wheel.m_worldTransform.set_origin(
 		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
 	);
+
 }
 
 
@@ -221,9 +370,10 @@ real_t VehicleBody::_ray_cast(int p_idx,PhysicsDirectBodyState *s) {
 	real_t raylen = wheel.m_suspensionRestLength+wheel.m_wheelRadius;
 
 	Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
-	const Vector3& source = wheel.m_raycastInfo.m_hardPointWS;
+	Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
 	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
 	const Vector3& target = wheel.m_raycastInfo.m_contactPointWS;
+	source-=wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS;
 
 	real_t param = real_t(0.);
 
@@ -552,9 +702,10 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) {
 
 				//const btTransform& wheelTrans = getWheelTransformWS( i );
 
-				Matrix3 wheelBasis0 = wheelInfo.get_global_transform().basis;
+				Matrix3 wheelBasis0 = wheelInfo.m_worldTransform.basis;//get_global_transform().basis;
+
 				m_axle[i] = wheelBasis0.get_axis(Vector3::AXIS_X);
-				m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
+				//m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
 
 				const Vector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
 				real_t proj = m_axle[i].dot(surfNormalWS);
@@ -592,9 +743,9 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) {
 
 			if (wheelInfo.m_raycastInfo.m_isInContact)
 			{
-				if (wheelInfo.m_engineForce != 0.f)
+				if (engine_force != 0.f)
 				{
-					rollingFriction = wheelInfo.m_engineForce* s->get_step();
+					rollingFriction = engine_force* s->get_step();
 				} else
 				{
 					real_t defaultRollingFrictionImpulse = 0.f;
@@ -721,9 +872,11 @@ void VehicleBody::_direct_state_changed(Object *p_state) {
 		_update_wheel(i,s);
 	}
 
+
 	for(int i=0;i<wheels.size();i++) {
 
 		_ray_cast(i,s);
+		wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform);
 	}
 
 	_update_suspension(s);
@@ -808,6 +961,35 @@ real_t VehicleBody::get_friction() const{
 	return friction;
 }
 
+void VehicleBody::set_engine_force(float p_force) {
+
+	engine_force=p_force;
+}
+
+float VehicleBody::get_engine_force() const{
+
+	return engine_force;
+}
+
+void VehicleBody::set_brake(float p_brake){
+
+	brake=p_brake;
+}
+float VehicleBody::get_brake() const{
+
+	return brake;
+}
+
+void VehicleBody::set_steering(float p_steering){
+
+	m_steeringValue=p_steering;
+}
+float VehicleBody::get_steering() const{
+
+	return m_steeringValue;
+}
+
+
 void VehicleBody::_bind_methods(){
 
 	ObjectTypeDB::bind_method(_MD("set_mass","mass"),&VehicleBody::set_mass);
@@ -816,8 +998,20 @@ void VehicleBody::_bind_methods(){
 	ObjectTypeDB::bind_method(_MD("set_friction","friction"),&VehicleBody::set_friction);
 	ObjectTypeDB::bind_method(_MD("get_friction"),&VehicleBody::get_friction);
 
+	ObjectTypeDB::bind_method(_MD("set_engine_force","engine_force"),&VehicleBody::set_engine_force);
+	ObjectTypeDB::bind_method(_MD("get_engine_force"),&VehicleBody::get_engine_force);
+
+	ObjectTypeDB::bind_method(_MD("set_brake","brake"),&VehicleBody::set_brake);
+	ObjectTypeDB::bind_method(_MD("get_brake"),&VehicleBody::get_brake);
+
+	ObjectTypeDB::bind_method(_MD("set_steering","steering"),&VehicleBody::set_steering);
+	ObjectTypeDB::bind_method(_MD("get_steering"),&VehicleBody::get_steering);
+
 	ObjectTypeDB::bind_method(_MD("_direct_state_changed"),&VehicleBody::_direct_state_changed);
 
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/engine_force",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_engine_force"),_SCS("get_engine_force"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/brake",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_brake"),_SCS("get_brake"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/steering",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_steering"),_SCS("get_steering"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/mass",PROPERTY_HINT_RANGE,"0.01,65536,0.01"),_SCS("set_mass"),_SCS("get_mass"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/friction",PROPERTY_HINT_RANGE,"0.01,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
 
@@ -833,8 +1027,11 @@ VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
 	m_currentVehicleSpeedKmHour = real_t(0.);
 	m_steeringValue = real_t(0.);
 
+	engine_force=0;
+	brake=0;
+
+
 
-	mass=1;
 	friction=1;
 
 	ccd=false;
@@ -842,5 +1039,6 @@ VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
 	exclude.insert(get_rid());
 	PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
 
+	set_mass(40);
 }
 
diff --git a/scene/3d/vehicle_body.h b/scene/3d/vehicle_body.h
index 97137da699..285cca142d 100644
--- a/scene/3d/vehicle_body.h
+++ b/scene/3d/vehicle_body.h
@@ -14,6 +14,8 @@ friend class VehicleBody;
 
 	Transform m_worldTransform;
 	Transform local_xform;
+	bool engine_traction;
+	bool steers;
 
 
 	Vector3	m_chassisConnectionPointCS; //const
@@ -39,7 +41,7 @@ friend class VehicleBody;
 	real_t	m_rotation;
 	real_t	m_deltaRotation;
 	real_t	m_rollInfluence;
-	real_t	m_engineForce;
+	//real_t	m_engineForce;
 	real_t	m_brake;
 
 	real_t	m_clippedInvContactDotSuspension;
@@ -69,6 +71,35 @@ protected:
 
 public:
 
+	void set_radius(float p_radius);
+	float get_radius() const;
+
+	void set_suspension_rest_length(float p_length);
+	float get_suspension_rest_length() const;
+
+	void set_suspension_travel(float p_length);
+	float get_suspension_travel() const;
+
+	void set_suspension_stiffness(float p_value);
+	float get_suspension_stiffness() const;
+
+	void set_suspension_max_force(float p_value);
+	float get_suspension_max_force() const;
+
+	void set_damping_compression(float p_value);
+	float get_damping_compression() const;
+
+	void set_damping_relaxation(float p_value);
+	float get_damping_relaxation() const;
+
+	void set_friction_slip(float p_value);
+	float get_friction_slip() const;
+
+	void set_use_as_traction(bool p_enable);
+	bool is_used_as_traction() const;
+
+	void set_use_as_steering(bool p_enabled);
+	bool is_used_as_steering() const;
 
 	VehicleWheel();
 
@@ -82,6 +113,9 @@ class VehicleBody : public PhysicsBody {
 	real_t mass;
 	real_t friction;
 
+	float engine_force;
+	float brake;
+
 	Vector3 linear_velocity;
 	Vector3  angular_velocity;
 	bool ccd;
@@ -135,6 +169,15 @@ public:
 	void set_friction(real_t p_friction);
 	real_t get_friction() const;
 
+	void set_engine_force(float p_engine_force);
+	float get_engine_force() const;
+
+	void set_brake(float p_force);
+	float get_brake() const;
+
+	void set_steering(float p_steering);
+	float get_steering() const;
+
 
 	VehicleBody();
 };
diff --git a/scene/gui/tree.cpp b/scene/gui/tree.cpp
index fb85f0c6b7..25f04379ef 100644
--- a/scene/gui/tree.cpp
+++ b/scene/gui/tree.cpp
@@ -2787,7 +2787,7 @@ int Tree::get_item_offset(TreeItem *p_item) const {
 
 		ofs+=compute_item_height(it)+cache.vseparation;
 
-		if (it->childs) {
+		if (it->childs && !it->collapsed) {
 
 			it=it->childs;
 
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index f7d6a246e6..02fed25883 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -401,6 +401,7 @@ void register_scene_types() {
 	ObjectTypeDB::register_virtual_type<CollisionObject>();
 	ObjectTypeDB::register_type<StaticBody>();
 	ObjectTypeDB::register_type<RigidBody>();
+	ObjectTypeDB::register_type<KinematicBody>();
 	ObjectTypeDB::register_type<CarBody>();
 	ObjectTypeDB::register_type<CarWheel>();
 	ObjectTypeDB::register_type<VehicleBody>();
diff --git a/servers/audio/sample_manager_sw.cpp b/servers/audio/sample_manager_sw.cpp
index 5a5aa1a34c..29564fe018 100644
--- a/servers/audio/sample_manager_sw.cpp
+++ b/servers/audio/sample_manager_sw.cpp
@@ -139,7 +139,7 @@ void SampleManagerMallocSW::sample_set_data(RID p_sample, const DVector<uint8_t>
 	DVector<uint8_t>::Read buffer_r=p_buffer.read();
 	const uint8_t *src = buffer_r.ptr();
 	uint8_t *dst = (uint8_t*)s->data;
-	print_line("set data: "+itos(s->length_bytes));
+	//print_line("set data: "+itos(s->length_bytes));
 
 	for(int i=0;i<s->length_bytes;i++) {
 
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index 3e39dc3bb6..569195d2c3 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -70,7 +70,7 @@ class AreaSW : public CollisionObjectSW{
 
 					return area_shape < p_key.area_shape;
 				} else
-					return body_shape < p_key.area_shape;
+					return body_shape < p_key.body_shape;
 			} else
 				return rid < p_key.rid;
 
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index c50bfac472..3c838367c2 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -29,7 +29,7 @@
 #include "body_pair_sw.h"
 #include "collision_solver_sw.h"
 #include "space_sw.h"
-
+#include "os/os.h"
 
 /*
 #define NO_ACCUMULATE_IMPULSES
@@ -174,6 +174,11 @@ void BodyPairSW::validate_contacts() {
 
 bool BodyPairSW::setup(float p_step) {
 
+	//cannot collide
+	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
+		collided=false;
+		return false;
+	}
 
 	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
 
@@ -197,10 +202,6 @@ bool BodyPairSW::setup(float p_step) {
 		return false;
 
 
-	//cannot collide
-	if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
-		return false;
-	}
 
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
@@ -217,6 +218,7 @@ bool BodyPairSW::setup(float p_step) {
 	}
 
 
+
 	real_t inv_dt = 1.0/p_step;
 
 	for(int i=0;i<contact_count;i++) {
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 52edc0faa7..0fd754ba25 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -195,7 +195,7 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 			_set_inv_transform(get_transform().affine_inverse());
 			_inv_mass=0;
 			_set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
-			set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
+			//set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
 			linear_velocity=Vector3();
 			angular_velocity=Vector3();
 		} break;
@@ -203,14 +203,12 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 
 			_inv_mass=mass>0?(1.0/mass):0;
 			_set_static(false);
-			simulated_motion=false; //jic
 
 		} break;
 		case PhysicsServer::BODY_MODE_CHARACTER: {
 
 			_inv_mass=mass>0?(1.0/mass):0;
 			_set_static(false);
-			simulated_motion=false; //jic
 		} break;
 	}
 
@@ -235,13 +233,19 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
 		case PhysicsServer::BODY_STATE_TRANSFORM: {
 
 
-			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+			if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+				new_transform=p_variant;
+				//wakeup_neighbours();
+				set_active(true);
+
+			} else if (mode==PhysicsServer::BODY_MODE_STATIC) {
 				_set_transform(p_variant);
 				_set_inv_transform(get_transform().affine_inverse());
 				wakeup_neighbours();
 			} else {
 				Transform t = p_variant;
 				t.orthonormalize();
+				new_transform=get_transform(); //used as old to compute motion
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
 
@@ -353,7 +357,7 @@ void BodySW::_compute_area_gravity(const AreaSW *p_area) {
 void BodySW::integrate_forces(real_t p_step) {
 
 
-	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+	if (mode==PhysicsServer::BODY_MODE_STATIC)
 		return;
 
 	AreaSW *current_area = get_space()->get_default_area();
@@ -374,28 +378,56 @@ void BodySW::integrate_forces(real_t p_step) {
 	_compute_area_gravity(current_area);
 	density=current_area->get_density();
 
-	if (!omit_force_integration) {
-		//overriden by direct state query
+	Vector3 motion;
+	bool do_motion=false;
 
-		Vector3 force=gravity*mass;
-		force+=applied_force;
-		Vector3 torque=applied_torque;
+	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
 
-		real_t damp = 1.0 - p_step * density;
+		//compute motion, angular and etc. velocities from prev transform
+		linear_velocity = (new_transform.origin - get_transform().origin)/p_step;
 
-		if (damp<0) // reached zero in the given time
-			damp=0;
+		//compute a FAKE angular velocity, not so easy
+		Matrix3 rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
+		Vector3 axis;
+		float angle;
+
+		rot.get_axis_and_angle(axis,angle);
+		axis.normalize();
+		angular_velocity=axis.normalized() * (angle/p_step);
+
+		motion = new_transform.origin - get_transform().origin;
+		do_motion=true;
+
+	} else {
+		if (!omit_force_integration) {
+			//overriden by direct state query
 
-		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+			Vector3 force=gravity*mass;
+			force+=applied_force;
+			Vector3 torque=applied_torque;
 
-		if (angular_damp<0) // reached zero in the given time
-			angular_damp=0;
+			real_t damp = 1.0 - p_step * density;
 
-		linear_velocity*=damp;
-		angular_velocity*=angular_damp;
+			if (damp<0) // reached zero in the given time
+				damp=0;
+
+			real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+
+			if (angular_damp<0) // reached zero in the given time
+				angular_damp=0;
+
+			linear_velocity*=damp;
+			angular_velocity*=angular_damp;
+
+			linear_velocity+=_inv_mass * force * p_step;
+			angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
+		}
+
+		if (continuous_cd) {
+			motion=linear_velocity*p_step;
+			do_motion=true;
+		}
 
-		linear_velocity+=_inv_mass * force * p_step;
-		angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
 	}
 
 	applied_force=Vector3();
@@ -406,8 +438,11 @@ void BodySW::integrate_forces(real_t p_step) {
 	biased_angular_velocity=Vector3();
 	biased_linear_velocity=Vector3();
 
-	if (continuous_cd) //shapes temporarily extend for raycast
-		_update_shapes_with_motion(linear_velocity*p_step);
+
+	if (do_motion) {//shapes temporarily extend for raycast
+		_update_shapes_with_motion(motion);
+	}
+
 
 	current_area=NULL; // clear the area, so it is set in the next frame
 	contact_count=0;
@@ -419,9 +454,16 @@ void BodySW::integrate_velocities(real_t p_step) {
 	if (mode==PhysicsServer::BODY_MODE_STATIC)
 		return;
 
+	if (fi_callback)
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+
 	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
-		if (fi_callback)
-			get_space()->body_add_to_state_query_list(&direct_state_query_list);
+
+		_set_transform(new_transform,false);
+		_set_inv_transform(new_transform.affine_inverse());				;
+		if (linear_velocity==Vector3() && angular_velocity==Vector3())
+			set_active(false); //stopped moving, deactivate
+
 		return;
 	}
 
@@ -475,14 +517,13 @@ void BodySW::integrate_velocities(real_t p_step) {
 
 	_update_inertia_tensor();
 
-	if (fi_callback) {
-
-		get_space()->body_add_to_state_query_list(&direct_state_query_list);
-	}
+	//if (fi_callback) {
 
+	//	get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	//
 }
 
-
+/*
 void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
 
 	Transform inv_xform = p_xform.affine_inverse();
@@ -514,6 +555,7 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
 
 
 }
+*/
 
 void BodySW::wakeup_neighbours() {
 
@@ -562,12 +604,7 @@ void BodySW::call_queries() {
 
 	}
 
-	if (simulated_motion) {
 
-	//	linear_velocity=Vector3();
-	//	angular_velocity=0;
-		simulated_motion=false;
-	}
 }
 
 
@@ -634,7 +671,7 @@ BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_upda
 	_set_static(false);
 	density=0;
 	contact_count=0;
-	simulated_motion=false;
+
 	still_time=0;
 	continuous_cd=false;
 	can_sleep=false;
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 8d30069777..f627c41231 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -71,11 +71,12 @@ class BodySW : public CollisionObjectSW {
 	VSet<RID> exceptions;
 	bool omit_force_integration;
 	bool active;
-	bool simulated_motion;
+
 	bool continuous_cd;
 	bool can_sleep;
 	void _update_inertia();
 	virtual void _shapes_changed();
+	Transform new_transform;
 
 	Map<ConstraintSW*,int> constraint_map;
 
@@ -235,7 +236,7 @@ public:
 	void integrate_forces(real_t p_step);
 	void integrate_velocities(real_t p_step);
 
-	void simulate_motion(const Transform& p_xform,real_t p_step);
+	//void simulate_motion(const Transform& p_xform,real_t p_step);
 	void call_queries();
 	void wakeup_neighbours();
 
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index 156004d15d..f34aa19cae 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -216,4 +216,5 @@ CollisionObjectSW::CollisionObjectSW(Type p_type) {
 	type=p_type;
 	space=NULL;
 	instance_id=0;
+	layer_mask=1;
 }
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index 6d60f2f078..558a48f6fd 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -47,6 +47,7 @@ private:
 	Type type;
 	RID self;
 	ObjectID instance_id;
+	uint32_t layer_mask;
 
 	struct Shape {
 
@@ -71,7 +72,7 @@ protected:
 	void _update_shapes_with_motion(const Vector3& p_motion);
 	void _unregister_shapes();
 
-	_FORCE_INLINE_ void _set_transform(const Transform& p_transform) { transform=p_transform; _update_shapes(); }
+	_FORCE_INLINE_ void _set_transform(const Transform& p_transform,bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) _update_shapes(); }
 	_FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; }
 	void _set_static(bool p_static);
 
@@ -104,6 +105,8 @@ public:
 	_FORCE_INLINE_ SpaceSW* get_space() const { return space; }
 
 
+	_FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; }
+	_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
 
 	void remove_shape(ShapeSW *p_shape);
 	void remove_shape(int p_index);
diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp
index 1cd40db772..750874f507 100644
--- a/servers/physics/collision_solver_sat.cpp
+++ b/servers/physics/collision_solver_sat.cpp
@@ -43,7 +43,9 @@ struct _CollectorCallback {
 	_FORCE_INLINE_ void call(const Vector3& p_point_A, const Vector3& p_point_B) {
 
 		//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
-		// return;
+		//	return;
+//		print_line("** A: "+p_point_A+" B: "+p_point_B+" D: "+rtos(p_point_A.distance_to(p_point_B)));
+
 		if (swap)
 			callback(p_point_B,p_point_A,userdata);
 		else
@@ -231,11 +233,14 @@ static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_
 	for (int i=0;i<clipbuf_len;i++) {
 
 		float d = plane_B.distance_to(clipbuf_src[i]);
-		if (d>CMP_EPSILON)
-			continue;
+		//if (d>CMP_EPSILON)
+		//	continue;
 
 		Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d;
 
+		if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B))
+			continue;
+
 		p_callback->call(clipbuf_src[i],closest_B);
 		added++;
 
@@ -301,7 +306,7 @@ static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_po
 
 
 
-template<class ShapeA, class ShapeB>
+template<class ShapeA, class ShapeB, bool withMargin=false>
 class SeparatorAxisTest {
 
 	const ShapeA *shape_A;
@@ -311,7 +316,8 @@ class SeparatorAxisTest {
 	real_t best_depth;
 	Vector3 best_axis;
 	_CollectorCallback *callback;
-
+	real_t margin_A;
+	real_t margin_B;
 	Vector3 separator_axis;
 
 public:
@@ -340,6 +346,13 @@ public:
 		shape_A->project_range(axis,*transform_A,min_A,max_A);
 		shape_B->project_range(axis,*transform_B,min_B,max_B);
 
+		if (withMargin) {
+			min_A-=margin_A;
+			max_A+=margin_A;
+			min_B-=margin_B;
+			max_B+=margin_B;
+		}
+
 		min_B -= ( max_A - min_A ) * 0.5;
 		max_B += ( max_A - min_A ) * 0.5;
 
@@ -394,6 +407,14 @@ public:
 			supports_A[i] = transform_A->xform(supports_A[i]);
 		}
 
+		if (withMargin) {
+
+			for(int i=0;i<support_count_A;i++) {
+				supports_A[i]+=-best_axis*margin_A;
+			}
+
+		}
+
 
 		Vector3 supports_B[max_supports];
 		int support_count_B;
@@ -401,8 +422,16 @@ public:
 		for(int i=0;i<support_count_B;i++) {
 			supports_B[i] = transform_B->xform(supports_B[i]);
 		}
+
+		if (withMargin) {
+
+			for(int i=0;i<support_count_B;i++) {
+				supports_B[i]+=best_axis*margin_B;
+			}
+		}
 /*
 		print_line("best depth: "+rtos(best_depth));
+		print_line("best axis: "+(best_axis));
 		for(int i=0;i<support_count_A;i++) {
 
 			print_line("A-"+itos(i)+": "+supports_A[i]);
@@ -423,13 +452,16 @@ public:
 
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback) {
+	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback,real_t p_margin_A=0,real_t p_margin_B=0) {
 		best_depth=1e15;
 		shape_A=p_shape_A;
 		shape_B=p_shape_B;
 		transform_A=&p_transform_A;
 		transform_B=&p_transform_B;
 		callback=p_callback;
+		margin_A=p_margin_A;
+		margin_B=p_margin_B;
+
 	}
 
 };
@@ -440,16 +472,17 @@ public:
 /****** SAT TESTS *******/
 
 
-typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback);
+typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback,float,float);
 
 
-static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
 	const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,SphereShapeSW> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector);
+	SeparatorAxisTest<SphereShapeSW,SphereShapeSW,withMargin> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	// previous axis
 
@@ -462,13 +495,14 @@ static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_trans
 	separator.generate_contacts();
 }
 
-static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
 	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,BoxShapeSW> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector);
+	SeparatorAxisTest<SphereShapeSW,BoxShapeSW,withMargin> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -516,13 +550,13 @@ static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transfor
 
 }
 
-
-static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
 	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+	SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW,withMargin> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -540,7 +574,7 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran
 
 	Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis;
 
-	if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) )
+	if (!separator.test_axis( (capsule_ball_2 - p_transform_a.origin).normalized() ) )
 		return;
 
 	//capsule edge, sphere
@@ -556,13 +590,14 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran
 	separator.generate_contacts();
 }
 
-static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
 	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+	SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW,withMargin> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 
 	if (!separator.test_previous_axis())
@@ -626,14 +661,15 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform
 
 }
 
-static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
 	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
 
 
 
-	SeparatorAxisTest<SphereShapeSW,FaceShapeSW> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector);
+	SeparatorAxisTest<SphereShapeSW,FaceShapeSW,withMargin> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 
 	Vector3 vertex[3]={
@@ -669,16 +705,14 @@ static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transfo
 }
 
 
-
-
-
-static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
 	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
 
-	SeparatorAxisTest<BoxShapeSW,BoxShapeSW> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector);
+	SeparatorAxisTest<BoxShapeSW,BoxShapeSW,withMargin> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -723,18 +757,69 @@ static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a
 		}
 	}
 
+	if (withMargin) {
+		//add endpoint test between closest vertices and edges
+
+		// calculate closest point to sphere
+
+		Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin;
+
+		Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
+
+		Vector3 support_a=p_transform_a.xform( Vector3(
+
+			(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+			(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+			(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
+		) );
+
+
+		Vector3 cnormal_b=p_transform_b.basis.xform_inv( -ab_vec );
+
+		Vector3 support_b=p_transform_b.xform( Vector3(
+
+			(cnormal_b.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
+			(cnormal_b.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
+			(cnormal_b.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z
+		) );
+
+		Vector3 axis_ab = (support_a-support_b);
+
+		if (!separator.test_axis( axis_ab.normalized()  )) {
+			return;
+		}
+
+		//now try edges, which become cylinders!
+
+		for(int i=0;i<3;i++) {
+
+			//a ->b
+			Vector3 axis_a = p_transform_a.basis.get_axis(i);
+
+			if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+				return;
+
+			//b ->a
+			Vector3 axis_b = p_transform_b.basis.get_axis(i);
+
+			if (!separator.test_axis( axis_ab.cross(axis_b).cross(axis_b).normalized() ))
+				return;
+
+		}
+	}
+
 	separator.generate_contacts();
 
 
 }
 
-
-static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
 	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
 
-	SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+	SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW,withMargin> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -828,15 +913,15 @@ static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transfo
 	separator.generate_contacts();
 }
 
-
-static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 
 	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
 	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
 
-	SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+	SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW,withMargin> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -886,18 +971,84 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_
 		}
 	}
 
+	if (withMargin) {
+
+		// calculate closest points between vertices and box edges
+		for(int v=0;v<vertex_count;v++) {
+
+
+			Vector3 vtxb = p_transform_b.xform(vertices[v]);
+			Vector3 ab_vec = vtxb - p_transform_a.origin;
+
+			Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
+
+			Vector3 support_a=p_transform_a.xform( Vector3(
+
+				(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+				(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+				(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
+			) );
+
+
+			Vector3 axis_ab = support_a-vtxb;
+
+			if (!separator.test_axis( axis_ab.normalized()  )) {
+				return;
+			}
+
+			//now try edges, which become cylinders!
+
+			for(int i=0;i<3;i++) {
+
+				//a ->b
+				Vector3 axis_a = p_transform_a.basis.get_axis(i);
+
+				if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+					return;
+			}
+		}
+
+		//convex edges and box points
+		for (int i=0;i<2;i++) {
+			for (int j=0;j<2;j++) {
+				for (int k=0;k<2;k++) {
+					Vector3 he = box_A->get_half_extents();
+					he.x*=(i*2-1);
+					he.y*=(j*2-1);
+					he.z*=(k*2-1);
+					Vector3 point=p_transform_a.origin;
+					for(int l=0;l<3;l++)
+						point+=p_transform_a.basis.get_axis(l)*he[l];
+
+					for(int e=0;e<edge_count;e++) {
+
+						Vector3 p1=p_transform_b.xform(vertices[edges[e].a]);
+						Vector3 p2=p_transform_b.xform(vertices[edges[e].b]);
+						Vector3 n = (p2-p1);
+
+
+						if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() ))
+							return;
+					}
+				}
+			}
+		}
+	}
+
 	separator.generate_contacts();
 
 
 }
 
-static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+template<bool withMargin>
+static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
 	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
 
-	SeparatorAxisTest<BoxShapeSW,FaceShapeSW> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector);
+	SeparatorAxisTest<BoxShapeSW,FaceShapeSW,withMargin> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	Vector3 vertex[3]={
 		p_transform_b.xform( face_B->vertex[0] ),
@@ -918,13 +1069,14 @@ static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_
 	}
 
 	// combined edges
+
 	for(int i=0;i<3;i++) {
 
 		Vector3 e=vertex[i]-vertex[(i+1)%3];
 
-		for (int i=0;i<3;i++) {
+		for (int j=0;j<3;j++) {
 
-			Vector3 axis = p_transform_a.basis.get_axis(i);
+			Vector3 axis = p_transform_a.basis.get_axis(j);
 
 			if (!separator.test_axis( e.cross(axis).normalized() ))
 				return;
@@ -932,16 +1084,82 @@ static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_
 
 	}
 
+	if (withMargin) {
+
+		// calculate closest points between vertices and box edges
+		for(int v=0;v<3;v++) {
+
+
+			Vector3 ab_vec = vertex[v] - p_transform_a.origin;
+
+			Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
+
+			Vector3 support_a=p_transform_a.xform( Vector3(
+
+				(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+				(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+				(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
+			) );
+
+
+			Vector3 axis_ab = support_a-vertex[v];
+
+			if (!separator.test_axis( axis_ab.normalized()  )) {
+				return;
+			}
+
+			//now try edges, which become cylinders!
+
+			for(int i=0;i<3;i++) {
+
+				//a ->b
+				Vector3 axis_a = p_transform_a.basis.get_axis(i);
+
+				if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+					return;
+			}
+		}
+
+		//convex edges and box points, there has to be a way to speed up this (get closest point?)
+		for (int i=0;i<2;i++) {
+			for (int j=0;j<2;j++) {
+				for (int k=0;k<2;k++) {
+					Vector3 he = box_A->get_half_extents();
+					he.x*=(i*2-1);
+					he.y*=(j*2-1);
+					he.z*=(k*2-1);
+					Vector3 point=p_transform_a.origin;
+					for(int l=0;l<3;l++)
+						point+=p_transform_a.basis.get_axis(l)*he[l];
+
+					for(int e=0;e<3;e++) {
+
+						Vector3 p1=vertex[e];
+						Vector3 p2=vertex[(e+1)%3];
+
+						Vector3 n = (p2-p1);
+
+						if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() ))
+							return;
+					}
+				}
+			}
+		}
+
+	}
+
 	separator.generate_contacts();
 
 }
 
-static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+
+template<bool withMargin>
+static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
 	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
 
-	SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector);
+	SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -993,13 +1211,14 @@ static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_tra
 
 }
 
-static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
 	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
 
-	SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+	SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW,withMargin> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1063,12 +1282,14 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform
 
 }
 
-static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+template<bool withMargin>
+static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
 	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
 
-	SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector);
+	SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW,withMargin> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 
 
@@ -1125,13 +1346,14 @@ static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transf
 }
 
 
-static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) {
+template<bool withMargin>
+static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
 	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
 
-	SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector);
+	SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1192,17 +1414,70 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Tr
 		}
 	}
 
+	if (withMargin) {
+
+		//vertex-vertex
+		for(int i=0;i<vertex_count_A;i++) {
+
+			Vector3 va = p_transform_a.xform(vertices_A[i]);
+
+			for(int j=0;j<vertex_count_B;j++) {
+
+				if (!separator.test_axis( (va-p_transform_b.xform(vertices_B[j])).normalized() ))
+					return;
+
+			}
+		}
+		//edge-vertex( hsell)
+
+		for (int i=0;i<edge_count_A;i++) {
+
+			Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] );
+			Vector3 e2=p_transform_a.basis.xform( vertices_A[ edges_A[i].b] );
+			Vector3 n = (e2-e1);
+
+			for(int j=0;j<vertex_count_B;j++) {
+
+				Vector3 e3=p_transform_b.xform(vertices_B[j]);
+
+
+				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+					return;
+			}
+		}
+
+		for (int i=0;i<edge_count_B;i++) {
+
+			Vector3 e1=p_transform_b.basis.xform( vertices_B[ edges_B[i].a] );
+			Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[i].b] );
+			Vector3 n = (e2-e1);
+
+			for(int j=0;j<vertex_count_A;j++) {
+
+				Vector3 e3=p_transform_a.xform(vertices_A[j]);
+
+
+				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+					return;
+			}
+		}
+
+
+	}
+
 	separator.generate_contacts();
 
 }
 
-static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) {
+
+template<bool withMargin>
+static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
 
 
 	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
 	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
 
-	SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector);
+	SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
 
 	const Geometry::MeshData &mesh = convex_polygon_A->get_mesh();
 
@@ -1252,12 +1527,62 @@ static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p
 		}
 	}
 
+
+	if (withMargin) {
+
+		//vertex-vertex
+		for(int i=0;i<vertex_count;i++) {
+
+			Vector3 va = p_transform_a.xform(vertices[i]);
+
+			for(int j=0;j<3;j++) {
+
+				if (!separator.test_axis( (va-vertex[j]).normalized() ))
+					return;
+
+			}
+		}
+		//edge-vertex( hsell)
+
+		for (int i=0;i<edge_count;i++) {
+
+			Vector3 e1=p_transform_a.basis.xform( vertices[ edges[i].a] );
+			Vector3 e2=p_transform_a.basis.xform( vertices[ edges[i].b] );
+			Vector3 n = (e2-e1);
+
+			for(int j=0;j<3;j++) {
+
+				Vector3 e3=vertex[j];
+
+
+				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+					return;
+			}
+		}
+
+		for (int i=0;i<3;i++) {
+
+			Vector3 e1=vertex[i];
+			Vector3 e2=vertex[(i+1)%3];
+			Vector3 n = (e2-e1);
+
+			for(int j=0;j<vertex_count;j++) {
+
+				Vector3 e3=p_transform_a.xform(vertices[j]);
+
+
+				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+					return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 
 }
 
 
-bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis) {
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis,float p_margin_a,float p_margin_b) {
 
 	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
 
@@ -1273,26 +1598,54 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_tran
 
 
 	static const CollisionFunc collision_table[5][5]={
-		{_collision_sphere_sphere,
-		 _collision_sphere_box,
-		 _collision_sphere_capsule,
-		 _collision_sphere_convex_polygon,
-		 _collision_sphere_face},
+		{_collision_sphere_sphere<false>,
+		 _collision_sphere_box<false>,
+		 _collision_sphere_capsule<false>,
+		 _collision_sphere_convex_polygon<false>,
+		 _collision_sphere_face<false>},
+		{0,
+		 _collision_box_box<false>,
+		 _collision_box_capsule<false>,
+		 _collision_box_convex_polygon<false>,
+		 _collision_box_face<false>},
+		{0,
+		 0,
+		 _collision_capsule_capsule<false>,
+		 _collision_capsule_convex_polygon<false>,
+		 _collision_capsule_face<false>},
+		{0,
+		 0,
+		 0,
+		 _collision_convex_polygon_convex_polygon<false>,
+		 _collision_convex_polygon_face<false>},
 		{0,
-		 _collision_box_box,
-		 _collision_box_capsule,
-		 _collision_box_convex_polygon,
-		 _collision_box_face},
+		 0,
+		 0,
+		 0,
+		 0},
+	};
+
+	static const CollisionFunc collision_table_margin[5][5]={
+		{_collision_sphere_sphere<true>,
+		 _collision_sphere_box<true>,
+		 _collision_sphere_capsule<true>,
+		 _collision_sphere_convex_polygon<true>,
+		 _collision_sphere_face<true>},
+		{0,
+		 _collision_box_box<true>,
+		 _collision_box_capsule<true>,
+		 _collision_box_convex_polygon<true>,
+		 _collision_box_face<true>},
 		{0,
 		 0,
-		 _collision_capsule_capsule,
-		 _collision_capsule_convex_polygon,
-		 _collision_capsule_face},
+		 _collision_capsule_capsule<true>,
+		 _collision_capsule_convex_polygon<true>,
+		 _collision_capsule_face<true>},
 		{0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon,
-		 _collision_convex_polygon_face},
+		 _collision_convex_polygon_convex_polygon<true>,
+		 _collision_convex_polygon_face<true>},
 		{0,
 		 0,
 		 0,
@@ -1311,20 +1664,30 @@ bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_tran
 	const ShapeSW *B=p_shape_B;
 	const Transform *transform_A=&p_transform_A;
 	const Transform *transform_B=&p_transform_B;
+	float margin_A=p_margin_a;
+	float margin_B=p_margin_b;
 
 	if (type_A > type_B) {
 		SWAP(A,B);
 		SWAP(transform_A,transform_B);
 		SWAP(type_A,type_B);
+		SWAP(margin_A,margin_B);
 		callback.swap = !callback.swap;
 	}
 
 
-	CollisionFunc collision_func = collision_table[type_A-2][type_B-2];
+	CollisionFunc collision_func;
+	if (margin_A!=0.0 || margin_B!=0.0) {
+		collision_func = collision_table_margin[type_A-2][type_B-2];
+
+	} else {
+		collision_func = collision_table[type_A-2][type_B-2];
+
+	}
 	ERR_FAIL_COND_V(!collision_func,false);
 
 
-	collision_func(A,*transform_A,B,*transform_B,&callback);
+	collision_func(A,*transform_A,B,*transform_B,&callback,margin_A,margin_B);
 
 	return callback.collided;
 
diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h
index 5023a17810..eeba53f160 100644
--- a/servers/physics/collision_solver_sat.h
+++ b/servers/physics/collision_solver_sat.h
@@ -32,6 +32,6 @@
 #include "collision_solver_sw.h"
 
 
-bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL);
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL,float p_margin_a=0,float p_margin_b=0);
 
 #endif // COLLISION_SOLVER_SAT_H
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp
index da28a4934f..673f2d4385 100644
--- a/servers/physics/collision_solver_sw.cpp
+++ b/servers/physics/collision_solver_sw.cpp
@@ -114,6 +114,10 @@ struct _ConcaveCollisionInfo {
 	bool collided;
 	int aabb_tests;
 	int collisions;
+	bool tested;
+	float margin_A;
+	float margin_B;
+	Vector3 close_A,close_B;
 
 };
 
@@ -123,7 +127,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
 	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
 	cinfo.aabb_tests++;
 
-	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result );
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,NULL,cinfo.margin_A,cinfo.margin_B);
 	if (!collided)
 		return;
 
@@ -132,7 +136,7 @@ void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
 
 }
 
-bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A,float p_margin_B) {
 
 
 	const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
@@ -146,6 +150,8 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform&
 	cinfo.swap_result=p_swap_result;
 	cinfo.collided=false;
 	cinfo.collisions=0;
+	cinfo.margin_A=p_margin_A;
+	cinfo.margin_B=p_margin_B;
 
 	cinfo.aabb_tests=0;
 
@@ -163,21 +169,23 @@ bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform&
 
 	     float smin,smax;
 	     p_shape_A->project_range(axis,rel_transform,smin,smax);
+	     smin-=p_margin_A;
+	     smax+=p_margin_A;
 	     smin*=axis_scale;
 	     smax*=axis_scale;
 
+
 	     local_aabb.pos[i]=smin;
 	     local_aabb.size[i]=smax-smin;
 	}
 
 	concave_B->cull(local_aabb,concave_callback,&cinfo);
 
-
 	return cinfo.collided;
 }
 
 
-bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) {
+bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis,float p_margin_A,float p_margin_B) {
 
 
 	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
@@ -225,17 +233,126 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p
 			return false;
 
 		if (!swap)
-			return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+			return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,p_margin_A,p_margin_B);
 		else
-			return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+			return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,p_margin_A,p_margin_B);
+
+
+
+	} else {
+
+		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis,p_margin_A,p_margin_B);
+	}
+
+
+	return false;
+}
+
+
+void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
+
+
+	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
+	cinfo.aabb_tests++;
+	if (cinfo.collided)
+		return;
+
+	Vector3 close_A,close_B;
+	cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A,*cinfo.transform_A,p_convex,*cinfo.transform_B,close_A,close_B);
+
+	if (cinfo.collided)
+		return;
+	if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
+
+		cinfo.close_A=close_A;
+		cinfo.close_B=close_B;
+		cinfo.tested=true;
+	}
+
+	cinfo.collisions++;
+
+}
+
+
+bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) {
+
+	if (p_shape_A->is_concave())
+		return false;
+
+	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) {
+
+		return false; //unsupported
 
+	} else if (p_shape_B->is_concave()) {
 
+		if (p_shape_A->is_concave())
+			return false;
+
+
+		const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
+
+		_ConcaveCollisionInfo cinfo;
+		cinfo.transform_A=&p_transform_A;
+		cinfo.shape_A=p_shape_A;
+		cinfo.transform_B=&p_transform_B;
+		cinfo.result_callback=NULL;
+		cinfo.userdata=NULL;
+		cinfo.swap_result=false;
+		cinfo.collided=false;
+		cinfo.collisions=0;
+		cinfo.aabb_tests=0;
+		cinfo.tested=false;
 
+		Transform rel_transform = p_transform_A;
+		rel_transform.origin-=p_transform_B.origin;
+
+		//quickly compute a local AABB
+
+		bool use_cc_hint=p_concave_hint!=AABB();
+		AABB cc_hint_aabb;
+		if (use_cc_hint) {
+			cc_hint_aabb=p_concave_hint;
+			cc_hint_aabb.pos-=p_transform_B.origin;
+		}
+
+		AABB local_aabb;
+		for(int i=0;i<3;i++) {
+
+		     Vector3 axis( p_transform_B.basis.get_axis(i) );
+		     float axis_scale = 1.0/axis.length();
+		     axis*=axis_scale;
+
+		     float smin,smax;
+
+		     if (use_cc_hint) {
+			     cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax);
+		     } else {
+			     p_shape_A->project_range(axis,rel_transform,smin,smax);
+		      }
+
+		     smin*=axis_scale;
+		     smax*=axis_scale;
+
+		     local_aabb.pos[i]=smin;
+		     local_aabb.size[i]=smax-smin;
+		}
+
+
+		concave_B->cull(local_aabb,concave_distance_callback,&cinfo);
+		if (!cinfo.collided) {
+//			print_line(itos(cinfo.tested));
+			r_point_A=cinfo.close_A;
+			r_point_B=cinfo.close_B;
+
+		}
+
+		return !cinfo.collided;
 	} else {
 
-		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis);
+		return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis..
 	}
 
 
 	return false;
 }
+
diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h
index e135ab92e0..430f057c7c 100644
--- a/servers/physics/collision_solver_sw.h
+++ b/servers/physics/collision_solver_sw.h
@@ -40,12 +40,14 @@ private:
 	static void concave_callback(void *p_userdata, ShapeSW *p_convex);
 	static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
 	static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
-	static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A=0,float p_margin_B=0);
+	static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex);
 
 public:
 
 
-	static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL);
+	static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
+	static bool solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis=NULL);
 
 };
 
diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp
index 37edc0d414..9b5b3d4f67 100644
--- a/servers/physics/gjk_epa.cpp
+++ b/servers/physics/gjk_epa.cpp
@@ -1,31 +1,14 @@
-/*************************************************************************/
-/*  gjk_epa.cpp                                                          */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                    http://www.godotengine.org                         */
-/*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
-/*                                                                       */
-/* 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.                */
-/*************************************************************************/
+/*************************************************/
+/*  gjk_epa.cpp                                  */
+/*************************************************/
+/*            This file is part of:              */
+/*                GODOT ENGINE                   */
+/*************************************************/
+/*       Source code within this file is:        */
+/*  (c) 2007-2010 Juan Linietsky, Ariel Manzur   */
+/*             All Rights Reserved.              */
+/*************************************************/
+
 #include "gjk_epa.h"
 
 /*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
@@ -798,8 +781,8 @@ bool Distance(	const ShapeSW*	shape0,
 			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
 			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
 		}
-		results.witnesses[0]	=	wtrs0.xform(w0);
-		results.witnesses[1]	=	wtrs0.xform(w1);
+		results.witnesses[0]	=	w0;
+		results.witnesses[1]	=	w1;
 		results.normal			=	w0-w1;
 		results.distance		=	results.normal.length();
 		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
@@ -881,6 +864,24 @@ bool Penetration(	const ShapeSW*	shape0,
 
 
 
+
+
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B) {
+
+
+	GjkEpa2::sResults res;
+
+	if (GjkEpa2::Distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) {
+
+		r_result_A=res.witnesses[0];
+		r_result_B=res.witnesses[1];
+		return true;
+	}
+
+	return false;
+
+}
+
 bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) {
 
 	GjkEpa2::sResults res;
diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h
index 0303478f17..08b0a65b15 100644
--- a/servers/physics/gjk_epa.h
+++ b/servers/physics/gjk_epa.h
@@ -1,31 +1,14 @@
-/*************************************************************************/
-/*  gjk_epa.h                                                            */
-/*************************************************************************/
-/*                       This file is part of:                           */
-/*                           GODOT ENGINE                                */
-/*                    http://www.godotengine.org                         */
-/*************************************************************************/
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
-/*                                                                       */
-/* 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.                */
-/*************************************************************************/
+/*************************************************/
+/*  gjk_epa.h                                    */
+/*************************************************/
+/*            This file is part of:              */
+/*                GODOT ENGINE                   */
+/*************************************************/
+/*       Source code within this file is:        */
+/*  (c) 2007-2010 Juan Linietsky, Ariel Manzur   */
+/*             All Rights Reserved.              */
+/*************************************************/
+
 #ifndef GJK_EPA_H
 #define GJK_EPA_H
 
@@ -36,5 +19,6 @@
 #include "collision_solver_sw.h"
 
 bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false);
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B);
 
 #endif
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index aff60b5881..a30383a88d 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -568,6 +568,25 @@ bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body)
 	return body->is_continuous_collision_detection_enabled();
 }
 
+void PhysicsServerSW::body_set_layer_mask(RID p_body, uint32_t p_mask) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_layer_mask(p_mask);
+
+}
+
+uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const{
+
+	const BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body,0);
+
+	return body->get_layer_mask();
+
+}
+
+
 void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -618,13 +637,6 @@ float PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const {
 };
 
 
-void PhysicsServerSW::body_static_simulate_motion(RID p_body,const Transform& p_new_transform) {
-
-	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND(!body);
-	body->simulate_motion(p_new_transform,last_step);
-
-};
 
 void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
 
@@ -1021,11 +1033,18 @@ void PhysicsServerSW::step(float p_step) {
 	last_step=p_step;
 	PhysicsDirectBodyStateSW::singleton->step=p_step;
 
+	island_count=0;
+	active_objects=0;
+	collision_pairs=0;
 	for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
 
 		stepper->step((SpaceSW*)E->get(),p_step,iterations);
+		island_count+=E->get()->get_island_count();
+		active_objects+=E->get()->get_active_objects();
+		collision_pairs+=E->get()->get_collision_pairs();
 	}
-};
+
+}
 
 void PhysicsServerSW::sync() {
 
@@ -1054,9 +1073,72 @@ void PhysicsServerSW::finish() {
 };
 
 
+int PhysicsServerSW::get_process_info(ProcessInfo p_info) {
+
+	switch(p_info) {
+
+		case INFO_ACTIVE_OBJECTS: {
+
+			return active_objects;
+		} break;
+		case INFO_COLLISION_PAIRS: {
+			return collision_pairs;
+		} break;
+		case INFO_ISLAND_COUNT: {
+
+			return island_count;
+		} break;
+
+	}
+
+	return 0;
+}
+
+
+void PhysicsServerSW::_shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+
+	CollCbkData *cbk=(CollCbkData *)p_userdata;
+
+	if (cbk->max==0)
+		return;
+
+	if (cbk->amount == cbk->max) {
+		//find least deep
+		float min_depth=1e20;
+		int min_depth_idx=0;
+		for(int i=0;i<cbk->amount;i++) {
+
+			float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]);
+			if (d<min_depth) {
+				min_depth=d;
+				min_depth_idx=i;
+			}
+
+		}
+
+		float d = p_point_A.distance_squared_to(p_point_B);
+		if (d<min_depth)
+			return;
+		cbk->ptr[min_depth_idx*2+0]=p_point_A;
+		cbk->ptr[min_depth_idx*2+1]=p_point_B;
+
+
+	} else {
+
+		cbk->ptr[cbk->amount*2+0]=p_point_A;
+		cbk->ptr[cbk->amount*2+1]=p_point_B;
+		cbk->amount++;
+	}
+}
+
+
 PhysicsServerSW::PhysicsServerSW() {
 
 	BroadPhaseSW::create_func=BroadPhaseOctree::_create;
+	island_count=0;
+	active_objects=0;
+	collision_pairs=0;
 
 	active=true;
 
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index 0822d76936..185867e817 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -47,6 +47,10 @@ friend class PhysicsDirectSpaceStateSW;
 	bool doing_sync;
 	real_t last_step;
 
+	int island_count;
+	int active_objects;
+	int collision_pairs;
+
 	StepSW *stepper;
 	Set<const SpaceSW*> active_spaces;
 
@@ -61,6 +65,15 @@ friend class PhysicsDirectSpaceStateSW;
 //	void _clear_query(QuerySW *p_query);
 public:
 
+	struct CollCbkData {
+
+		int max;
+		int amount;
+		Vector3 *ptr;
+	};
+
+	static void _shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+
 	virtual RID shape_create(ShapeType p_shape);
 	virtual void shape_set_data(RID p_shape, const Variant& p_data);
 	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
@@ -146,15 +159,15 @@ public:
 	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
 	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
 
+	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask);
+	virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const;
+
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags);
 	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const;
 
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
 
-	//advanced simulation
-	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform);
-
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
 
@@ -209,6 +222,8 @@ public:
 	virtual void flush_queries();
 	virtual void finish();
 
+	int get_process_info(ProcessInfo p_info);
+
 	PhysicsServerSW();
 	~PhysicsServerSW();
 
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index 1312b7da95..bd4be05bb9 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -928,8 +928,8 @@ void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supp
 	for (int i=0;i<3;i++) {
 
 		int nx=(i+1)%3;
-		//if (i!=vert_support_idx && nx!=vert_support_idx)
-		//	continue;
+		if (i!=vert_support_idx && nx!=vert_support_idx)
+			continue;
 
 	// check if edge is valid as a support
 		float dot=(vertex[i]-vertex[nx]).normalized().dot(n);
@@ -951,8 +951,12 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,
 
 
 	bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);
-	if (c)
+	if (c) {
 		r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;
+		if (r_normal.dot(p_end-p_begin)>0) {
+			r_normal=-r_normal;
+		}
+	}
 
 	return c;
 }
@@ -1070,13 +1074,15 @@ void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params
 				&res)) {
 
 
-			float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);
+			float d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
 			//TODO, seems segmen/triangle intersection is broken :(
 			if (d>0 && d<p_params->min_d) {
 
 				p_params->min_d=d;
 				p_params->result=res;
 				p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;
+				if (p_params->normal.dot(p_params->dir)>0)
+					p_params->normal=-p_params->normal;
 				p_params->collisions++;
 			}
 
@@ -1107,7 +1113,7 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vecto
 	params.from=p_begin;
 	params.to=p_end;
 	params.collisions=0;
-	params.normal=(p_end-p_begin).normalized();
+	params.dir=(p_end-p_begin).normalized();
 
 	params.faces=fr.ptr();
 	params.vertices=vr.ptr();
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
index 890d6d8741..cdb21556b8 100644
--- a/servers/physics/shape_sw.h
+++ b/servers/physics/shape_sw.h
@@ -26,405 +26,446 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#ifndef SHAPE_SW_H
-#define SHAPE_SW_H
-
-#include "servers/physics_server.h"
-#include "bsp_tree.h"
-#include "geometry.h"
-/*
-
-SHAPE_LINE, ///< plane:"plane"
-SHAPE_SEGMENT, ///< float:"length"
-SHAPE_CIRCLE, ///< float:"radius"
-SHAPE_RECTANGLE, ///< vec3:"extents"
-SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
-SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
-SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
-
-*/
-
-class ShapeSW;
-
-class ShapeOwnerSW {
-public:
-
-	virtual void _shape_changed()=0;
-	virtual void remove_shape(ShapeSW *p_shape)=0;
-
-	virtual ~ShapeOwnerSW() {}
-};
-
-
-class ShapeSW {
-
-	RID self;
-	AABB aabb;
-	bool configured;
-	real_t custom_bias;
-
-	Map<ShapeOwnerSW*,int> owners;
-protected:
-
-	void configure(const AABB& p_aabb);
-public:
-
-	enum {
-		MAX_SUPPORTS=8
-	};
-
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
-	_FORCE_INLINE_ RID get_self() const {return  self; }
-
-	virtual PhysicsServer::ShapeType get_type() const=0;
-
-	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
-	_FORCE_INLINE_ bool is_configured() const { return configured; }
-
-	virtual bool is_concave() const { return false; }
-
-	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 bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
-	virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
-
-	virtual void set_data(const Variant& p_data)=0;
-	virtual Variant get_data() const=0;
-
-	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
-	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
-
-	void add_owner(ShapeOwnerSW *p_owner);
-	void remove_owner(ShapeOwnerSW *p_owner);
-	bool is_owner(ShapeOwnerSW *p_owner) const;
-	const Map<ShapeOwnerSW*,int>& get_owners() const;
-
-	ShapeSW();
-	virtual ~ShapeSW();
-};
-
-
-class ConcaveShapeSW : public ShapeSW {
-
-public:
-
-	virtual bool is_concave() const { return true; }
-	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
-
-	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
-
-	ConcaveShapeSW() {}
-};
-
-class PlaneShapeSW : public ShapeSW {
-
-	Plane plane;
-
-	void _setup(const Plane& p_plane);
-public:
-
-	Plane get_plane() const;
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	PlaneShapeSW();
-};
-
-class RayShapeSW : public ShapeSW {
-
-	float length;
-
-	void _setup(float p_length);
-public:
-
-	float get_length() const;
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	RayShapeSW();
-};
-
-class SphereShapeSW : public ShapeSW {
-
-	real_t radius;
-
-	void _setup(real_t p_radius);
-public:
-
-	real_t get_radius() const;
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
-
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	SphereShapeSW();
-};
-
-class BoxShapeSW : public ShapeSW {
-
-	Vector3 half_extents;
-	void _setup(const Vector3& p_half_extents);
-public:
-
-	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
-
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	BoxShapeSW();
-};
-
-class CapsuleShapeSW : public ShapeSW {
-
-	real_t height;
-	real_t radius;
-
-
-	void _setup(real_t p_height,real_t p_radius);
-public:
-
-	_FORCE_INLINE_ real_t get_height() const { return height; }
-	_FORCE_INLINE_ real_t get_radius() const { return radius; }
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
-
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	CapsuleShapeSW();
-};
-
-struct ConvexPolygonShapeSW : public ShapeSW {
-
-	Geometry::MeshData mesh;
-
-	void _setup(const Vector<Vector3>& p_vertices);
-public:
-
-	const Geometry::MeshData& get_mesh() const { return mesh; }
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
-
-	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	ConvexPolygonShapeSW();
-
-};
-
-
-struct _VolumeSW_BVH;
-struct FaceShapeSW;
-
-struct ConcavePolygonShapeSW : public ConcaveShapeSW {
-	// always a trimesh
-
-	struct Face {
-
-		Vector3 normal;
-		int indices[3];
-	};
-
-	DVector<Face> faces;
-	DVector<Vector3> vertices;
-
-	struct BVH {
-
-		AABB aabb;
-		int left;
-		int right;
-
-		int face_index;
-	};
-
-	DVector<BVH> bvh;
-
-	struct _CullParams {
-
-		AABB aabb;
-		Callback callback;
-		void *userdata;
-		const Face *faces;
-		const Vector3 *vertices;
-		const BVH *bvh;
-		FaceShapeSW *face;
-	};
-
-	struct _SegmentCullParams {
-
-		Vector3 from;
-		Vector3 to;
-		const Face *faces;
-		const Vector3 *vertices;
-		const BVH *bvh;
-
-		Vector3 result;
-		Vector3 normal;
-		real_t min_d;
-		int collisions;
-
-	};
-
-	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-	void _cull(int p_idx,_CullParams *p_params) const;
-
-	void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
-
-
-	void _setup(DVector<Vector3> p_faces);
-public:
-
-	DVector<Vector3> get_faces() const;
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
-
-	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 void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
-	virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	ConcavePolygonShapeSW();
-
-};
-
-
-struct HeightMapShapeSW : public ConcaveShapeSW {
-
-	DVector<real_t> heights;
-	int width;
-	int depth;
-	float cell_size;
-
-//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-//	void _cull(int p_idx,_CullParams *p_params) const;
-
-	void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
-public:
-
-	DVector<real_t> get_heights() const;
-	int get_width() const;
-	int get_depth() const;
-	float get_cell_size() const;
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
-
-	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 void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
-	virtual Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
-
-	HeightMapShapeSW();
-
-};
-
-//used internally
-struct FaceShapeSW : public ShapeSW {
-
-	Vector3 normal; //cache
-	Vector3 vertex[3];
-
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
-
-	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
-
-	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	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;
-
-	Vector3 get_moment_of_inertia(float p_mass) const;
-
-	virtual void set_data(const Variant& p_data) {}
-	virtual Variant get_data() const { return Variant(); }
-
-	FaceShapeSW();
-};
-
-
-
-
-
-struct _ShapeTestConvexBSPSW {
-
-	const BSP_Tree *bsp;
-	const ShapeSW *shape;
-	Transform transform;
-
-	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
-
-		shape->project_range(p_normal,transform,r_min,r_max);
-	}
-
-};
-
-
-
-
-#endif // SHAPESW_H
+#ifndef SHAPE_SW_H
+#define SHAPE_SW_H
+
+#include "servers/physics_server.h"
+#include "bsp_tree.h"
+#include "geometry.h"
+/*
+
+SHAPE_LINE, ///< plane:"plane"
+SHAPE_SEGMENT, ///< float:"length"
+SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+
+*/
+
+class ShapeSW;
+
+class ShapeOwnerSW {
+public:
+
+	virtual void _shape_changed()=0;
+	virtual void remove_shape(ShapeSW *p_shape)=0;
+
+	virtual ~ShapeOwnerSW() {}
+};
+
+
+class ShapeSW {
+
+	RID self;
+	AABB aabb;
+	bool configured;
+	real_t custom_bias;
+
+	Map<ShapeOwnerSW*,int> owners;
+protected:
+
+	void configure(const AABB& p_aabb);
+public:
+
+	enum {
+		MAX_SUPPORTS=8
+	};
+
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ RID get_self() const {return  self; }
+
+	virtual PhysicsServer::ShapeType get_type() const=0;
+
+	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
+
+	virtual bool is_concave() const { return false; }
+
+	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 bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
+	virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
+
+	virtual void set_data(const Variant& p_data)=0;
+	virtual Variant get_data() const=0;
+
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
+
+	void add_owner(ShapeOwnerSW *p_owner);
+	void remove_owner(ShapeOwnerSW *p_owner);
+	bool is_owner(ShapeOwnerSW *p_owner) const;
+	const Map<ShapeOwnerSW*,int>& get_owners() const;
+
+	ShapeSW();
+	virtual ~ShapeSW();
+};
+
+
+class ConcaveShapeSW : public ShapeSW {
+
+public:
+
+	virtual bool is_concave() const { return true; }
+	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
+	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+
+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+
+	ConcaveShapeSW() {}
+};
+
+class PlaneShapeSW : public ShapeSW {
+
+	Plane plane;
+
+	void _setup(const Plane& p_plane);
+public:
+
+	Plane get_plane() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	PlaneShapeSW();
+};
+
+class RayShapeSW : public ShapeSW {
+
+	float length;
+
+	void _setup(float p_length);
+public:
+
+	float get_length() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	RayShapeSW();
+};
+
+class SphereShapeSW : public ShapeSW {
+
+	real_t radius;
+
+	void _setup(real_t p_radius);
+public:
+
+	real_t get_radius() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
+
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	SphereShapeSW();
+};
+
+class BoxShapeSW : public ShapeSW {
+
+	Vector3 half_extents;
+	void _setup(const Vector3& p_half_extents);
+public:
+
+	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
+
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	BoxShapeSW();
+};
+
+class CapsuleShapeSW : public ShapeSW {
+
+	real_t height;
+	real_t radius;
+
+
+	void _setup(real_t p_height,real_t p_radius);
+public:
+
+	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
+
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	CapsuleShapeSW();
+};
+
+struct ConvexPolygonShapeSW : public ShapeSW {
+
+	Geometry::MeshData mesh;
+
+	void _setup(const Vector<Vector3>& p_vertices);
+public:
+
+	const Geometry::MeshData& get_mesh() const { return mesh; }
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
+
+	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 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 Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	ConvexPolygonShapeSW();
+
+};
+
+
+struct _VolumeSW_BVH;
+struct FaceShapeSW;
+
+struct ConcavePolygonShapeSW : public ConcaveShapeSW {
+	// always a trimesh
+
+	struct Face {
+
+		Vector3 normal;
+		int indices[3];
+	};
+
+	DVector<Face> faces;
+	DVector<Vector3> vertices;
+
+	struct BVH {
+
+		AABB aabb;
+		int left;
+		int right;
+
+		int face_index;
+	};
+
+	DVector<BVH> bvh;
+
+	struct _CullParams {
+
+		AABB aabb;
+		Callback callback;
+		void *userdata;
+		const Face *faces;
+		const Vector3 *vertices;
+		const BVH *bvh;
+		FaceShapeSW *face;
+	};
+
+	struct _SegmentCullParams {
+
+		Vector3 from;
+		Vector3 to;
+		const Face *faces;
+		const Vector3 *vertices;
+		const BVH *bvh;
+		Vector3 dir;
+
+		Vector3 result;
+		Vector3 normal;
+		real_t min_d;
+		int collisions;
+
+	};
+
+	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+	void _cull(int p_idx,_CullParams *p_params) const;
+
+	void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
+
+
+	void _setup(DVector<Vector3> p_faces);
+public:
+
+	DVector<Vector3> get_faces() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+	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 void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	ConcavePolygonShapeSW();
+
+};
+
+
+struct HeightMapShapeSW : public ConcaveShapeSW {
+
+	DVector<real_t> heights;
+	int width;
+	int depth;
+	float cell_size;
+
+//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+//	void _cull(int p_idx,_CullParams *p_params) const;
+
+	void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
+public:
+
+	DVector<real_t> get_heights() const;
+	int get_width() const;
+	int get_depth() const;
+	float get_cell_size() const;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
+
+	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 void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+	virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data);
+	virtual Variant get_data() const;
+
+	HeightMapShapeSW();
+
+};
+
+//used internally
+struct FaceShapeSW : public ShapeSW {
+
+	Vector3 normal; //cache
+	Vector3 vertex[3];
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
+
+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+	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;
+
+	Vector3 get_moment_of_inertia(float p_mass) const;
+
+	virtual void set_data(const Variant& p_data) {}
+	virtual Variant get_data() const { return Variant(); }
+
+	FaceShapeSW();
+};
+
+
+struct MotionShapeSW : public ShapeSW {
+
+	ShapeSW *shape;
+	Vector3 motion;
+
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
+
+
+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+		Vector3 cast = p_transform.basis.xform(motion);
+		real_t mina,maxa;
+		real_t minb,maxb;
+		Transform ofsb = p_transform;
+		ofsb.origin+=cast;
+		shape->project_range(p_normal,p_transform,mina,maxa);
+		shape->project_range(p_normal,ofsb,minb,maxb);
+		r_min=MIN(mina,minb);
+		r_max=MAX(maxa,maxb);
+	}
+
+	Vector3 get_support(const Vector3& p_normal) const {
+
+		Vector3 support = shape->get_support(p_normal);
+		if (p_normal.dot(motion)>0) {
+			support+=motion;
+		}
+		return support;
+	}
+	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 { return false; }
+
+	Vector3 get_moment_of_inertia(float p_mass) const { return Vector3(); }
+
+	virtual void set_data(const Variant& p_data) {}
+	virtual Variant get_data() const { return Variant(); }
+
+	MotionShapeSW()  { configure(AABB()); }
+};
+
+
+
+
+struct _ShapeTestConvexBSPSW {
+
+	const BSP_Tree *bsp;
+	const ShapeSW *shape;
+	Transform transform;
+
+	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
+
+		shape->project_range(p_normal,transform,r_min,r_max);
+	}
+
+};
+
+
+
+
+#endif // SHAPESW_H
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index ad86a62280..da023e1144 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -32,7 +32,22 @@
 #include "physics_server_sw.h"
 
 
-bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+_FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {
+
+	if ((p_object->get_layer_mask()&p_layer_mask)==0)
+		return false;
+
+	if (p_object->get_type()==CollisionObjectSW::TYPE_AREA && !(p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA))
+		return false;
+
+	BodySW *body = static_cast<BodySW*>(p_object);
+
+	return (1<<body->get_mode())&p_type_mask;
+
+}
+
+
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
 
 
 	ERR_FAIL_COND_V(space->locked,false);
@@ -58,8 +73,8 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
 
 	for(int i=0;i<amount;i++) {
 
-		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
-			continue; //ignore area
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
 
 		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
 			continue;
@@ -114,7 +129,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
 }
 
 
-int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+int PhysicsDirectSpaceStateSW::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,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
 
 	if (p_result_max<=0)
 		return 0;
@@ -136,8 +151,9 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
 		if (cc>=p_result_max)
 			break;
 
-		if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
-			continue; //ignore area
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
 
 		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
 			continue;
@@ -146,7 +162,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
 		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
 		int shape_idx=space->intersection_query_subindex_results[i];
 
-		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL))
+		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL,p_margin,0))
 			continue;
 
 		r_results[cc].collider_id=col_obj->get_instance_id();
@@ -163,6 +179,283 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
 
 }
 
+
+bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {
+
+
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,false);
+
+	AABB aabb = p_xform.xform(shape->get_aabb());
+	aabb=aabb.merge(AABB(aabb.pos+p_motion,aabb.size)); //motion
+	aabb=aabb.grow(p_margin);
+
+	//if (p_motion!=Vector3())
+	//	print_line(p_motion);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	float best_safe=1;
+	float best_unsafe=1;
+
+	Transform xform_inv = p_xform.affine_inverse();
+	MotionShapeSW mshape;
+	mshape.shape=shape;
+	mshape.motion=xform_inv.basis.xform(p_motion);
+
+	bool best_first=true;
+
+	Vector3 closest_A,closest_B;
+
+	for(int i=0;i<amount;i++) {
+
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue; //ignore excluded
+
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		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,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+			//print_line("failed motion cast (no collision)");
+			continue;
+		}
+
+
+		//test initial overlap
+#if 0
+		if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {
+			print_line("failed initial cast (collision at begining)");
+			return false;
+		}
+#else
+		sep_axis=p_motion.normalized();
+
+		if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+			//print_line("failed motion cast (no collision)");
+			return false;
+		}
+#endif
+
+
+		//just do kinematic solving
+		float low=0;
+		float hi=1;
+		Vector3 mnormal=p_motion.normalized();
+
+		for(int i=0;i<8;i++) { //steps should be customizable..
+
+			Transform xfa = p_xform;
+			float ofs = (low+hi)*0.5;
+
+			Vector3 sep=mnormal; //important optimization for this to work fast enough
+
+			mshape.motion=xform_inv.basis.xform(p_motion*ofs);
+
+			Vector3 lA,lB;
+
+			bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,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_first=true; //force reset
+			best_safe=low;
+			best_unsafe=hi;
+		}
+
+		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {
+			closest_A=point_A;
+			closest_B=point_B;
+			r_info->collider_id=col_obj->get_instance_id();
+			r_info->rid=col_obj->get_self();
+			r_info->shape=shape_idx;
+			r_info->point=closest_B;
+			r_info->normal=(closest_A-closest_B).normalized();
+			best_first=false;
+			if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {
+				const BodySW *body=static_cast<const BodySW*>(col_obj);
+				r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
+			}
+
+		}
+
+
+	}
+
+	p_closest_safe=best_safe;
+	p_closest_unsafe=best_unsafe;	
+
+	return true;
+}
+
+bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask){
+
+	if (p_result_max<=0)
+		return 0;
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+
+	AABB aabb = p_shape_xform.xform(shape->get_aabb());
+	aabb=aabb.grow(p_margin);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	bool collided=false;
+	int cc=0;
+	r_result_count=0;
+
+	PhysicsServerSW::CollCbkData cbk;
+	cbk.max=p_result_max;
+	cbk.amount=0;
+	cbk.ptr=r_results;
+	CollisionSolverSW::CallbackResult cbkres=NULL;
+
+	PhysicsServerSW::CollCbkData *cbkptr=NULL;
+	if (p_result_max>0) {
+		cbkptr=&cbk;
+		cbkres=PhysicsServerSW::_shape_col_cbk;
+	}
+
+
+	for(int i=0;i<amount;i++) {
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (p_exclude.has( col_obj->get_self() )) {
+			continue;
+		}
+
+		//print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx));
+		//print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb()));
+
+		if (CollisionSolverSW::solve_static(shape,p_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=true;
+		}
+
+	}
+
+	r_result_count=cbk.amount;
+
+	return collided;
+
+}
+
+
+struct _RestCallbackData {
+
+	const CollisionObjectSW *object;
+	const CollisionObjectSW *best_object;
+	int shape;
+	int best_shape;
+	Vector3 best_contact;
+	Vector3 best_normal;
+	float best_len;
+};
+
+static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
+
+
+	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+
+	Vector3 contact_rel = p_point_B - p_point_A;
+	float len = contact_rel.length();
+	if (len <= rd->best_len)
+		return;
+
+	rd->best_len=len;
+	rd->best_contact=p_point_B;
+	rd->best_normal=contact_rel/len;
+	rd->best_object=rd->object;
+	rd->best_shape=rd->shape;
+
+}
+bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
+
+
+	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape,0);
+
+	AABB aabb = p_shape_xform.xform(shape->get_aabb());
+	aabb=aabb.grow(p_margin);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	_RestCallbackData rcd;
+	rcd.best_len=0;
+	rcd.best_object=NULL;
+	rcd.best_shape=0;
+
+	for(int i=0;i<amount;i++) {
+
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		if (p_exclude.has( col_obj->get_self() ))
+			continue;
+
+		rcd.object=col_obj;
+		rcd.shape=shape_idx;
+		bool sc = CollisionSolverSW::solve_static(shape,p_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)
+		return false;
+
+	r_info->collider_id=rcd.best_object->get_instance_id();
+	r_info->shape=rcd.best_shape;
+	r_info->normal=rcd.best_normal;
+	r_info->point=rcd.best_contact;
+	r_info->rid=rcd.best_object->get_self();
+	if (rcd.best_object->get_type()==CollisionObjectSW::TYPE_BODY) {
+
+		const BodySW *body = static_cast<const BodySW*>(rcd.best_object);
+		Vector3 rel_vec = r_info->point-body->get_transform().get_origin();
+		r_info->linear_velocity = body->get_linear_velocity() +
+				(body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);
+
+
+	} else {
+		r_info->linear_velocity=Vector3();
+	}
+
+	return true;
+}
+
+
 PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
 
@@ -194,6 +487,8 @@ void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionO
 
 	SpaceSW *self = (SpaceSW*)p_self;
 
+	self->collision_pairs++;
+
 	if (type_A==CollisionObjectSW::TYPE_AREA) {
 
 
@@ -221,6 +516,7 @@ void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,Collision
 
 
 	SpaceSW *self = (SpaceSW*)p_self;
+	self->collision_pairs--;
 	ConstraintSW *c = (ConstraintSW*)p_data;
 	memdelete(c);
 }
@@ -398,6 +694,9 @@ PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {
 
 SpaceSW::SpaceSW() {
 
+	collision_pairs=0;
+	active_objects=0;
+	island_count=0;
 
 	locked=false;
 	contact_recycle_radius=0.01;
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h
index cec1053400..a97647dcfc 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -46,8 +46,11 @@ public:
 
 	SpaceSW *space;
 
-	bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
-	int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	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_layer_mask=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,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=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,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 
 	PhysicsDirectSpaceStateSW();
 };
@@ -94,6 +97,10 @@ class SpaceSW {
 
 	bool locked;
 
+	int island_count;
+	int active_objects;
+	int collision_pairs;
+
 friend class PhysicsDirectSpaceStateSW;
 
 public:
@@ -147,6 +154,14 @@ public:
 	void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
 	real_t get_param(PhysicsServer::SpaceParameter p_param) const;
 
+	void set_island_count(int p_island_count) { island_count=p_island_count; }
+	int get_island_count() const { return island_count; }
+
+	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+	int get_active_objects() const { return active_objects; }
+
+	int get_collision_pairs() const { return collision_pairs; }
+
 	PhysicsDirectSpaceStateSW *get_direct_state();
 
 	SpaceSW();
diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp
index b7815d2250..6d95804875 100644
--- a/servers/physics/step_sw.cpp
+++ b/servers/physics/step_sw.cpp
@@ -49,7 +49,7 @@ void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_
 			if (i==E->get())
 				continue;
 			BodySW *b = c->get_body_ptr()[i];
-			if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+			if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC)
 				continue; //no go
 			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
 		}
@@ -86,8 +86,10 @@ void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
 	BodySW *b = p_island;
 	while(b) {
 
-		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) {
+			b=b->get_island_next();
 			continue; //ignore for static
+		}
 
 		if (!b->sleep_test(p_delta))
 			can_sleep=false;
@@ -100,8 +102,10 @@ void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
 	b = p_island;
 	while(b) {
 
-		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC)
+		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) {
+			b=b->get_island_next();
 			continue; //ignore for static
+		}
 
 		bool active = b->is_active();
 
@@ -131,6 +135,7 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
 		active_count++;
 	}
 
+	p_space->set_active_objects(active_count);
 
 
 	/* GENERATE CONSTRAINT ISLANDS */
@@ -164,6 +169,8 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
 		b=b->next();
 	}
 
+	p_space->set_island_count(island_count);
+
 	const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list();
 
 	while(aml.first()) {
@@ -207,9 +214,9 @@ void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
 
 	b = body_list->first();
 	while(b) {
-
+		const SelfList<BodySW>*n=b->next();
 		b->self()->integrate_velocities(p_delta);
-		b=b->next();
+		b=n;
 	}
 
 	/* SLEEP / WAKE UP ISLANDS */
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index 51e6ccd166..0eda1050fa 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/servers/physics_2d/area_2d_sw.h
@@ -71,7 +71,7 @@ class Area2DSW : public CollisionObject2DSW{
 
 					return area_shape < p_key.area_shape;
 				} else
-					return body_shape < p_key.area_shape;
+					return body_shape < p_key.body_shape;
 			} else
 				return rid < p_key.rid;
 
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index 7d85183645..f73ed5732e 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -593,8 +593,8 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr
 	//this collision is kind of pointless
 
 
-	if (!separator.test_previous_axis())
-		return;
+	//if (!separator.test_previous_axis())
+	//	return;
 
 	if (!separator.test_cast())
 		return;
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 2171a9c2c4..09fa3f9b6a 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -1086,9 +1086,15 @@ void Physics2DServerSW::step(float p_step) {
 
 	last_step=p_step;
 	Physics2DDirectBodyStateSW::singleton->step=p_step;
+	island_count=0;
+	active_objects=0;
+	collision_pairs=0;
 	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
 
 		stepper->step((Space2DSW*)E->get(),p_step,iterations);
+		island_count+=E->get()->get_island_count();
+		active_objects+=E->get()->get_active_objects();
+		collision_pairs+=E->get()->get_collision_pairs();
 	}
 };
 
@@ -1118,6 +1124,27 @@ void Physics2DServerSW::finish() {
 	memdelete(direct_state);
 };
 
+int Physics2DServerSW::get_process_info(ProcessInfo p_info) {
+
+	switch(p_info) {
+
+		case INFO_ACTIVE_OBJECTS: {
+
+			return active_objects;
+		} break;
+		case INFO_COLLISION_PAIRS: {
+			return collision_pairs;
+		} break;
+		case INFO_ISLAND_COUNT: {
+
+			return island_count;
+		} break;
+
+	}
+
+	return 0;
+}
+
 
 Physics2DServerSW::Physics2DServerSW() {
 
@@ -1125,8 +1152,13 @@ Physics2DServerSW::Physics2DServerSW() {
 //	BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
 
 	active=true;
+	island_count=0;
+	active_objects=0;
+	collision_pairs=0;
+
 };
 
+
 Physics2DServerSW::~Physics2DServerSW() {
 
 };
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 09ca029127..7ffffe669f 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -47,6 +47,11 @@ friend class Physics2DDirectSpaceStateSW;
 	bool doing_sync;
 	real_t last_step;
 
+	int island_count;
+	int active_objects;
+	int collision_pairs;
+
+
 	Step2DSW *stepper;
 	Set<const Space2DSW*> active_spaces;
 
@@ -223,6 +228,8 @@ public:
 	virtual void flush_queries();
 	virtual void finish();
 
+	int get_process_info(ProcessInfo p_info);
+
 	Physics2DServerSW();
 	~Physics2DServerSW();
 
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index d3fcf1fab2..8500a6194f 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -582,5 +582,6 @@ public:
 
 };
 
+#undef DEFAULT_PROJECT_RANGE_CAST
 
 #endif // SHAPE_2D_2DSW_H
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 5fbf828c38..21a99cd4b2 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -323,7 +323,7 @@ bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_s
 }
 
 
-struct _RestCallbackData {
+struct _RestCallbackData2D {
 
 	const CollisionObject2DSW *object;
 	const CollisionObject2DSW *best_object;
@@ -337,7 +337,7 @@ struct _RestCallbackData {
 static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
 
 
-	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+	_RestCallbackData2D *rd=(_RestCallbackData2D*)p_userdata;
 
 	Vector2 contact_rel = p_point_B - p_point_A;
 	float len = contact_rel.length();
@@ -365,7 +365,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape
 
 	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
 
-	_RestCallbackData rcd;
+	_RestCallbackData2D rcd;
 	rcd.best_len=0;
 	rcd.best_object=NULL;
 	rcd.best_shape=0;
@@ -443,6 +443,7 @@ void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,Collis
 	}
 
 	Space2DSW *self = (Space2DSW*)p_self;
+	self->collision_pairs++;
 
 	if (type_A==CollisionObject2DSW::TYPE_AREA) {
 
@@ -468,8 +469,8 @@ void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,Collis
 void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {
 
 
-
 	Space2DSW *self = (Space2DSW*)p_self;
+	self->collision_pairs--;
 	Constraint2DSW *c = (Constraint2DSW*)p_data;
 	memdelete(c);
 }
@@ -646,6 +647,10 @@ Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {
 Space2DSW::Space2DSW() {
 
 
+	collision_pairs=0;
+	active_objects=0;
+	island_count=0;
+
 	locked=false;
 	contact_recycle_radius=0.01;
 	contact_max_separation=0.05;
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index bd41097fba..c638a0c45b 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -97,6 +97,10 @@ class Space2DSW {
 
 	bool locked;
 
+	int island_count;
+	int active_objects;
+	int collision_pairs;
+
 friend class Physics2DDirectSpaceStateSW;
 
 public:
@@ -153,6 +157,14 @@ public:
 	void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
 	real_t get_param(Physics2DServer::SpaceParameter p_param) const;
 
+	void set_island_count(int p_island_count) { island_count=p_island_count; }
+	int get_island_count() const { return island_count; }
+
+	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+	int get_active_objects() const { return active_objects; }
+
+	int get_collision_pairs() const { return collision_pairs; }
+
 	Physics2DDirectSpaceStateSW *get_direct_state();
 
 	Space2DSW();
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 29f4a58287..e75f9300ce 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -137,6 +137,8 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
 		active_count++;
 	}
 
+	p_space->set_active_objects(active_count);
+
 	/* GENERATE CONSTRAINT ISLANDS */
 
 	Body2DSW *island_list=NULL;
@@ -168,6 +170,8 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
 		b=b->next();
 	}
 
+	p_space->set_island_count(island_count);
+
 	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
 
 
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
index 9cbd7414bd..da8ac5f9c8 100644
--- a/servers/physics_2d_server.cpp
+++ b/servers/physics_2d_server.cpp
@@ -110,17 +110,132 @@ Physics2DDirectBodyState::Physics2DDirectBodyState() {}
 
 
 
-Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
+void Physics2DShapeQueryParameters::set_shape(const RES &p_shape) {
+
+	ERR_FAIL_COND(p_shape.is_null());
+	shape=p_shape->get_rid();
+}
+
+void Physics2DShapeQueryParameters::set_shape_rid(const RID& p_shape) {
+
+	shape=p_shape;
+}
+
+RID Physics2DShapeQueryParameters::get_shape_rid() const {
+
+	return shape;
+}
+
+void Physics2DShapeQueryParameters::set_transform(const Matrix32& p_transform){
+
+	transform=p_transform;
+}
+Matrix32 Physics2DShapeQueryParameters::get_transform() const{
+
+	return transform;
+}
+
+void Physics2DShapeQueryParameters::set_motion(const Vector2& p_motion){
+
+	motion=p_motion;
+}
+Vector2 Physics2DShapeQueryParameters::get_motion() const{
+
+	return motion;
+}
+
+void Physics2DShapeQueryParameters::set_margin(float p_margin){
+
+	margin=p_margin;
+}
+float Physics2DShapeQueryParameters::get_margin() const{
+
+	return margin;
+}
+
+void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask){
+
+	layer_mask=p_layer_mask;
+}
+int Physics2DShapeQueryParameters::get_layer_mask() const{
+
+	return layer_mask;
+}
+
+
+void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask){
+
+	object_type_mask=p_object_type_mask;
+}
+int Physics2DShapeQueryParameters::get_object_type_mask() const{
+
+	return object_type_mask;
+}
+void Physics2DShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) {
+
+	exclude.clear();;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+}
+
+Vector<RID> Physics2DShapeQueryParameters::get_exclude() const{
+
+	Vector<RID> ret;
+	ret.resize(exclude.size());
+	int idx=0;
+	for(Set<RID>::Element *E=exclude.front();E;E=E->next()) {
+		ret[idx]=E->get();
+	}
+	return ret;
+}
+
+void Physics2DShapeQueryParameters::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape2D"),&Physics2DShapeQueryParameters::set_shape);
+	ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&Physics2DShapeQueryParameters::set_shape_rid);
+	ObjectTypeDB::bind_method(_MD("get_shape_rid"),&Physics2DShapeQueryParameters::get_shape_rid);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DShapeQueryParameters::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DShapeQueryParameters::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("set_motion","motion"),&Physics2DShapeQueryParameters::set_motion);
+	ObjectTypeDB::bind_method(_MD("get_motion"),&Physics2DShapeQueryParameters::get_motion);
+
+	ObjectTypeDB::bind_method(_MD("set_margin","margin"),&Physics2DShapeQueryParameters::set_margin);
+	ObjectTypeDB::bind_method(_MD("get_margin"),&Physics2DShapeQueryParameters::get_margin);
+
+	ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&Physics2DShapeQueryParameters::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&Physics2DShapeQueryParameters::get_layer_mask);
+
+	ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&Physics2DShapeQueryParameters::set_object_type_mask);
+	ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&Physics2DShapeQueryParameters::get_object_type_mask);
+
+	ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&Physics2DShapeQueryParameters::set_exclude);
+	ObjectTypeDB::bind_method(_MD("get_exclude"),&Physics2DShapeQueryParameters::get_exclude);
+
+
+}
+
+Physics2DShapeQueryParameters::Physics2DShapeQueryParameters() {
+
+	margin=0;
+	layer_mask=0x7FFFFFFF;
+	object_type_mask=Physics2DDirectSpaceState::TYPE_MASK_COLLISION;
+}
+
+
+Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) {
 
 	RayResult inters;
 	Set<RID> exclude;
 	for(int i=0;i<p_exclude.size();i++)
 		exclude.insert(p_exclude[i]);
 
-	bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
+	bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask);
 
 	if (!res)
-		return Variant();
+		return Dictionary(true);
 
 	Dictionary d(true);
 	d["position"]=inters.position;
@@ -133,59 +248,71 @@ Variant Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const V
 	return d;
 }
 
-Variant Physics2DDirectSpaceState::_intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
-
-	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
-	if (p_result_max<=0)
-		return Variant();
-
-	Set<RID> exclude;
-	for(int i=0;i<p_exclude.size();i++)
-		exclude.insert(p_exclude[i]);
-
-	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
-
-	int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask);
+Array Physics2DDirectSpaceState::_intersect_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results) {
 
-	if (rc==0)
-		return Variant();
+	Vector<ShapeResult> sr;
+	sr.resize(p_max_results);
+	int rc = intersect_shape(psq->shape,psq->transform,psq->motion,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask);
+	Array ret;
+	ret.resize(rc);
+	for(int i=0;i<rc;i++) {
 
-	Ref<Physics2DShapeQueryResult>  result = memnew( Physics2DShapeQueryResult );
-	result->result.resize(rc);
-	for(int i=0;i<rc;i++)
-		result->result[i]=res[i];
-
-	return result;
+		Dictionary d;
+		d["rid"]=sr[i].rid;
+		d["collider_id"]=sr[i].collider_id;
+		d["collider"]=sr[i].collider;
+		d["shape"]=sr[i].shape;
+		ret[i]=d;
+	}
 
+	return ret;
 }
 
+Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq){
 
-Variant Physics2DDirectSpaceState::_cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
-
+	float closest_safe,closest_unsafe;
+	bool res = cast_motion(psq->shape,psq->transform,psq->motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	if (!res)
+		return Array();
+	Array ret(true);
+	ret.resize(2);
+	ret[0]=closest_safe;
+	ret[0]=closest_unsafe;
+	return ret;
 
-#if 0
-	Set<RID> exclude;
-	for(int i=0;i<p_exclude.size();i++)
-		exclude.insert(p_exclude[i]);
+}
+Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results){
 
+	Vector<Vector2> ret;
+	ret.resize(p_max_results*2);
+	int rc=0;
+	bool res = collide_shape(psq->shape,psq->transform,psq->motion,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	if (!res)
+		return Array();
+	Array r;
+	r.resize(rc*2);
+	for(int i=0;i<rc*2;i++)
+		r[i]=ret[i];
+	return r;
 
-	bool result = cast_motion(p_shape,p_xform,p_motion,0,mcc,exclude,p_user_mask);
+}
+Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq){
 
-	if (!result)
-		return Variant();
+	ShapeRestInfo sri;
 
-	Dictionary d(true);
-	d["point"]=mcc.point;
-	d["normal"]=mcc.normal;
-	d["rid"]=mcc.rid;
-	d["collider_id"]=mcc.collider_id;
-	d["collider"]=mcc.collider;
-	d["shape"]=mcc.shape;
+	bool res = rest_info(psq->shape,psq->transform,psq->motion,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	Dictionary r(true);
+	if (!res)
+		return r;
 
-	return d;
-#endif
-	return Variant();
+	r["point"]=sri.point;
+	r["normal"]=sri.normal;
+	r["rid"]=sri.rid;
+	r["collider_id"]=sri.collider_id;
+	r["shape"]=sri.shape;
+	r["linear_velocity"]=sri.linear_velocity;
 
+	return r;
 }
 
 
@@ -200,9 +327,19 @@ Physics2DDirectSpaceState::Physics2DDirectSpaceState() {
 void Physics2DDirectSpaceState::_bind_methods() {
 
 
-	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","umask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
-	ObjectTypeDB::bind_method(_MD("intersect_shape:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
-	ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION));
+	ObjectTypeDB::bind_method(_MD("intersect_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("cast_motion","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_cast_motion);
+	ObjectTypeDB::bind_method(_MD("collide_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_collide_shape,DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("get_rest_info","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_get_rest_info);
+	//ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+
+	BIND_CONSTANT( TYPE_MASK_STATIC_BODY );
+	BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY );
+	BIND_CONSTANT( TYPE_MASK_RIGID_BODY );
+	BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY );
+	BIND_CONSTANT( TYPE_MASK_AREA );
+	BIND_CONSTANT( TYPE_MASK_COLLISION );
 
 }
 
@@ -375,6 +512,8 @@ void Physics2DServer::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_active","active"),&Physics2DServer::set_active);
 
+	ObjectTypeDB::bind_method(_MD("get_process_info"),&Physics2DServer::get_process_info);
+
 //	ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init);
 //	ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step);
 //	ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync);
@@ -434,6 +573,10 @@ void Physics2DServer::_bind_methods() {
 	BIND_CONSTANT( AREA_BODY_ADDED );
 	BIND_CONSTANT( AREA_BODY_REMOVED );
 
+	BIND_CONSTANT( INFO_ACTIVE_OBJECTS );
+	BIND_CONSTANT( INFO_COLLISION_PAIRS );
+	BIND_CONSTANT( INFO_ISLAND_COUNT );
+
 
 }
 
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index def1e69992..17a21e46a3 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -31,6 +31,7 @@
 
 #include "object.h"
 #include "reference.h"
+#include "resource.h"
 
 class Physics2DDirectSpaceState;
 
@@ -84,14 +85,60 @@ public:
 class Physics2DShapeQueryResult;
 
 
+//used for script
+class Physics2DShapeQueryParameters : public Reference {
+
+	OBJ_TYPE(Physics2DShapeQueryParameters, Reference);
+friend class Physics2DDirectSpaceState;
+	RID shape;
+	Matrix32 transform;
+	Vector2 motion;
+	float margin;
+	Set<RID> exclude;
+	uint32_t layer_mask;
+	uint32_t object_type_mask;
+protected:
+	static void _bind_methods();
+public:
+
+
+	void set_shape(const RES& p_shape);
+	void set_shape_rid(const RID& p_shape);
+	RID get_shape_rid() const;
+
+	void set_transform(const Matrix32& p_transform);
+	Matrix32 get_transform() const;
+
+	void set_motion(const Vector2& p_motion);
+	Vector2 get_motion() const;
+
+	void set_margin(float p_margin);
+	float get_margin() const;
+
+	void set_layer_mask(int p_layer_mask);
+	int get_layer_mask() const;
+
+	void set_object_type_mask(int p_object_type_mask);
+	int get_object_type_mask() const;
+
+	void set_exclude(const Vector<RID>& p_exclude);
+	Vector<RID> get_exclude() const;
+
+	Physics2DShapeQueryParameters();
+
+};
+
+
 class Physics2DDirectSpaceState : public Object {
 
 	OBJ_TYPE( Physics2DDirectSpaceState, Object );
 
-	Variant _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0);
-	Variant _intersect_shape(const RID& p_shape, const Matrix32& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0);
-	Variant _cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0);
+	Dictionary _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 
+	Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32);
+	Array _cast_motion(const Ref<Physics2DShapeQueryParameters> &p_shape_query);
+	Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32);
+	Dictionary _get_rest_info(const Ref<Physics2DShapeQueryParameters> &p_shape_query);
 
 protected:
 	static void _bind_methods();
@@ -131,8 +178,6 @@ public:
 
 	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
-
-
 	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
 	virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
@@ -448,6 +493,15 @@ public:
 	virtual void flush_queries()=0;
 	virtual void finish()=0;
 
+	enum ProcessInfo {
+
+		INFO_ACTIVE_OBJECTS,
+		INFO_COLLISION_PAIRS,
+		INFO_ISLAND_COUNT
+	};
+
+	virtual int get_process_info(ProcessInfo p_info)=0;
+
 	Physics2DServer();
 	~Physics2DServer();
 };
@@ -465,5 +519,6 @@ VARIANT_ENUM_CAST( Physics2DServer::JointType );
 VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );
 //VARIANT_ENUM_CAST( Physics2DServer::ObjectType );
 VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus );
+VARIANT_ENUM_CAST( Physics2DServer::ProcessInfo );
 
 #endif
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 070bc5e062..e2dd3e14eb 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -113,6 +113,112 @@ PhysicsDirectBodyState::PhysicsDirectBodyState() {}
 
 
 
+void PhysicsShapeQueryParameters::set_shape(const RES &p_shape) {
+
+	ERR_FAIL_COND(p_shape.is_null());
+	shape=p_shape->get_rid();
+}
+
+void PhysicsShapeQueryParameters::set_shape_rid(const RID& p_shape) {
+
+	shape=p_shape;
+}
+
+RID PhysicsShapeQueryParameters::get_shape_rid() const {
+
+	return shape;
+}
+
+void PhysicsShapeQueryParameters::set_transform(const Matrix32& p_transform){
+
+	transform=p_transform;
+}
+Matrix32 PhysicsShapeQueryParameters::get_transform() const{
+
+	return transform;
+}
+
+void PhysicsShapeQueryParameters::set_margin(float p_margin){
+
+	margin=p_margin;
+}
+
+float PhysicsShapeQueryParameters::get_margin() const{
+
+	return margin;
+}
+
+void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask){
+
+	layer_mask=p_layer_mask;
+}
+int PhysicsShapeQueryParameters::get_layer_mask() const{
+
+	return layer_mask;
+}
+
+
+void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask){
+
+	object_type_mask=p_object_type_mask;
+}
+int PhysicsShapeQueryParameters::get_object_type_mask() const{
+
+	return object_type_mask;
+}
+void PhysicsShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) {
+
+	exclude.clear();;
+	for(int i=0;i<p_exclude.size();i++)
+		exclude.insert(p_exclude[i]);
+
+}
+
+Vector<RID> PhysicsShapeQueryParameters::get_exclude() const{
+
+	Vector<RID> ret;
+	ret.resize(exclude.size());
+	int idx=0;
+	for(Set<RID>::Element *E=exclude.front();E;E=E->next()) {
+		ret[idx]=E->get();
+	}
+	return ret;
+}
+
+void PhysicsShapeQueryParameters::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape"),&PhysicsShapeQueryParameters::set_shape);
+	ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&PhysicsShapeQueryParameters::set_shape_rid);
+	ObjectTypeDB::bind_method(_MD("get_shape_rid"),&PhysicsShapeQueryParameters::get_shape_rid);
+
+	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsShapeQueryParameters::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsShapeQueryParameters::get_transform);
+
+	ObjectTypeDB::bind_method(_MD("set_margin","margin"),&PhysicsShapeQueryParameters::set_margin);
+	ObjectTypeDB::bind_method(_MD("get_margin"),&PhysicsShapeQueryParameters::get_margin);
+
+	ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&PhysicsShapeQueryParameters::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsShapeQueryParameters::get_layer_mask);
+
+	ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&PhysicsShapeQueryParameters::set_object_type_mask);
+	ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&PhysicsShapeQueryParameters::get_object_type_mask);
+
+	ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&PhysicsShapeQueryParameters::set_exclude);
+	ObjectTypeDB::bind_method(_MD("get_exclude"),&PhysicsShapeQueryParameters::get_exclude);
+
+
+}
+
+PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() {
+
+	margin=0;
+	layer_mask=0x7FFFFFFF;
+	object_type_mask=PhysicsDirectSpaceState::TYPE_MASK_COLLISION;
+}
+
+
+
+/////////////////////////////////////
 Variant PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
 
 	RayResult inters;
@@ -150,7 +256,7 @@ Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Tran
 
 	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
 
-	int rc = intersect_shape(p_shape,p_xform,res,p_result_max,exclude,p_user_mask);
+	int rc = intersect_shape(p_shape,p_xform,0,res,p_result_max,exclude);
 
 	if (rc==0)
 		return Variant();
@@ -308,8 +414,6 @@ void PhysicsServer::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&PhysicsServer::body_set_param);
 	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&PhysicsServer::body_get_param);
 
-	ObjectTypeDB::bind_method(_MD("body_static_simulate_motion","body","new_xform"),&PhysicsServer::body_static_simulate_motion);
-
 	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&PhysicsServer::body_set_state);
 	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state);
 
@@ -355,6 +459,8 @@ void PhysicsServer::_bind_methods() {
 	//ObjectTypeDB::bind_method(_MD("flush_queries"),&PhysicsServer::flush_queries);
 
 
+	ObjectTypeDB::bind_method(_MD("get_process_info"),&PhysicsServer::get_process_info);
+
 	BIND_CONSTANT( SHAPE_PLANE );
 	BIND_CONSTANT( SHAPE_RAY );
 	BIND_CONSTANT( SHAPE_SPHERE );
@@ -407,6 +513,11 @@ void PhysicsServer::_bind_methods() {
 	BIND_CONSTANT( AREA_BODY_ADDED );
 	BIND_CONSTANT( AREA_BODY_REMOVED );
 
+	BIND_CONSTANT( INFO_ACTIVE_OBJECTS );
+	BIND_CONSTANT( INFO_COLLISION_PAIRS );
+	BIND_CONSTANT( INFO_ISLAND_COUNT );
+
+
 
 }
 
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 955caffe5b..5709974cc0 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -30,7 +30,7 @@
 #define PHYSICS_SERVER_H
 
 #include "object.h"
-#include "reference.h"
+#include "resource.h"
 
 class PhysicsDirectSpaceState;
 
@@ -87,6 +87,45 @@ public:
 
 class PhysicsShapeQueryResult;
 
+class PhysicsShapeQueryParameters : public Reference {
+
+	OBJ_TYPE(PhysicsShapeQueryParameters, Reference);
+friend class PhysicsDirectSpaceState;
+	RID shape;
+	Matrix32 transform;
+	float margin;
+	Set<RID> exclude;
+	uint32_t layer_mask;
+	uint32_t object_type_mask;
+protected:
+	static void _bind_methods();
+public:
+
+
+	void set_shape(const RES& p_shape);
+	void set_shape_rid(const RID& p_shape);
+	RID get_shape_rid() const;
+
+	void set_transform(const Matrix32& p_transform);
+	Matrix32 get_transform() const;
+
+	void set_margin(float p_margin);
+	float get_margin() const;
+
+	void set_layer_mask(int p_layer_mask);
+	int get_layer_mask() const;
+
+	void set_object_type_mask(int p_object_type_mask);
+	int get_object_type_mask() const;
+
+	void set_exclude(const Vector<RID>& p_exclude);
+	Vector<RID> get_exclude() const;
+
+	PhysicsShapeQueryParameters();
+
+};
+
+
 
 class PhysicsDirectSpaceState : public Object {
 
@@ -101,6 +140,15 @@ protected:
 
 public:
 
+	enum ObjectTypeMask {
+		TYPE_MASK_STATIC_BODY=1<<0,
+		TYPE_MASK_KINEMATIC_BODY=1<<1,
+		TYPE_MASK_RIGID_BODY=1<<2,
+		TYPE_MASK_CHARACTER_BODY=1<<3,
+		TYPE_MASK_AREA=1<<4,
+		TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
+	};
+
 	struct RayResult {
 
 		Vector3 position;
@@ -111,7 +159,7 @@ public:
 		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_user_mask=0)=0;
+	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
 
 	struct ShapeResult {
 
@@ -122,7 +170,25 @@ public:
 
 	};
 
-	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0)=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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+
+	struct ShapeRestInfo {
+
+		Vector3 point;
+		Vector3 normal;
+		RID rid;
+		ObjectID collider_id;
+		int shape;
+		Vector3 linear_velocity; //velocity at contact point
+
+	};
+
+	virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL)=0;
+
+	virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+
+	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_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+
 
 	PhysicsDirectSpaceState();
 };
@@ -303,6 +369,9 @@ public:
 	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
 	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
 
+	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0;
+	virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const=0;
+
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
 	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
 
@@ -317,8 +386,6 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
 
-	//advanced simulation
-	virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform)=0;
 
 	//state
 	enum BodyState {
@@ -420,6 +487,15 @@ public:
 	virtual void flush_queries()=0;
 	virtual void finish()=0;
 
+	enum ProcessInfo {
+
+		INFO_ACTIVE_OBJECTS,
+		INFO_COLLISION_PAIRS,
+		INFO_ISLAND_COUNT
+	};
+
+	virtual int get_process_info(ProcessInfo p_info)=0;
+
 	PhysicsServer();
 	~PhysicsServer();
 };
@@ -437,5 +513,6 @@ VARIANT_ENUM_CAST( PhysicsServer::BodyAxisLock );
 //VARIANT_ENUM_CAST( PhysicsServer::DampedStringParam );
 //VARIANT_ENUM_CAST( PhysicsServer::ObjectType );
 VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus );
+VARIANT_ENUM_CAST( PhysicsServer::ProcessInfo );
 
 #endif
diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp
index 638156d813..f8d38d15c0 100644
--- a/servers/register_server_types.cpp
+++ b/servers/register_server_types.cpp
@@ -55,6 +55,7 @@ void register_server_types() {
 	ObjectTypeDB::register_virtual_type<Physics2DDirectBodyState>();
 	ObjectTypeDB::register_virtual_type<Physics2DDirectSpaceState>();
 	ObjectTypeDB::register_virtual_type<Physics2DShapeQueryResult>();
+	ObjectTypeDB::register_type<Physics2DShapeQueryParameters>();
 
 	ObjectTypeDB::register_virtual_type<PhysicsDirectBodyState>();
 	ObjectTypeDB::register_virtual_type<PhysicsDirectSpaceState>();
diff --git a/tools/collada/collada.cpp b/tools/collada/collada.cpp
index 9962eed1b2..c416286c19 100644
--- a/tools/collada/collada.cpp
+++ b/tools/collada/collada.cpp
@@ -1388,8 +1388,11 @@ void Collada::_parse_morph_controller(XMLParser& parser, String p_id) {
 	state.morph_controller_data_map[p_id]=MorphControllerData();
 	MorphControllerData &morphdata = state.morph_controller_data_map[p_id];
 
+	print_line("morph source: "+parser.get_attribute_value("source")+" id: "+p_id);
 	morphdata.mesh=_uri_to_id(parser.get_attribute_value("source"));
+	print_line("morph source2: "+morphdata.mesh);
 	morphdata.mode=parser.get_attribute_value("method");
+	printf("JJmorph: %p\n",&morphdata);
 	String current_source;
 
 	while(parser.read()==OK) {
diff --git a/tools/editor/icons/icon_instance_options.png b/tools/editor/icons/icon_instance_options.png
new file mode 100644
index 0000000000..2d3e98b2ea
--- /dev/null
+++ b/tools/editor/icons/icon_instance_options.png
diff --git a/tools/editor/io_plugins/editor_import_collada.cpp b/tools/editor/io_plugins/editor_import_collada.cpp
index 15a671d623..2dce3fa8f5 100644
--- a/tools/editor/io_plugins/editor_import_collada.cpp
+++ b/tools/editor/io_plugins/editor_import_collada.cpp
@@ -564,6 +564,7 @@ Error ColladaImport::_create_mesh_surfaces(Ref<Mesh>& p_mesh,const Map<String,Co
 	bool local_xform_mirror=p_local_xform.basis.determinant() < 0;
 
 	if (p_morph_data) {
+
 		//add morphie target
 		ERR_FAIL_COND_V( !p_morph_data->targets.has("MORPH_TARGET"), ERR_INVALID_DATA );
 		String mt = p_morph_data->targets["MORPH_TARGET"];
@@ -1478,8 +1479,11 @@ Error ColladaImport::_create_resources(Collada::Node *p_node) {
 			Transform apply_xform;
 			Vector<int> bone_remap;
 
+			print_line("mesh: "+String(mi->get_name()));
+
 			if (ng->controller) {
 
+				print_line("has controller");
 				if (collada.state.skin_controller_data_map.has(ng->source)) {
 
 
@@ -1528,9 +1532,12 @@ Error ColladaImport::_create_resources(Collada::Node *p_node) {
 						bone_remap[i]=bone_remap_map[str];
 					}
 				} else if (collada.state.morph_controller_data_map.has(ng->source)) {
+					print_line("is morph "+ng->source);
 					//it's a morph!!
-					morph = &collada.state.morph_controller_data_map[meshid];
+					morph = &collada.state.morph_controller_data_map[ng->source];
 					meshid=morph->mesh;
+					printf("KKmorph: %p\n",morph);
+					print_line("morph mshid: "+meshid);
 				} else {
 					ERR_EXPLAIN("Controller Instance Source '"+ng->source+"' is neither skin or morph!");
 					ERR_FAIL_V( ERR_INVALID_DATA );
diff --git a/tools/editor/plugins/baked_light_baker.cpp b/tools/editor/plugins/baked_light_baker.cpp
index 1574ce81d7..dea83e0ff8 100644
--- a/tools/editor/plugins/baked_light_baker.cpp
+++ b/tools/editor/plugins/baked_light_baker.cpp
@@ -338,7 +338,7 @@ void BakedLightBaker::_fix_lights() {
 				}
 				if (dl.type==VS::LIGHT_OMNI) {
 
-					dl.area=4.0*Math_PI*pow(dl.radius,2.0);
+					dl.area=4.0*Math_PI*pow(dl.radius,2.0f);
 					dl.constant=1.0/3.5;
 				} else {
 
diff --git a/tools/editor/property_editor.cpp b/tools/editor/property_editor.cpp
index 4ac2ff0594..645d967a4b 100644
--- a/tools/editor/property_editor.cpp
+++ b/tools/editor/property_editor.cpp
@@ -38,6 +38,7 @@
 #include "scene/scene_string_names.h"
 #include "editor_settings.h"
 #include "editor_import_export.h"
+#include "editor_node.h"
 
 void CustomPropertyEditor::_notification(int p_what) {
 	
@@ -1619,6 +1620,7 @@ CustomPropertyEditor::CustomPropertyEditor() {
 	scene_tree = memnew( SceneTreeDialog );
 	add_child(scene_tree);
 	scene_tree->connect("selected", this,"_node_path_selected");
+	scene_tree->get_tree()->set_show_enabled_subscene(true);
 
 	texture_preview = memnew( TextureFrame );
 	add_child( texture_preview);
@@ -2037,6 +2039,17 @@ void PropertyEditor::update_tree() {
 	List<PropertyInfo> plist;
 	obj->get_property_list(&plist,true);
 
+	bool draw_red=false;
+
+	{
+		Node *nod = obj->cast_to<Node>();
+		Node *es = EditorNode::get_singleton()->get_edited_scene();
+		if (nod && es!=nod && nod->get_owner()!=es) {
+			draw_red=true;
+		}
+	}
+
+
 	Color sscolor=get_color("prop_subsection","Editor");
 
 	TreeItem * current_category=NULL;
@@ -2141,11 +2154,16 @@ void PropertyEditor::update_tree() {
 					
 		item->set_metadata( 0, d );
 		item->set_metadata( 1, p.name );
+
+		if (draw_red)
+			item->set_custom_color(0,Color(0.8,0.4,0.20));
+
 		
 		if (p.name==selected_property) {
 
 			item->select(1);
 		}
+
 		
 		//printf("property %s type %i\n",p.name.ascii().get_data(),p.type);
 		switch( p.type ) {
diff --git a/tools/editor/scene_tree_dock.cpp b/tools/editor/scene_tree_dock.cpp
index c4c3a10f74..e7f4beb46e 100644
--- a/tools/editor/scene_tree_dock.cpp
+++ b/tools/editor/scene_tree_dock.cpp
@@ -108,6 +108,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 
 		case TOOL_NEW: {
 
+
+			if (!_validate_no_foreign())
+				break;
 			create_dialog->popup_centered_ratio();
 		} break;
 		case TOOL_INSTANCE: {
@@ -124,6 +127,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 				break;
 			}
 
+			if (!_validate_no_foreign())
+				break;
+
 			file->set_mode(FileDialog::MODE_OPEN_FILE);
 			List<String> extensions;
 			ResourceLoader::get_recognized_extensions_for_type("PackedScene",&extensions);
@@ -147,6 +153,8 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (!current)
 				break;
 
+			if (!_validate_no_foreign())
+				break;
 			connect_dialog->popup_centered_ratio();
 			connect_dialog->set_node(current);
 
@@ -156,6 +164,8 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			Node *current = scene_tree->get_selected();
 			if (!current)
 				break;
+			if (!_validate_no_foreign())
+				break;
 			groups_editor->set_current(current);
 			groups_editor->popup_centered_ratio();
 		} break;
@@ -165,6 +175,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (!selected)
 				break;
 
+			if (!_validate_no_foreign())
+				break;
+
 			Ref<Script> existing = selected->get_script();
 			if (existing.is_valid())
 				editor->push_item(existing.ptr());
@@ -183,6 +196,7 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (!scene_tree->get_selected())
 				break;
 
+
 			if (scene_tree->get_selected()==edited_scene) {
 
 
@@ -195,6 +209,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			}
 
 
+			if (!_validate_no_foreign())
+				break;
+
 			Node * node=scene_tree->get_selected();
 			ERR_FAIL_COND(!node->get_parent());
 			int current_pos = node->get_index();
@@ -214,6 +231,7 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (!edited_scene)
 				break;
 
+
 			if (editor_selection->is_selected(edited_scene)) {
 
 
@@ -225,6 +243,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 				break;
 			}
 
+			if (!_validate_no_foreign())
+				break;
+
 			List<Node*> selection = editor_selection->get_selected_node_list();
 
 			List<Node*> reselect;
@@ -313,6 +334,7 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (!scene_tree->get_selected())
 				break;
 
+
 			if (editor_selection->is_selected(edited_scene)) {
 
 
@@ -324,6 +346,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 				break;
 			}
 
+			if (!_validate_no_foreign())
+				break;
+
 			List<Node*> nodes = editor_selection->get_selected_node_list();
 			Set<Node*> nodeset;
 			for(List<Node*>::Element *E=nodes.front();E;E=E->next()) {
@@ -341,6 +366,9 @@ void SceneTreeDock::_tool_selected(int p_tool, bool p_confirm_override) {
 			if (remove_list.empty())
 				return;
 
+			if (!_validate_no_foreign())
+				break;
+
 			if (p_confirm_override) {
 				_delete_confirm();
 
@@ -707,6 +735,25 @@ void SceneTreeDock::_node_prerenamed(Node* p_node, const String& p_new_name) {
 
 }
 
+bool SceneTreeDock::_validate_no_foreign() {
+
+	List<Node*> selection = editor_selection->get_selected_node_list();
+
+	for (List<Node*>::Element *E=selection.front();E;E=E->next()) {
+
+		if (E->get()!=edited_scene && E->get()->get_owner()!=edited_scene) {
+
+			accept->get_ok()->set_text("Makes Sense!");
+			accept->set_text("Can't operate on nodes from a foreign scene!");
+			accept->popup_centered(Size2(300,70));;
+			return false;
+
+		}
+	}
+
+	return true;
+}
+
 void SceneTreeDock::_node_reparent(NodePath p_path,bool p_node_only) {
 
 
@@ -894,7 +941,7 @@ void SceneTreeDock::_delete_confirm() {
 void SceneTreeDock::_update_tool_buttons() {
 
 	Node *sel = scene_tree->get_selected();
-	bool disable = !sel;
+	bool disable = !sel || (sel!=edited_scene && sel->get_owner()!=edited_scene);
 	bool disable_root = disable || sel->get_parent()==scene_root;
 
 	tool_buttons[TOOL_INSTANCE]->set_disabled(disable);
diff --git a/tools/editor/scene_tree_dock.h b/tools/editor/scene_tree_dock.h
index 99ef16e6f6..e55a54377a 100644
--- a/tools/editor/scene_tree_dock.h
+++ b/tools/editor/scene_tree_dock.h
@@ -115,6 +115,7 @@ class SceneTreeDock : public VBoxContainer {
 
 	void _import_subscene();
 
+	bool _validate_no_foreign();
 
 	void _fill_path_renames(Vector<StringName> base_path,Vector<StringName> new_base_path,Node * p_node, List<Pair<NodePath,NodePath> > *p_renames);
 
diff --git a/tools/editor/scene_tree_editor.cpp b/tools/editor/scene_tree_editor.cpp
index e0202be84e..59bc24487a 100644
--- a/tools/editor/scene_tree_editor.cpp
+++ b/tools/editor/scene_tree_editor.cpp
@@ -45,6 +45,40 @@ Node *SceneTreeEditor::get_scene_node() {
 	return NULL;
 }
 
+
+void SceneTreeEditor::_subscene_option(int p_idx) {
+
+	Object *obj = ObjectDB::get_instance(instance_node);
+	if (!obj)
+		return;
+	Node *node = obj->cast_to<Node>();
+	if (!node)
+		return;
+
+	switch(p_idx) {
+
+		case SCENE_MENU_SHOW_CHILDREN: {
+
+			if (node->has_meta("__editor_show_subtree")) {
+				instance_menu->set_item_checked(0,true);
+				node->set_meta("__editor_show_subtree",Variant());
+				_update_tree();
+			} else {
+				node->set_meta("__editor_show_subtree",true);
+				_update_tree();
+			}
+
+		} break;
+		case SCENE_MENU_OPEN: {
+
+			emit_signal("open",node->get_filename());
+		} break;
+
+	}
+
+}
+
+
 void SceneTreeEditor::_cell_button_pressed(Object *p_item,int p_column,int p_id) {
 
 	TreeItem *item=p_item->cast_to<TreeItem>();
@@ -57,7 +91,19 @@ void SceneTreeEditor::_cell_button_pressed(Object *p_item,int p_column,int p_id)
 
 	if (p_id==BUTTON_SUBSCENE) {
 		//open scene request
-		emit_signal("open",n->get_filename());
+		Rect2 item_rect = tree->get_item_rect(item,0);
+		item_rect.pos.y-=tree->get_scroll().y;
+		item_rect.pos+=tree->get_global_pos();
+		instance_menu->set_pos(item_rect.pos+Vector2(0,item_rect.size.y));
+		instance_menu->set_size(Vector2(item_rect.size.x,0));
+		if (n->has_meta("__editor_show_subtree"))
+			instance_menu->set_item_checked(0,true);
+		else
+			instance_menu->set_item_checked(0,false);
+
+		instance_menu->popup();
+		instance_node=n->get_instance_ID();
+		//emit_signal("open",n->get_filename());
 	} else if (p_id==BUTTON_SCRIPT) {
 		RefPtr script=n->get_script();
 		if (!script.is_null())
@@ -119,9 +165,19 @@ void SceneTreeEditor::_add_nodes(Node *p_node,TreeItem *p_parent) {
 
 	// only owned nodes are editable, since nodes can create their own (manually owned) child nodes,
 	// which the editor needs not to know about.
-	
-	if (!display_foreign && p_node->get_owner()!=get_scene_node() && p_node!=get_scene_node())
-		return;
+
+	bool part_of_subscene=false;
+
+	if (!display_foreign && p_node->get_owner()!=get_scene_node() && p_node!=get_scene_node()) {
+
+		if ((show_enabled_subscene || can_open_instance) && p_node->get_owner() && p_node->get_owner()->get_owner()==get_scene_node() && p_node->get_owner()->has_meta("__editor_show_subtree")) {
+
+			part_of_subscene=true;
+			//allow
+		} else {
+			return;
+		}
+	}
 
 	TreeItem *item = tree->create_item(p_parent);
 	item->set_text(0, p_node->get_name() );
@@ -143,8 +199,12 @@ void SceneTreeEditor::_add_nodes(Node *p_node,TreeItem *p_parent) {
 		icon=get_icon( (has_icon(p_node->get_type(),"EditorIcons")?p_node->get_type():String("Object")),"EditorIcons");
 	item->set_icon(0, icon );
 	item->set_metadata( 0,p_node->get_path() );	
-	
-	if (marked.has(p_node)) {
+	if (part_of_subscene) {
+
+		//item->set_selectable(0,marked_selectable);
+		item->set_custom_color(0,Color(0.8,0.4,0.20));
+
+	} else if (marked.has(p_node)) {
 				
 		item->set_selectable(0,marked_selectable);
 		item->set_custom_color(0,Color(0.8,0.1,0.10));
@@ -163,7 +223,7 @@ void SceneTreeEditor::_add_nodes(Node *p_node,TreeItem *p_parent) {
 
 	if (p_node!=get_scene_node() && p_node->get_filename()!="" && can_open_instance) {
 
-		item->add_button(0,get_icon("Load","EditorIcons"),BUTTON_SUBSCENE);
+		item->add_button(0,get_icon("InstanceOptions","EditorIcons"),BUTTON_SUBSCENE);
 		item->set_tooltip(0,"Instance: "+p_node->get_filename());
 	}
 
@@ -425,6 +485,7 @@ void SceneTreeEditor::_notification(int p_what) {
 
 		get_scene()->connect("tree_changed",this,"_tree_changed");
 		get_scene()->connect("node_removed",this,"_node_removed");
+		instance_menu->set_item_icon(2,get_icon("Load","EditorIcons"));
 		tree->connect("item_collapsed",this,"_cell_collapsed");
 
 //		get_scene()->connect("tree_changed",this,"_tree_changed",Vector<Variant>(),CONNECT_DEFERRED);
@@ -646,6 +707,7 @@ void SceneTreeEditor::_cell_collapsed(Object *p_obj) {
 }
 
 
+
 void SceneTreeEditor::_bind_methods() {
 	
 	ObjectTypeDB::bind_method("_tree_changed",&SceneTreeEditor::_tree_changed);
@@ -659,6 +721,7 @@ void SceneTreeEditor::_bind_methods() {
 	ObjectTypeDB::bind_method("_selection_changed",&SceneTreeEditor::_selection_changed);
 	ObjectTypeDB::bind_method("_cell_button_pressed",&SceneTreeEditor::_cell_button_pressed);
 	ObjectTypeDB::bind_method("_cell_collapsed",&SceneTreeEditor::_cell_collapsed);
+	ObjectTypeDB::bind_method("_subscene_option",&SceneTreeEditor::_subscene_option);
 
 	ObjectTypeDB::bind_method("_node_script_changed",&SceneTreeEditor::_node_script_changed);
 	ObjectTypeDB::bind_method("_node_visibility_changed",&SceneTreeEditor::_node_visibility_changed);
@@ -714,10 +777,20 @@ SceneTreeEditor::SceneTreeEditor(bool p_label,bool p_can_rename, bool p_can_open
 	error = memnew( AcceptDialog );
 	add_child(error);
 
+	show_enabled_subscene=false;
+
 	last_hash=0;
 	pending_test_update=false;
 	updating_tree=false;
 	blocked=0;
+
+	instance_menu = memnew( PopupMenu );
+	instance_menu->add_check_item("Show Children",SCENE_MENU_SHOW_CHILDREN);
+	instance_menu->add_separator();
+	instance_menu->add_item("Open in Editor",SCENE_MENU_OPEN);
+	instance_menu->connect("item_pressed",this,"_subscene_option");
+	add_child(instance_menu);
+
 }
 
 
diff --git a/tools/editor/scene_tree_editor.h b/tools/editor/scene_tree_editor.h
index 19375ba638..5e88c5a41d 100644
--- a/tools/editor/scene_tree_editor.h
+++ b/tools/editor/scene_tree_editor.h
@@ -51,8 +51,15 @@ class SceneTreeEditor : public Control {
 		BUTTON_GROUP=4,
 	};
 
+	enum {
+		SCENE_MENU_SHOW_CHILDREN,
+		SCENE_MENU_OPEN,
+	};
+
 	Tree *tree;
 	Node *selected;
+	PopupMenu *instance_menu;
+	ObjectID instance_node;
 
 	AcceptDialog *error;
 
@@ -78,6 +85,7 @@ class SceneTreeEditor : public Control {
 	bool can_rename;
 	bool can_open_instance;
 	bool updating_tree;
+	bool show_enabled_subscene;
 	
 	void _renamed();
 	UndoRedo *undo_redo;
@@ -95,6 +103,7 @@ class SceneTreeEditor : public Control {
 	void _update_selection(TreeItem *item);
 	void _node_script_changed(Node *p_node);
 	void _node_visibility_changed(Node *p_node);
+	void _subscene_option(int p_idx);
 
 	void _selection_changed();
 	Node *get_scene_node();
@@ -112,6 +121,8 @@ public:
 	void set_can_rename(bool p_can_rename) { can_rename=p_can_rename; }
 	void set_editor_selection(EditorSelection *p_selection);
 
+	void set_show_enabled_subscene(bool p_show) { show_enabled_subscene=p_show; }
+
 	void update_tree() { _update_tree(); }
 
 	SceneTreeEditor(bool p_label=true,bool p_can_rename=false, bool p_can_open_instance=false);
diff --git a/tools/editor/spatial_editor_gizmos.cpp b/tools/editor/spatial_editor_gizmos.cpp
index c5bf43884e..c2acfed8ff 100644
--- a/tools/editor/spatial_editor_gizmos.cpp
+++ b/tools/editor/spatial_editor_gizmos.cpp
@@ -1538,6 +1538,70 @@ CarWheelSpatialGizmo::CarWheelSpatialGizmo(CarWheel* p_car_wheel){
 }
 
 
+/////
+
+
+void VehicleWheelSpatialGizmo::redraw() {
+
+	clear();
+
+
+	Vector<Vector3> points;
+
+	float r = car_wheel->get_radius();
+	const int skip=10;
+	for(int i=0;i<=360;i+=skip) {
+
+		float ra=Math::deg2rad(i);
+		float rb=Math::deg2rad(i+skip);
+		Point2 a = Vector2(Math::sin(ra),Math::cos(ra))*r;
+		Point2 b = Vector2(Math::sin(rb),Math::cos(rb))*r;
+
+		points.push_back(Vector3(0,a.x,a.y));
+		points.push_back(Vector3(0,b.x,b.y));
+
+		const int springsec=4;
+
+		for(int j=0;j<springsec;j++) {
+			float t = car_wheel->get_suspension_rest_length()*5;
+			points.push_back(Vector3(a.x,i/360.0*t/springsec+j*(t/springsec),a.y)*0.2);
+			points.push_back(Vector3(b.x,(i+skip)/360.0*t/springsec+j*(t/springsec),b.y)*0.2);
+		}
+
+
+	}
+
+	//travel
+	points.push_back(Vector3(0,0,0));
+	points.push_back(Vector3(0,car_wheel->get_suspension_rest_length(),0));
+
+	//axis
+	points.push_back(Vector3(r*0.2,car_wheel->get_suspension_rest_length(),0));
+	points.push_back(Vector3(-r*0.2,car_wheel->get_suspension_rest_length(),0));
+	//axis
+	points.push_back(Vector3(r*0.2,0,0));
+	points.push_back(Vector3(-r*0.2,0,0));
+
+	//forward line
+	points.push_back(Vector3(0,-r,0));
+	points.push_back(Vector3(0,-r,r*2));
+	points.push_back(Vector3(0,-r,r*2));
+	points.push_back(Vector3(r*2*0.2,-r,r*2*0.8));
+	points.push_back(Vector3(0,-r,r*2));
+	points.push_back(Vector3(-r*2*0.2,-r,r*2*0.8));
+
+	add_lines(points,SpatialEditorGizmos::singleton->car_wheel_material);
+	add_collision_segments(points);
+
+}
+
+VehicleWheelSpatialGizmo::VehicleWheelSpatialGizmo(VehicleWheel* p_car_wheel){
+
+	set_spatial_node(p_car_wheel);
+	car_wheel=p_car_wheel;
+}
+
+
 
 ///
 
@@ -2292,6 +2356,11 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		Ref<CarWheelSpatialGizmo> misg = memnew( CarWheelSpatialGizmo(p_spatial->cast_to<CarWheel>()) );
 		return misg;
 	}
+	if (p_spatial->cast_to<VehicleWheel>()) {
+
+		Ref<VehicleWheelSpatialGizmo> misg = memnew( VehicleWheelSpatialGizmo(p_spatial->cast_to<VehicleWheel>()) );
+		return misg;
+	}
 
 	return Ref<SpatialEditorGizmo>();
 }
diff --git a/tools/editor/spatial_editor_gizmos.h b/tools/editor/spatial_editor_gizmos.h
index e5c3417166..f519b84a29 100644
--- a/tools/editor/spatial_editor_gizmos.h
+++ b/tools/editor/spatial_editor_gizmos.h
@@ -45,6 +45,7 @@
 #include "scene/3d/ray_cast.h"
 #include "scene/3d/navigation_mesh.h"
 #include "scene/3d/car_body.h"
+#include "scene/3d/vehicle_body.h"
 
 
 class Camera;
@@ -328,6 +329,20 @@ public:
 };
 
 
+class VehicleWheelSpatialGizmo  : public SpatialGizmoTool {
+
+	OBJ_TYPE(VehicleWheelSpatialGizmo,SpatialGizmoTool);
+
+	VehicleWheel* car_wheel;
+
+public:
+
+	void redraw();
+	VehicleWheelSpatialGizmo(VehicleWheel* p_car_wheel=NULL);
+
+};
+
+
 class NavigationMeshSpatialGizmo  : public SpatialGizmoTool {
 
 	OBJ_TYPE(NavigationMeshSpatialGizmo,SpatialGizmoTool);
diff --git a/tools/pck/pck_packer.cpp b/tools/pck/pck_packer.cpp
index aade9fafe1..09611b3a93 100644
--- a/tools/pck/pck_packer.cpp
+++ b/tools/pck/pck_packer.cpp
@@ -137,6 +137,7 @@ Error PCKPacker::flush(bool p_verbose) {
 		if (p_verbose) {
 			if (count % 100 == 0) {
 				printf("%i/%i (%.2f\%)\r", count, files.size(), float(count) / files.size() * 100);
+				fflush(stdout);
 			};
 		};
 	};
author	Juan Linietsky <reduzio@gmail.com>	2014-09-02 23:13:40 -0300
committer	Juan Linietsky <reduzio@gmail.com>	2014-09-02 23:13:40 -0300
commit	1a2cb755e2d8b9d59178f36702f6dff7235b9088 (patch)
tree	4a88f47c8b984522e36ac973accb34bdcb00363b
parent	89fa70706f9166765c3ac3f799225a467800f065 (diff)