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!

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]
+		if not (f[0] in ARGUMENTS): # allow command line to override platform flags
-		print(f[0]+":"+f[1])
+			env[f[0]] = f[1]
+			print(f[0]+":"+f[1])
 
 	env.module_list=[]
 

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_NONE ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_RANGE  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RANGE ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_ENUM  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_RANGE ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_LENGTH  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_ENUM ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FLAGS  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_EXP_EASING ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_FILE  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_LENGTH ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_DIR  ),
+	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_KEY_ACCEL ),
-	BIND_GLOBAL_CONSTANT( PROPERTY_HINT_RESOURCE_TYPE  ),
+	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  ),

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

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

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;

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>

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

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

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

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

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: {}
 	}
 

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

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

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

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

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

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
 }
 

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

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

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'])

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;
+			old_invalid=true;
-                    outside=true;
+			outside=true;
 
 		} break;
 		case WM_MOUSEMOVE: {
 
-                    if (outside) {
+			if (outside) {
 
-                        CursorShape c=cursor_shape;
+				CursorShape c=cursor_shape;
-                        cursor_shape=CURSOR_MAX;
+				cursor_shape=CURSOR_MAX;
-                        set_cursor_shape(c);
+				set_cursor_shape(c);
-                        outside=false;
+				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;

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;

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

scene/3d/physics_body.cpp

@@ -57,10 +57,56 @@ float PhysicsBody::get_inverse_mass() const {
 	return 0;
 }
 
+
+void PhysicsBody::set_layer_mask(uint32_t p_mask) {
+
+	layer_mask=p_mask;
+	PhysicsServer::get_singleton()->body_set_layer_mask(get_rid(),p_mask);
+}
+
+uint32_t PhysicsBody::get_layer_mask() const {
+
+	return layer_mask;
+}
+
+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"));
+}
+
+
 PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) {
 
+	layer_mask=1;
+
+}
 
 
+void StaticBody::set_friction(real_t p_friction){
+
+	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{
+
+	return friction;
+}
+
+void StaticBody::set_bounce(real_t p_bounce){
+
+	ERR_FAIL_COND(p_bounce<0 || p_bounce>1);
+
+	bounce=p_bounce;
+	PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_BOUNCE,bounce);
+
+}
+real_t StaticBody::get_bounce() const{
+
+	return bounce;
 }
 
 void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) {
@@ -86,131 +132,36 @@ Vector3 StaticBody::get_constant_angular_velocity() const {
 }
 
 
-void StaticBody::_state_notify(Object *p_object) {
-
-	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);
-
-	setting=false;
-
-
-}
-
-void StaticBody::_update_xform() {
-
-	if (!pre_xform || !pending)
-		return;
-
-	setting=true;
-
-
-	Transform new_xform = get_global_transform(); //obtain the new one
-
-	//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);
-
-	PhysicsServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion!
-
-	setting=false;
-	pending=false;
-
-}
-
-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: {
-
-			if (simulating_motion && !pending && is_inside_scene() && !setting && !get_scene()->is_editor_hint()) {
-
-				call_deferred(SceneStringNames::get_singleton()->_update_xform);
-				pending=true;
-			}
-
-		} break;
-	}
-
-
-}
-
-void StaticBody::set_simulate_motion(bool p_enable) {
-
-	if (p_enable==simulating_motion)
-		return;
-	simulating_motion=p_enable;
-
-	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");
-
-	} else {
-		memdelete( pre_xform );
-		pre_xform=NULL;
-		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName());
-		pending=false;
-	}
-}
-
-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;
+	bounce=0;
-	pre_xform=NULL;
+	friction=1;
-	setting=false;
-	pending=false;
-	//constant_angular_velocity=0;
-
 }
 
 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()  {
+
+
+}
+
 

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

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

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

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;
 

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

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

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;
 

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

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) {
+	Vector3 motion;
-		//overriden by direct state query
+	bool do_motion=false;
 
-		Vector3 force=gravity*mass;
+	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
-		force+=applied_force;
-		Vector3 torque=applied_torque;
 
-		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
+		//compute a FAKE angular velocity, not so easy
-			damp=0;
+		Matrix3 rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
+		Vector3 axis;
+		float angle;
 
-		real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+		rot.get_axis_and_angle(axis,angle);
+		axis.normalize();
+		angular_velocity=axis.normalized() * (angle/p_step);
 
-		if (angular_damp<0) // reached zero in the given time
+		motion = new_transform.origin - get_transform().origin;
-			angular_damp=0;
+		do_motion=true;
 
-		linear_velocity*=damp;
+	} else {
-		angular_velocity*=angular_damp;
+		if (!omit_force_integration) {
+			//overriden by direct state query
+
+			Vector3 force=gravity*mass;
+			force+=applied_force;
+			Vector3 torque=applied_torque;
+
+			real_t damp = 1.0 - p_step * density;
+
+			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) {
+	//if (fi_callback) {
-
-		get_space()->body_add_to_state_query_list(&direct_state_query_list);
-	}
 
+	//	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;

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

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

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

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)
+		//if (d>CMP_EPSILON)
-			continue;
+		//	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
 
 }
 
-
+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) {
+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
 }
 
 
-
+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) {
-
-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) {
 
 
 	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();
 
 
 }
 
-
+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) {
+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();
 }
 
-
+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) {
+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_sphere<false>,
-		 _collision_sphere_box,
+		 _collision_sphere_box<false>,
-		 _collision_sphere_capsule,
+		 _collision_sphere_capsule<false>,
-		 _collision_sphere_convex_polygon,
+		 _collision_sphere_convex_polygon<false>,
-		 _collision_sphere_face},
+		 _collision_sphere_face<false>},
 		{0,
-		 _collision_box_box,
+		 _collision_box_box<false>,
-		 _collision_box_capsule,
+		 _collision_box_capsule<false>,
-		 _collision_box_convex_polygon,
+		 _collision_box_convex_polygon<false>,
-		 _collision_box_face},
+		 _collision_box_face<false>},
 		{0,
 		 0,
-		 _collision_capsule_capsule,
+		 _collision_capsule_capsule<false>,
-		 _collision_capsule_convex_polygon,
+		 _collision_capsule_convex_polygon<false>,
-		 _collision_capsule_face},
+		 _collision_capsule_face<false>},
 		{0,
 		 0,
 		 0,
-		 _collision_convex_polygon_convex_polygon,
+		 _collision_convex_polygon_convex_polygon<false>,
-		 _collision_convex_polygon_face},
+		 _collision_convex_polygon_face<false>},
+		{0,
+		 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<true>,
+		 _collision_capsule_convex_polygon<true>,
+		 _collision_capsule_face<true>},
+		{0,
+		 0,
+		 0,
+		 _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;
 

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

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

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

servers/physics/gjk_epa.cpp

@@ -1,31 +1,14 @@
-/*************************************************************************/
+/*************************************************/
-/*  gjk_epa.cpp                                                          */
+/*  gjk_epa.cpp                                  */
-/*************************************************************************/
+/*************************************************/
-/*                       This file is part of:                           */
+/*            This file is part of:              */
-/*                           GODOT ENGINE                                */
+/*                GODOT ENGINE                   */
-/*                    http://www.godotengine.org                         */
+/*************************************************/
-/*************************************************************************/
+/*       Source code within this file is:        */
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*  (c) 2007-2010 Juan Linietsky, Ariel Manzur   */
-/*                                                                       */
+/*             All Rights Reserved.              */
-/* Permission is hereby granted, free of charge, to any person obtaining */
+/*************************************************/
-/* a copy of this software and associated documentation files (the       */
+
-/* "Software"), to deal in the Software without restriction, including   */
-/* without limitation the rights to use, copy, modify, merge, publish,   */
-/* distribute, sublicense, and/or sell copies of the Software, and to    */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions:                                             */
-/*                                                                       */
-/* The above copyright notice and this permission notice shall be        */
-/* included in all copies or substantial portions of the Software.       */
-/*                                                                       */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
-/*************************************************************************/
 #include "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[0]	=	w0;
-		results.witnesses[1]	=	wtrs0.xform(w1);
+		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;

servers/physics/gjk_epa.h

@@ -1,31 +1,14 @@
-/*************************************************************************/
+/*************************************************/
-/*  gjk_epa.h                                                            */
+/*  gjk_epa.h                                    */
-/*************************************************************************/
+/*************************************************/
-/*                       This file is part of:                           */
+/*            This file is part of:              */
-/*                           GODOT ENGINE                                */
+/*                GODOT ENGINE                   */
-/*                    http://www.godotengine.org                         */
+/*************************************************/
-/*************************************************************************/
+/*       Source code within this file is:        */
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
+/*  (c) 2007-2010 Juan Linietsky, Ariel Manzur   */
-/*                                                                       */
+/*             All Rights Reserved.              */
-/* 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.                */
-/*************************************************************************/
 #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

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;
 

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

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)
+		if (i!=vert_support_idx && nx!=vert_support_idx)
-		//	continue;
+			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();

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
+#ifndef SHAPE_SW_H
-#define SHAPE_SW_H
+#define SHAPE_SW_H
-
+
-#include "servers/physics_server.h"
+#include "servers/physics_server.h"
-#include "bsp_tree.h"
+#include "bsp_tree.h"
-#include "geometry.h"
+#include "geometry.h"
-/*
+/*
-
+
-SHAPE_LINE, ///< plane:"plane"
+SHAPE_LINE, ///< plane:"plane"
-SHAPE_SEGMENT, ///< float:"length"
+SHAPE_SEGMENT, ///< float:"length"
-SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_CIRCLE, ///< float:"radius"
-SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_RECTANGLE, ///< vec3:"extents"
-SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
-SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+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
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
-
+
-*/
+*/
-
+
-class ShapeSW;
+class ShapeSW;
-
+
-class ShapeOwnerSW {
+class ShapeOwnerSW {
-public:
+public:
-
+
-	virtual void _shape_changed()=0;
+	virtual void _shape_changed()=0;
-	virtual void remove_shape(ShapeSW *p_shape)=0;
+	virtual void remove_shape(ShapeSW *p_shape)=0;
-
+
-	virtual ~ShapeOwnerSW() {}
+	virtual ~ShapeOwnerSW() {}
-};
+};
-
+
-
+
-class ShapeSW {
+class ShapeSW {
-
+
-	RID self;
+	RID self;
-	AABB aabb;
+	AABB aabb;
-	bool configured;
+	bool configured;
-	real_t custom_bias;
+	real_t custom_bias;
-
+
-	Map<ShapeOwnerSW*,int> owners;
+	Map<ShapeOwnerSW*,int> owners;
-protected:
+protected:
-
+
-	void configure(const AABB& p_aabb);
+	void configure(const AABB& p_aabb);
-public:
+public:
-
+
-	enum {
+	enum {
-		MAX_SUPPORTS=8
+		MAX_SUPPORTS=8
-	};
+	};
-
+
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
-	_FORCE_INLINE_ RID get_self() const {return  self; }
+	_FORCE_INLINE_ RID get_self() const {return  self; }
-
+
-	virtual PhysicsServer::ShapeType get_type() const=0;
+	virtual PhysicsServer::ShapeType get_type() const=0;
-
+
-	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
+	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
-	_FORCE_INLINE_ bool is_configured() const { return configured; }
+	_FORCE_INLINE_ bool is_configured() const { return configured; }
-
+
-	virtual bool is_concave() const { return false; }
+	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 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 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 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 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 Vector3 get_moment_of_inertia(float p_mass) const=0;
-
+
-	virtual void set_data(const Variant& p_data)=0;
+	virtual void set_data(const Variant& p_data)=0;
-	virtual Variant get_data() const=0;
+	virtual Variant get_data() const=0;
-
+
-	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_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; }
+	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
-
+
-	void add_owner(ShapeOwnerSW *p_owner);
+	void add_owner(ShapeOwnerSW *p_owner);
-	void remove_owner(ShapeOwnerSW *p_owner);
+	void remove_owner(ShapeOwnerSW *p_owner);
-	bool is_owner(ShapeOwnerSW *p_owner) const;
+	bool is_owner(ShapeOwnerSW *p_owner) const;
-	const Map<ShapeOwnerSW*,int>& get_owners() const;
+	const Map<ShapeOwnerSW*,int>& get_owners() const;
-
+
-	ShapeSW();
+	ShapeSW();
-	virtual ~ShapeSW();
+	virtual ~ShapeSW();
-};
+};
-
+
-
+
-class ConcaveShapeSW : public ShapeSW {
+class ConcaveShapeSW : public ShapeSW {
-
+
-public:
+public:
-
+
-	virtual bool is_concave() const { return true; }
+	virtual bool is_concave() const { return true; }
-	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
+	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 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;
+	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
-
+
-	ConcaveShapeSW() {}
+	ConcaveShapeSW() {}
-};
+};
-
+
-class PlaneShapeSW : public ShapeSW {
+class PlaneShapeSW : public ShapeSW {
-
+
-	Plane plane;
+	Plane plane;
-
+
-	void _setup(const Plane& p_plane);
+	void _setup(const Plane& p_plane);
-public:
+public:
-
+
-	Plane get_plane() const;
+	Plane get_plane() const;
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
-
+
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	PlaneShapeSW();
+	PlaneShapeSW();
-};
+};
-
+
-class RayShapeSW : public ShapeSW {
+class RayShapeSW : public ShapeSW {
-
+
-	float length;
+	float length;
-
+
-	void _setup(float p_length);
+	void _setup(float p_length);
-public:
+public:
-
+
-	float get_length() const;
+	float get_length() const;
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-
+
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	RayShapeSW();
+	RayShapeSW();
-};
+};
-
+
-class SphereShapeSW : public ShapeSW {
+class SphereShapeSW : public ShapeSW {
-
+
-	real_t radius;
+	real_t radius;
-
+
-	void _setup(real_t p_radius);
+	void _setup(real_t p_radius);
-public:
+public:
-
+
-	real_t get_radius() const;
+	real_t get_radius() const;
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	SphereShapeSW();
+	SphereShapeSW();
-};
+};
-
+
-class BoxShapeSW : public ShapeSW {
+class BoxShapeSW : public ShapeSW {
-
+
-	Vector3 half_extents;
+	Vector3 half_extents;
-	void _setup(const Vector3& p_half_extents);
+	void _setup(const Vector3& p_half_extents);
-public:
+public:
-
+
-	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	BoxShapeSW();
+	BoxShapeSW();
-};
+};
-
+
-class CapsuleShapeSW : public ShapeSW {
+class CapsuleShapeSW : public ShapeSW {
-
+
-	real_t height;
+	real_t height;
-	real_t radius;
+	real_t radius;
-
+
-
+
-	void _setup(real_t p_height,real_t p_radius);
+	void _setup(real_t p_height,real_t p_radius);
-public:
+public:
-
+
-	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_height() const { return height; }
-	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	CapsuleShapeSW();
+	CapsuleShapeSW();
-};
+};
-
+
-struct ConvexPolygonShapeSW : public ShapeSW {
+struct ConvexPolygonShapeSW : public ShapeSW {
-
+
-	Geometry::MeshData mesh;
+	Geometry::MeshData mesh;
-
+
-	void _setup(const Vector<Vector3>& p_vertices);
+	void _setup(const Vector<Vector3>& p_vertices);
-public:
+public:
-
+
-	const Geometry::MeshData& get_mesh() const { return mesh; }
+	const Geometry::MeshData& get_mesh() const { return mesh; }
-
+
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
+	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 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 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 void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_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 Vector3 get_moment_of_inertia(float p_mass) const;
-
+
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant& p_data);
-	virtual Variant get_data() const;
+	virtual Variant get_data() const;
-
+
-	ConvexPolygonShapeSW();
+	ConvexPolygonShapeSW();
-
+
-};
+};
-
+
-
+
-struct _VolumeSW_BVH;
+struct _VolumeSW_BVH;
-struct FaceShapeSW;
+struct FaceShapeSW;
-
+
-struct ConcavePolygonShapeSW : public ConcaveShapeSW {
+struct ConcavePolygonShapeSW : public ConcaveShapeSW {
-	// always a trimesh
+	// always a trimesh
-
+
-	struct Face {
+	struct Face {
-
+
-		Vector3 normal;
+		Vector3 normal;
-		int indices[3];
+		int indices[3];
-	};
+	};
-
+
-	DVector<Face> faces;
+	DVector<Face> faces;
-	DVector<Vector3> vertices;
+	DVector<Vector3> vertices;
-
+
-	struct BVH {
+	struct BVH {
-
+
-		AABB aabb;
+		AABB aabb;
-		int left;
+		int left;
-		int right;
+		int right;
-
+
-		int face_index;
+		int face_index;
-	};
+	};
-
+
-	DVector<BVH> bvh;
+	DVector<BVH> bvh;
-
+
-	struct _CullParams {
+	struct _CullParams {
-
+
-		AABB aabb;
+		AABB aabb;
-		Callback callback;
+		Callback callback;
-		void *userdata;
+		void *userdata;
-		const Face *faces;
+		const Face *faces;
-		const Vector3 *vertices;
+		const Vector3 *vertices;
-		const BVH *bvh;
+		const BVH *bvh;
-		FaceShapeSW *face;
+		FaceShapeSW *face;
-	};
+	};
-
+
-	struct _SegmentCullParams {
+	struct _SegmentCullParams {
-
+
-		Vector3 from;
+		Vector3 from;
-		Vector3 to;
+		Vector3 to;
-		const Face *faces;
+		const Face *faces;
-		const Vector3 *vertices;
+		const Vector3 *vertices;
-		const BVH *bvh;
+		const BVH *bvh;
-
+		Vector3 dir;
-		Vector3 result;
+
-		Vector3 normal;
+		Vector3 result;
-		real_t min_d;
+		Vector3 normal;
-		int collisions;
+		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 _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 _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
-
+
-	void _setup(DVector<Vector3> p_faces);
+
-public:
+	void _setup(DVector<Vector3> p_faces);
-
+public:
-	DVector<Vector3> get_faces() const;
+
-
+	DVector<Vector3> get_faces() const;
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
-
+	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 void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-
+	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
-
+	virtual bool intersect_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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-	virtual void set_data(const Variant& p_data);
+
-	virtual Variant get_data() const;
+	virtual void set_data(const Variant& p_data);
-
+	virtual Variant get_data() const;
-	ConcavePolygonShapeSW();
+
-
+	ConcavePolygonShapeSW();
-};
+
-
+};
-
+
-struct HeightMapShapeSW : public ConcaveShapeSW {
+
-
+struct HeightMapShapeSW : public ConcaveShapeSW {
-	DVector<real_t> heights;
+
-	int width;
+	DVector<real_t> heights;
-	int depth;
+	int width;
-	float cell_size;
+	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 _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:
+	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;
+	DVector<real_t> get_heights() const;
-	int get_depth() const;
+	int get_width() const;
-	float get_cell_size() const;
+	int get_depth() const;
-
+	float get_cell_size() const;
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
+
-
+	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 void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) 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 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 Vector3 get_moment_of_inertia(float p_mass) const;
-	virtual void set_data(const Variant& p_data);
+
-	virtual Variant get_data() const;
+	virtual void set_data(const Variant& p_data);
-
+	virtual Variant get_data() const;
-	HeightMapShapeSW();
+
-
+	HeightMapShapeSW();
-};
+
-
+};
-//used internally
+
-struct FaceShapeSW : public ShapeSW {
+//used internally
-
+struct FaceShapeSW : public ShapeSW {
-	Vector3 normal; //cache
+
-	Vector3 vertex[3];
+	Vector3 normal; //cache
-
+	Vector3 vertex[3];
-	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
-
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
-	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
+
-
+	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;
+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	Vector3 get_support(const Vector3& p_normal) const;
-	bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_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;
+
-
+	Vector3 get_moment_of_inertia(float p_mass) const;
-	virtual void set_data(const Variant& p_data) {}
+
-	virtual Variant get_data() const { return Variant(); }
+	virtual void set_data(const Variant& p_data) {}
-
+	virtual Variant get_data() const { return Variant(); }
-	FaceShapeSW();
+
-};
+	FaceShapeSW();
-
+};
-
+
-
+
-
+struct MotionShapeSW : public ShapeSW {
-
+
-struct _ShapeTestConvexBSPSW {
+	ShapeSW *shape;
-
+	Vector3 motion;
-	const BSP_Tree *bsp;
+
-	const ShapeSW *shape;
+	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
-	Transform transform;
+
-
+
-	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
+	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
-
+
-		shape->project_range(p_normal,transform,r_min,r_max);
+		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);
-#endif // SHAPESW_H
+		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

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)
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
-			continue; //ignore area
+			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)
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
-			continue; //ignore area
+			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;

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

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 */

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;
 

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())
+	//if (!separator.test_previous_axis())
-		return;
+	//	return;
 
 	if (!separator.test_cast())
 		return;

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

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

servers/physics_2d/shape_2d_sw.h

@@ -582,5 +582,6 @@ public:
 
 };
 
+#undef DEFAULT_PROJECT_RANGE_CAST
 
 #endif // SHAPE_2D_2DSW_H

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;

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

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

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) {
+Array Physics2DDirectSpaceState::_intersect_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results) {
 
-	ERR_FAIL_INDEX_V(p_result_max,4096,Variant());
+	Vector<ShapeResult> sr;
-	if (p_result_max<=0)
+	sr.resize(p_max_results);
-		return Variant();
+	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++) {
 
-	Set<RID> exclude;
+		Dictionary d;
-	for(int i=0;i<p_exclude.size();i++)
+		d["rid"]=sr[i].rid;
-		exclude.insert(p_exclude[i]);
+		d["collider_id"]=sr[i].collider_id;
-
+		d["collider"]=sr[i].collider;
-	ShapeResult *res=(ShapeResult*)alloca(p_result_max*sizeof(ShapeResult));
+		d["shape"]=sr[i].shape;
-
+		ret[i]=d;
-	int rc = intersect_shape(p_shape,p_xform,Vector2(),0,res,p_result_max,exclude,p_user_mask);
+	}
-
-	if (rc==0)
-		return Variant();
-
-	Ref<Physics2DShapeQueryResult>  result = memnew( Physics2DShapeQueryResult );
-	result->result.resize(rc);
-	for(int i=0;i<rc;i++)
-		result->result[i]=res[i];
-
-	return result;
 
+	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;
 
+}
+Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results){
 
-#if 0
+	Vector<Vector2> ret;
-	Set<RID> exclude;
+	ret.resize(p_max_results*2);
-	for(int i=0;i<p_exclude.size();i++)
+	int rc=0;
-		exclude.insert(p_exclude[i]);
+	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;
 
+}
+Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq){
 
-	bool result = cast_motion(p_shape,p_xform,p_motion,0,mcc,exclude,p_user_mask);
+	ShapeRestInfo sri;
 
-	if (!result)
+	bool res = rest_info(psq->shape,psq->transform,psq->motion,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask);
-		return Variant();
+	Dictionary r(true);
+	if (!res)
+		return r;
 
-	Dictionary d(true);
+	r["point"]=sri.point;
-	d["point"]=mcc.point;
+	r["normal"]=sri.normal;
-	d["normal"]=mcc.normal;
+	r["rid"]=sri.rid;
-	d["rid"]=mcc.rid;
+	r["collider_id"]=sri.collider_id;
-	d["collider_id"]=mcc.collider_id;
+	r["shape"]=sri.shape;
-	d["collider"]=mcc.collider;
+	r["linear_velocity"]=sri.linear_velocity;
-	d["shape"]=mcc.shape;
-
-	return d;
-#endif
-	return Variant();
 
+	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_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:Physics2DShapeQueryResult","shape","xform","result_max","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+	ObjectTypeDB::bind_method(_MD("intersect_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(32));
-	ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
+	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 );
+
 
 }
 

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

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

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

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

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

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

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 {
 

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

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

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

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())
+	bool part_of_subscene=false;
-		return;
+
+	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 (part_of_subscene) {
-	if (marked.has(p_node)) {
+
+		//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);
+
 }
 
 

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

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

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

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