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!

7 files changed, 800 insertions, 120 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index ecd147afde..47d78399b6 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -803,7 +803,7 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	//print_line("margin: "+rtos(margin));
 	do {
 
-		//fill exclude list..
+		//motion recover
 		for(int i=0;i<get_shape_count();i++) {
 
 
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 721fd368e1..f5e3ad66ee 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -57,160 +57,111 @@ float PhysicsBody::get_inverse_mass() const {
 	return 0;
 }
 
-PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) {
-
 
+void PhysicsBody::set_layer_mask(uint32_t p_mask) {
 
+	layer_mask=p_mask;
+	PhysicsServer::get_singleton()->body_set_layer_mask(get_rid(),p_mask);
 }
 
-void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) {
-
-	constant_linear_velocity=p_vel;
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity);
+uint32_t PhysicsBody::get_layer_mask() const {
 
+	return layer_mask;
 }
 
-void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) {
-
-	constant_angular_velocity=p_vel;
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity);
+void PhysicsBody::_bind_methods() {
+	ObjectTypeDB::bind_method(_MD("set_layer_mask","mask"),&PhysicsBody::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsBody::get_layer_mask);
+	ADD_PROPERTY(PropertyInfo(Variant::INT,"layers",PROPERTY_HINT_ALL_FLAGS),_SCS("set_layer_mask"),_SCS("get_layer_mask"));
 }
 
-Vector3 StaticBody::get_constant_linear_velocity() const {
-
-	return constant_linear_velocity;
-}
-Vector3 StaticBody::get_constant_angular_velocity() const {
-
-	return constant_angular_velocity;
-}
 
+PhysicsBody::PhysicsBody(PhysicsServer::BodyMode p_mode) : CollisionObject( PhysicsServer::get_singleton()->body_create(p_mode), false) {
 
-void StaticBody::_state_notify(Object *p_object) {
+	layer_mask=1;
 
-	if (!pre_xform)
-		return;
+}
 
-	PhysicsDirectBodyState *p2d = (PhysicsDirectBodyState*)p_object;
-	setting=true;
 
-	Transform new_xform = p2d->get_transform();
-	*pre_xform=new_xform;
-	set_ignore_transform_notification(true);
-	set_global_transform(new_xform);
-	set_ignore_transform_notification(false);
+void StaticBody::set_friction(real_t p_friction){
 
-	setting=false;
+	ERR_FAIL_COND(p_friction<0 || p_friction>1);
 
+	friction=p_friction;
+	PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_FRICTION,friction);
 
 }
+real_t StaticBody::get_friction() const{
 
-void StaticBody::_update_xform() {
-
-	if (!pre_xform || !pending)
-		return;
-
-	setting=true;
-
-
-	Transform new_xform = get_global_transform(); //obtain the new one
+	return friction;
+}
 
-	//set_block_transform_notify(true);
-	set_ignore_transform_notification(true);
-	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_TRANSFORM,*pre_xform); //then simulate motion!
-	set_global_transform(*pre_xform); //but restore state to previous one in both visual and physics
-	set_ignore_transform_notification(false);
+void StaticBody::set_bounce(real_t p_bounce){
 
-	PhysicsServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion!
+	ERR_FAIL_COND(p_bounce<0 || p_bounce>1);
 
-	setting=false;
-	pending=false;
+	bounce=p_bounce;
+	PhysicsServer::get_singleton()->body_set_param(get_rid(),PhysicsServer::BODY_PARAM_BOUNCE,bounce);
 
 }
+real_t StaticBody::get_bounce() const{
 
-void StaticBody::_notification(int p_what) {
-
-	switch(p_what) {
-
-		case NOTIFICATION_ENTER_SCENE: {
-
-			if (pre_xform)
-				*pre_xform = get_global_transform();
-			pending=false;
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
+	return bounce;
+}
 
-			if (simulating_motion && !pending && is_inside_scene() && !setting && !get_scene()->is_editor_hint()) {
+void StaticBody::set_constant_linear_velocity(const Vector3& p_vel) {
 
-				call_deferred(SceneStringNames::get_singleton()->_update_xform);
-				pending=true;
-			}
+	constant_linear_velocity=p_vel;
+	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_LINEAR_VELOCITY,constant_linear_velocity);
 
-		} break;
-	}
+}
 
+void StaticBody::set_constant_angular_velocity(const Vector3& p_vel) {
 
+	constant_angular_velocity=p_vel;
+	PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_ANGULAR_VELOCITY,constant_angular_velocity);
 }
 
-void StaticBody::set_simulate_motion(bool p_enable) {
-
-	if (p_enable==simulating_motion)
-		return;
-	simulating_motion=p_enable;
+Vector3 StaticBody::get_constant_linear_velocity() const {
 
-	if (p_enable) {
-		pre_xform = memnew( Transform );
-		if (is_inside_scene())
-			*pre_xform=get_transform();
-//		query = PhysicsServer::get_singleton()->query_create(this,"_state_notify",Variant());
-	//	PhysicsServer::get_singleton()->query_body_direct_state(query,get_rid());
-		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_state_notify");
+	return constant_linear_velocity;
+}
+Vector3 StaticBody::get_constant_angular_velocity() const {
 
-	} else {
-		memdelete( pre_xform );
-		pre_xform=NULL;
-		PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),NULL,StringName());
-		pending=false;
-	}
+	return constant_angular_velocity;
 }
 
-bool StaticBody::is_simulating_motion() const {
 
-	return simulating_motion;
-}
 
 void StaticBody::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("set_simulate_motion","enabled"),&StaticBody::set_simulate_motion);
-	ObjectTypeDB::bind_method(_MD("is_simulating_motion"),&StaticBody::is_simulating_motion);
-	ObjectTypeDB::bind_method(_MD("_update_xform"),&StaticBody::_update_xform);
-	ObjectTypeDB::bind_method(_MD("_state_notify"),&StaticBody::_state_notify);
 	ObjectTypeDB::bind_method(_MD("set_constant_linear_velocity","vel"),&StaticBody::set_constant_linear_velocity);
 	ObjectTypeDB::bind_method(_MD("set_constant_angular_velocity","vel"),&StaticBody::set_constant_angular_velocity);
 	ObjectTypeDB::bind_method(_MD("get_constant_linear_velocity"),&StaticBody::get_constant_linear_velocity);
 	ObjectTypeDB::bind_method(_MD("get_constant_angular_velocity"),&StaticBody::get_constant_angular_velocity);
 
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"simulate_motion"),_SCS("set_simulate_motion"),_SCS("is_simulating_motion"));
+	ObjectTypeDB::bind_method(_MD("set_friction","friction"),&StaticBody::set_friction);
+	ObjectTypeDB::bind_method(_MD("get_friction"),&StaticBody::get_friction);
+
+	ObjectTypeDB::bind_method(_MD("set_bounce","bounce"),&StaticBody::set_bounce);
+	ObjectTypeDB::bind_method(_MD("get_bounce"),&StaticBody::get_bounce);
+
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"friction",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"bounce",PROPERTY_HINT_RANGE,"0,1,0.01"),_SCS("set_bounce"),_SCS("get_bounce"));
+
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_linear_velocity"),_SCS("set_constant_linear_velocity"),_SCS("get_constant_linear_velocity"));
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3,"constant_angular_velocity"),_SCS("set_constant_angular_velocity"),_SCS("get_constant_angular_velocity"));
 }
 
 StaticBody::StaticBody() : PhysicsBody(PhysicsServer::BODY_MODE_STATIC) {
 
-	simulating_motion=false;
-	pre_xform=NULL;
-	setting=false;
-	pending=false;
-	//constant_angular_velocity=0;
-
+	bounce=0;
+	friction=1;
 }
 
 StaticBody::~StaticBody() {
 
-	if (pre_xform)
-		memdelete(pre_xform);
-	//if (query.is_valid())
-	//	PhysicsServer::get_singleton()->free(query);
+
 }
 
 
@@ -768,4 +719,418 @@ RigidBody::~RigidBody() {
 
 
 }
+//////////////////////////////////////////////////////
+//////////////////////////
+
+
+Variant KinematicBody::_get_collider() const {
+
+	ObjectID oid=get_collider();
+	if (oid==0)
+		return Variant();
+	Object *obj = ObjectDB::get_instance(oid);
+	if (!obj)
+		return Variant();
+
+	Reference *ref = obj->cast_to<Reference>();
+	if (ref) {
+		return Ref<Reference>(ref);
+	}
+
+	return obj;
+}
+
+
+bool KinematicBody::_ignores_mode(PhysicsServer::BodyMode p_mode) const {
+
+	switch(p_mode) {
+		case PhysicsServer::BODY_MODE_STATIC: return !collide_static;
+		case PhysicsServer::BODY_MODE_KINEMATIC: return !collide_kinematic;
+		case PhysicsServer::BODY_MODE_RIGID: return !collide_rigid;
+		case PhysicsServer::BODY_MODE_CHARACTER: return !collide_character;
+	}
+
+	return true;
+}
+
+Vector3 KinematicBody::move(const Vector3& p_motion) {
+
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+
+	colliding=false;
+	ERR_FAIL_COND_V(!is_inside_scene(),Vector3());
+	PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
+	ERR_FAIL_COND_V(!dss,Vector3());
+	const int max_shapes=32;
+	Vector3 sr[max_shapes*2];
+	int res_shapes;
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+
+	//recover first
+	int recover_attempts=4;
+
+	bool collided=false;
+	uint32_t mask=0;
+	if (collide_static)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
+	if (collide_kinematic)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
+	if (collide_rigid)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
+	if (collide_character)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
+
+//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
+
+	//print_line("margin: "+rtos(margin));
+
+	float m = margin;
+	//m=0.001;
+
+	do {
+
+		//motion recover
+		for(int i=0;i<get_shape_count();i++) {
+
+
+			if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),m,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) {
+				collided=true;
+			}
+
+		}
+
+
+
+		if (!collided)
+			break;
+
+		//print_line("have to recover");
+		Vector3 recover_motion;
+		bool all_outside=true;
+		for(int j=0;j<8;j++) {
+			for(int i=0;i<res_shapes;i++) {
+
+				Vector3 a = sr[i*2+0];
+				Vector3 b = sr[i*2+1];
+				//print_line(String()+a+" -> "+b);
+#if 0
+				float d = a.distance_to(b);
+
+				//if (d<margin)
+				///	continue;
+	   ///
+	   ///
+				recover_motion+=(b-a)*0.2;
+#else
+				float dist = a.distance_to(b);
+				if (dist>CMP_EPSILON) {
+					Vector3 norm = (b-a).normalized();
+					if (dist>margin*0.5)
+						all_outside=false;
+					float adv = norm.dot(recover_motion);
+					//print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv));
+					recover_motion+=norm*MAX(dist-adv,0)*0.4;
+				}
+#endif
+
+			}
+		}
+
+
+		if (recover_motion==Vector3()) {
+			collided=false;
+			break;
+		}
+
+		//print_line("**** RECOVER: "+recover_motion);
+
+		Transform gt = get_global_transform();
+		gt.origin+=recover_motion;
+		set_global_transform(gt);
+
+		recover_attempts--;
+
+		if (all_outside)
+			break;
+
+	} while (recover_attempts);
+
+
+	//move second
+	float safe = 1.0;
+	float unsafe = 1.0;
+	int best_shape=-1;
+
+	PhysicsDirectSpaceState::ShapeRestInfo rest;
+
+	//print_line("pos: "+get_global_transform().origin);
+	//print_line("motion: "+p_motion);
+
+
+	for(int i=0;i<get_shape_count();i++) {
+
+
+
+		float lsafe,lunsafe;
+		PhysicsDirectSpaceState::ShapeRestInfo lrest;
+		bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion,0, lsafe,lunsafe,exclude,get_layer_mask(),mask,&lrest);
+		//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
+		if (!valid) {
+			safe=0;
+			unsafe=0;
+			best_shape=i; //sadly it's the best
+			//print_line("initial stuck");
+
+			break;
+		}
+		if (lsafe==1.0) {
+			//print_line("initial free");
+			continue;
+		}
+		if (lsafe < safe) {
+
+			//print_line("initial at "+rtos(lsafe));
+			safe=lsafe;
+			safe=MAX(0,lsafe-0.01);
+			unsafe=lunsafe;
+			best_shape=i;
+			rest=lrest;
+		}
+	}
+
+
+	//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
+
+	if (safe>=1) {
+		//not collided
+		colliding=false;
+	} else {
+
+		colliding=true;
+
+		if (true || (safe==0 && unsafe==0)) { //use it always because it's more precise than GJK
+			//no advance, use rest info from collision
+			Transform ugt = get_global_transform();
+			ugt.origin+=p_motion*unsafe;
+
+			PhysicsDirectSpaceState::ShapeRestInfo rest_info;
+			bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), m,&rest,exclude,get_layer_mask(),mask);
+			if (!c2) {
+				//should not happen, but floating point precision is so weird..
+				colliding=false;
+			}
+
+		//	print_line("Rest Travel: "+rest.normal);
+
+		}
+
+		if (colliding) {
+
+			collision=rest.point;
+			normal=rest.normal;
+			collider=rest.collider_id;
+			collider_vel=rest.linear_velocity;
+		}
+	}
+
+	Vector3 motion=p_motion*safe;
+	//if (colliding)
+	//	motion+=normal*0.001;
+	Transform gt = get_global_transform();
+	gt.origin+=motion;
+	set_global_transform(gt);
+
+	return p_motion-motion;
+
+}
+
+Vector3 KinematicBody::move_to(const Vector3& p_position) {
+
+	return move(p_position-get_global_transform().origin);
+}
+
+bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+	PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
+	ERR_FAIL_COND_V(!dss,false);
+
+	uint32_t mask=0;
+	if (collide_static)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
+	if (collide_kinematic)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
+	if (collide_rigid)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
+	if (collide_character)
+		mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
+
+	Vector3 motion = p_position-get_global_transform().origin;
+	Transform xform=get_global_transform();
+
+	if (true || p_discrete) {
+
+		xform.origin+=motion;
+		motion=Vector3();
+	}
+
+	Set<RID> exclude;
+	exclude.insert(get_rid());
+
+	//fill exclude list..
+	for(int i=0;i<get_shape_count();i++) {
+
+
+		bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask);
+		if (col)
+			return false;
+	}
+
+	return true;
+}
+
+bool KinematicBody::is_colliding() const {
+
+	ERR_FAIL_COND_V(!is_inside_scene(),false);
+
+	return colliding;
+}
+Vector3 KinematicBody::get_collision_pos() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector3());
+	return collision;
+
+}
+Vector3 KinematicBody::get_collision_normal() const {
+
+	ERR_FAIL_COND_V(!colliding,Vector3());
+	return normal;
+
+}
+
+Vector3 KinematicBody::get_collider_velocity() const {
+
+	return collider_vel;
+}
+
+ObjectID KinematicBody::get_collider() const {
+
+	ERR_FAIL_COND_V(!colliding,0);
+	return collider;
+}
+
+void KinematicBody::set_collide_with_static_bodies(bool p_enable) {
+
+	collide_static=p_enable;
+}
+bool KinematicBody::can_collide_with_static_bodies() const {
+
+	return collide_static;
+}
+
+void KinematicBody::set_collide_with_rigid_bodies(bool p_enable) {
+
+	collide_rigid=p_enable;
+
+}
+bool KinematicBody::can_collide_with_rigid_bodies() const {
+
+
+	return collide_rigid;
+}
+
+void KinematicBody::set_collide_with_kinematic_bodies(bool p_enable) {
+
+	collide_kinematic=p_enable;
+
+}
+bool KinematicBody::can_collide_with_kinematic_bodies() const {
+
+	return collide_kinematic;
+}
+
+void KinematicBody::set_collide_with_character_bodies(bool p_enable) {
+
+	collide_character=p_enable;
+}
+bool KinematicBody::can_collide_with_character_bodies() const {
+
+	return collide_character;
+}
+
+void KinematicBody::set_collision_margin(float p_margin) {
+
+	margin=p_margin;
+}
+
+float KinematicBody::get_collision_margin() const{
+
+	return margin;
+}
+
+void KinematicBody::_bind_methods() {
+
+
+	ObjectTypeDB::bind_method(_MD("move","rel_vec"),&KinematicBody::move);
+	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody::move_to);
+
+	ObjectTypeDB::bind_method(_MD("can_move_to","position"),&KinematicBody::can_move_to);
+
+	ObjectTypeDB::bind_method(_MD("is_colliding"),&KinematicBody::is_colliding);
+
+	ObjectTypeDB::bind_method(_MD("get_collision_pos"),&KinematicBody::get_collision_pos);
+	ObjectTypeDB::bind_method(_MD("get_collision_normal"),&KinematicBody::get_collision_normal);
+	ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&KinematicBody::get_collider_velocity);
+	ObjectTypeDB::bind_method(_MD("get_collider:Object"),&KinematicBody::_get_collider);
+
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_static_bodies","enable"),&KinematicBody::set_collide_with_static_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_static_bodies"),&KinematicBody::can_collide_with_static_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_kinematic_bodies","enable"),&KinematicBody::set_collide_with_kinematic_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_kinematic_bodies"),&KinematicBody::can_collide_with_kinematic_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_rigid_bodies","enable"),&KinematicBody::set_collide_with_rigid_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_rigid_bodies"),&KinematicBody::can_collide_with_rigid_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collide_with_character_bodies","enable"),&KinematicBody::set_collide_with_character_bodies);
+	ObjectTypeDB::bind_method(_MD("can_collide_with_character_bodies"),&KinematicBody::can_collide_with_character_bodies);
+
+	ObjectTypeDB::bind_method(_MD("set_collision_margin","pixels"),&KinematicBody::set_collision_margin);
+	ObjectTypeDB::bind_method(_MD("get_collision_margin","pixels"),&KinematicBody::get_collision_margin);
+
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/static"),_SCS("set_collide_with_static_bodies"),_SCS("can_collide_with_static_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/kinematic"),_SCS("set_collide_with_kinematic_bodies"),_SCS("can_collide_with_kinematic_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/rigid"),_SCS("set_collide_with_rigid_bodies"),_SCS("can_collide_with_rigid_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"collide_with/character"),_SCS("set_collide_with_character_bodies"),_SCS("can_collide_with_character_bodies"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"collision/margin",PROPERTY_HINT_RANGE,"0.001,256,0.001"),_SCS("set_collision_margin"),_SCS("get_collision_margin"));
+
+
+}
+
+KinematicBody::KinematicBody() : PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC){
+
+	collide_static=true;
+	collide_rigid=true;
+	collide_kinematic=true;
+	collide_character=true;
+
+	colliding=false;
+	collider=0;
+	margin=0.001;
+}
+KinematicBody::~KinematicBody()  {
+
+
+}
+
 
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 616288e1f6..0b7a389449 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -38,8 +38,10 @@ class PhysicsBody : public CollisionObject {
 
 	OBJ_TYPE(PhysicsBody,CollisionObject);
 
+	uint32_t layer_mask;
 protected:
 
+	static void _bind_methods();
 	void _notification(int p_what);
 	PhysicsBody(PhysicsServer::BodyMode p_mode);
 public:
@@ -48,6 +50,10 @@ public:
 	virtual Vector3 get_angular_velocity() const;
 	virtual float get_inverse_mass() const;
 
+	void set_layer_mask(uint32_t p_mask);
+	uint32_t get_layer_mask() const;
+
+
 	PhysicsBody();
 
 };
@@ -56,25 +62,26 @@ class StaticBody : public PhysicsBody {
 
 	OBJ_TYPE(StaticBody,PhysicsBody);
 
-	Transform *pre_xform;
-	//RID query;
-	bool setting;
-	bool pending;
-	bool simulating_motion;
 	Vector3 constant_linear_velocity;
 	Vector3 constant_angular_velocity;
-	void _update_xform();
-	void _state_notify(Object *p_object);
+
+	real_t bounce;
+	real_t friction;
+
 
 protected:
 
-	void _notification(int p_what);
 	static void _bind_methods();
 
 public:
 
-	void set_simulate_motion(bool p_enable);
-	bool is_simulating_motion() const;
+
+	void set_friction(real_t p_friction);
+	real_t get_friction() const;
+
+	void set_bounce(real_t p_bounce);
+	real_t get_bounce() const;
+
 
 	void set_constant_linear_velocity(const Vector3& p_vel);
 	void set_constant_angular_velocity(const Vector3& p_vel);
@@ -237,4 +244,70 @@ public:
 
 VARIANT_ENUM_CAST(RigidBody::Mode);
 VARIANT_ENUM_CAST(RigidBody::AxisLock);
+
+
+
+
+
+class KinematicBody : public PhysicsBody {
+
+	OBJ_TYPE(KinematicBody,PhysicsBody);
+
+	float margin;
+	bool collide_static;
+	bool collide_rigid;
+	bool collide_kinematic;
+	bool collide_character;
+
+	bool colliding;
+	Vector3 collision;
+	Vector3 normal;
+	Vector3 collider_vel;
+	ObjectID collider;
+
+
+	Variant _get_collider() const;
+
+	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
+protected:
+
+	static void _bind_methods();
+public:
+
+	enum {
+		SLIDE_FLAG_FLOOR,
+		SLIDE_FLAG_WALL,
+		SLIDE_FLAG_ROOF
+	};
+
+	Vector3 move(const Vector3& p_motion);
+	Vector3 move_to(const Vector3& p_position);
+
+	bool can_move_to(const Vector3& p_position,bool p_discrete=false);
+	bool is_colliding() const;
+	Vector3 get_collision_pos() const;
+	Vector3 get_collision_normal() const;
+	Vector3 get_collider_velocity() const;
+	ObjectID get_collider() const;
+
+	void set_collide_with_static_bodies(bool p_enable);
+	bool can_collide_with_static_bodies() const;
+
+	void set_collide_with_rigid_bodies(bool p_enable);
+	bool can_collide_with_rigid_bodies() const;
+
+	void set_collide_with_kinematic_bodies(bool p_enable);
+	bool can_collide_with_kinematic_bodies() const;
+
+	void set_collide_with_character_bodies(bool p_enable);
+	bool can_collide_with_character_bodies() const;
+
+	void set_collision_margin(float p_margin);
+	float get_collision_margin() const;
+
+	KinematicBody();
+	~KinematicBody();
+
+};
+
 #endif // PHYSICS_BODY__H
diff --git a/scene/3d/vehicle_body.cpp b/scene/3d/vehicle_body.cpp
index 7680c1d56c..07abd1dcd2 100644
--- a/scene/3d/vehicle_body.cpp
+++ b/scene/3d/vehicle_body.cpp
@@ -125,19 +125,161 @@ void VehicleWheel::_update(PhysicsDirectBodyState *s) {
 	}
 }
 
+void VehicleWheel::set_radius(float p_radius) {
+
+	m_wheelRadius=p_radius;
+	update_gizmo();
+}
+
+float VehicleWheel::get_radius() const{
+
+	return m_wheelRadius;
+}
+
+void VehicleWheel::set_suspension_rest_length(float p_length){
+
+	m_suspensionRestLength=p_length;
+	update_gizmo();
+}
+float VehicleWheel::get_suspension_rest_length() const{
+
+	return m_suspensionRestLength;
+}
+
+void VehicleWheel::set_suspension_travel(float p_length){
+
+	m_maxSuspensionTravelCm=p_length/0.01;
+}
+float VehicleWheel::get_suspension_travel() const{
+
+	return m_maxSuspensionTravelCm*0.01;
+}
+
+void VehicleWheel::set_suspension_stiffness(float p_value){
+
+	m_suspensionStiffness=p_value;
+}
+float VehicleWheel::get_suspension_stiffness() const{
+
+	return m_suspensionStiffness;
+}
+
+void VehicleWheel::set_suspension_max_force(float p_value){
+
+	m_maxSuspensionForce=p_value;
+}
+float VehicleWheel::get_suspension_max_force() const{
+
+	return m_maxSuspensionForce;
+}
+
+void VehicleWheel::set_damping_compression(float p_value){
+
+	m_wheelsDampingCompression=p_value;
+}
+float VehicleWheel::get_damping_compression() const{
+
+	return m_wheelsDampingRelaxation;
+}
+
+void VehicleWheel::set_damping_relaxation(float p_value){
+
+	m_wheelsDampingRelaxation=p_value;
+}
+float VehicleWheel::get_damping_relaxation() const{
+
+	return m_wheelsDampingRelaxation;
+}
+
+void VehicleWheel::set_friction_slip(float p_value) {
+
+	m_frictionSlip=p_value;
+}
+float VehicleWheel::get_friction_slip() const{
+
+	return m_frictionSlip;
+}
+
+
 void VehicleWheel::_bind_methods() {
 
 
+	ObjectTypeDB::bind_method(_MD("set_radius","length"),&VehicleWheel::set_radius);
+	ObjectTypeDB::bind_method(_MD("get_radius"),&VehicleWheel::get_radius);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_rest_length","length"),&VehicleWheel::set_suspension_rest_length);
+	ObjectTypeDB::bind_method(_MD("get_suspension_rest_length"),&VehicleWheel::get_suspension_rest_length);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_travel","length"),&VehicleWheel::set_suspension_travel);
+	ObjectTypeDB::bind_method(_MD("get_suspension_travel"),&VehicleWheel::get_suspension_travel);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_stiffness","length"),&VehicleWheel::set_suspension_stiffness);
+	ObjectTypeDB::bind_method(_MD("get_suspension_stiffness"),&VehicleWheel::get_suspension_stiffness);
+
+	ObjectTypeDB::bind_method(_MD("set_suspension_max_force","length"),&VehicleWheel::set_suspension_max_force);
+	ObjectTypeDB::bind_method(_MD("get_suspension_max_force"),&VehicleWheel::get_suspension_max_force);
+
 
+	ObjectTypeDB::bind_method(_MD("set_damping_compression","length"),&VehicleWheel::set_damping_compression);
+	ObjectTypeDB::bind_method(_MD("get_damping_compression"),&VehicleWheel::get_damping_compression);
+
+	ObjectTypeDB::bind_method(_MD("set_damping_relaxation","length"),&VehicleWheel::set_damping_relaxation);
+	ObjectTypeDB::bind_method(_MD("get_damping_relaxation"),&VehicleWheel::get_damping_relaxation);
+
+	ObjectTypeDB::bind_method(_MD("set_use_as_traction","enable"),&VehicleWheel::set_use_as_traction);
+	ObjectTypeDB::bind_method(_MD("is_used_as_traction"),&VehicleWheel::is_used_as_traction);
+
+	ObjectTypeDB::bind_method(_MD("set_use_as_steering","enable"),&VehicleWheel::set_use_as_steering);
+	ObjectTypeDB::bind_method(_MD("is_used_as_steering"),&VehicleWheel::is_used_as_steering);
+
+	ObjectTypeDB::bind_method(_MD("set_friction_slip","length"),&VehicleWheel::set_friction_slip);
+	ObjectTypeDB::bind_method(_MD("get_friction_slip"),&VehicleWheel::get_friction_slip);
+
+
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/traction"),_SCS("set_use_as_traction"),_SCS("is_used_as_traction"));
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL,"type/steering"),_SCS("set_use_as_steering"),_SCS("is_used_as_steering"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/radius"),_SCS("set_radius"),_SCS("get_radius"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/rest_length"),_SCS("set_suspension_rest_length"),_SCS("get_suspension_rest_length"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"wheel/friction_slip"),_SCS("set_friction_slip"),_SCS("get_friction_slip"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/travel"),_SCS("set_suspension_travel"),_SCS("get_suspension_travel"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/stiffness"),_SCS("set_suspension_stiffness"),_SCS("get_suspension_stiffness"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"suspension/max_force"),_SCS("set_suspension_max_force"),_SCS("get_suspension_max_force"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/compression"),_SCS("set_damping_compression"),_SCS("get_damping_compression"));
+	ADD_PROPERTY(PropertyInfo(Variant::REAL,"damping/relaxation"),_SCS("set_damping_relaxation"),_SCS("get_damping_relaxation"));
+
+}
+
+
+void VehicleWheel::set_use_as_traction(bool p_enable) {
+
+	engine_traction=p_enable;
+}
+
+bool VehicleWheel::is_used_as_traction() const{
+
+	return engine_traction;
+}
+
+
+void VehicleWheel::set_use_as_steering(bool p_enabled){
+
+	steers=p_enabled;
+}
+
+bool VehicleWheel::is_used_as_steering() const{
+
+	return steers;
 }
 
 
 VehicleWheel::VehicleWheel() {
 
 
+	steers=false;
+	engine_traction=false;
 
 	m_steering = real_t(0.);
-	m_engineForce = real_t(0.);
+	//m_engineForce = real_t(0.);
 	m_rotation = real_t(0.);
 	m_deltaRotation = real_t(0.);
 	m_brake = real_t(0.);
@@ -172,6 +314,7 @@ void VehicleBody::_update_wheel_transform(VehicleWheel& wheel ,PhysicsDirectBody
 	//}
 
 	wheel.m_raycastInfo.m_hardPointWS = chassisTrans.xform( wheel.m_chassisConnectionPointCS );
+	//wheel.m_raycastInfo.m_hardPointWS+=s->get_linear_velocity()*s->get_step();
 	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.get_basis().xform( wheel.m_wheelDirectionCS).normalized();
 	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.get_basis().xform( wheel.m_wheelAxleCS ).normalized();
 }
@@ -189,12 +332,16 @@ void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) {
 //	up.normalize();
 
 	//rotate around steering over de wheelAxleWS
-	real_t steering = wheel.m_steering;
+	real_t steering = wheel.steers?m_steeringValue:0.0;
+	//print_line(itos(p_idx)+": "+rtos(steering));
 
 	Matrix3 steeringMat(up,steering);
 
 	Matrix3 rotatingMat(right,-wheel.m_rotation);
 
+//	if (p_idx==1)
+//		print_line("steeringMat " +steeringMat);
+
 	Matrix3 basis2(
 		right[0],up[0],fwd[0],
 		right[1],up[1],fwd[1],
@@ -202,9 +349,11 @@ void VehicleBody::_update_wheel(int p_idx,PhysicsDirectBodyState *s) {
 	);
 
 	wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2);
+	//wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat));
 	wheel.m_worldTransform.set_origin(
 		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
 	);
+
 }
 
 
@@ -221,9 +370,10 @@ real_t VehicleBody::_ray_cast(int p_idx,PhysicsDirectBodyState *s) {
 	real_t raylen = wheel.m_suspensionRestLength+wheel.m_wheelRadius;
 
 	Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
-	const Vector3& source = wheel.m_raycastInfo.m_hardPointWS;
+	Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
 	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
 	const Vector3& target = wheel.m_raycastInfo.m_contactPointWS;
+	source-=wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS;
 
 	real_t param = real_t(0.);
 
@@ -552,9 +702,10 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) {
 
 				//const btTransform& wheelTrans = getWheelTransformWS( i );
 
-				Matrix3 wheelBasis0 = wheelInfo.get_global_transform().basis;
+				Matrix3 wheelBasis0 = wheelInfo.m_worldTransform.basis;//get_global_transform().basis;
+
 				m_axle[i] = wheelBasis0.get_axis(Vector3::AXIS_X);
-				m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
+				//m_axle[i] = wheelInfo.m_raycastInfo.m_wheelAxleWS;
 
 				const Vector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
 				real_t proj = m_axle[i].dot(surfNormalWS);
@@ -592,9 +743,9 @@ void VehicleBody::_update_friction(PhysicsDirectBodyState *s) {
 
 			if (wheelInfo.m_raycastInfo.m_isInContact)
 			{
-				if (wheelInfo.m_engineForce != 0.f)
+				if (engine_force != 0.f)
 				{
-					rollingFriction = wheelInfo.m_engineForce* s->get_step();
+					rollingFriction = engine_force* s->get_step();
 				} else
 				{
 					real_t defaultRollingFrictionImpulse = 0.f;
@@ -721,9 +872,11 @@ void VehicleBody::_direct_state_changed(Object *p_state) {
 		_update_wheel(i,s);
 	}
 
+
 	for(int i=0;i<wheels.size();i++) {
 
 		_ray_cast(i,s);
+		wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform);
 	}
 
 	_update_suspension(s);
@@ -808,6 +961,35 @@ real_t VehicleBody::get_friction() const{
 	return friction;
 }
 
+void VehicleBody::set_engine_force(float p_force) {
+
+	engine_force=p_force;
+}
+
+float VehicleBody::get_engine_force() const{
+
+	return engine_force;
+}
+
+void VehicleBody::set_brake(float p_brake){
+
+	brake=p_brake;
+}
+float VehicleBody::get_brake() const{
+
+	return brake;
+}
+
+void VehicleBody::set_steering(float p_steering){
+
+	m_steeringValue=p_steering;
+}
+float VehicleBody::get_steering() const{
+
+	return m_steeringValue;
+}
+
+
 void VehicleBody::_bind_methods(){
 
 	ObjectTypeDB::bind_method(_MD("set_mass","mass"),&VehicleBody::set_mass);
@@ -816,8 +998,20 @@ void VehicleBody::_bind_methods(){
 	ObjectTypeDB::bind_method(_MD("set_friction","friction"),&VehicleBody::set_friction);
 	ObjectTypeDB::bind_method(_MD("get_friction"),&VehicleBody::get_friction);
 
+	ObjectTypeDB::bind_method(_MD("set_engine_force","engine_force"),&VehicleBody::set_engine_force);
+	ObjectTypeDB::bind_method(_MD("get_engine_force"),&VehicleBody::get_engine_force);
+
+	ObjectTypeDB::bind_method(_MD("set_brake","brake"),&VehicleBody::set_brake);
+	ObjectTypeDB::bind_method(_MD("get_brake"),&VehicleBody::get_brake);
+
+	ObjectTypeDB::bind_method(_MD("set_steering","steering"),&VehicleBody::set_steering);
+	ObjectTypeDB::bind_method(_MD("get_steering"),&VehicleBody::get_steering);
+
 	ObjectTypeDB::bind_method(_MD("_direct_state_changed"),&VehicleBody::_direct_state_changed);
 
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/engine_force",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_engine_force"),_SCS("get_engine_force"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/brake",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_brake"),_SCS("get_brake"));
+	ADD_PROPERTY( PropertyInfo(Variant::REAL,"motion/steering",PROPERTY_HINT_RANGE,"0.01,1024.0,0.01"),_SCS("set_steering"),_SCS("get_steering"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/mass",PROPERTY_HINT_RANGE,"0.01,65536,0.01"),_SCS("set_mass"),_SCS("get_mass"));
 	ADD_PROPERTY( PropertyInfo(Variant::REAL,"body/friction",PROPERTY_HINT_RANGE,"0.01,1,0.01"),_SCS("set_friction"),_SCS("get_friction"));
 
@@ -833,8 +1027,11 @@ VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
 	m_currentVehicleSpeedKmHour = real_t(0.);
 	m_steeringValue = real_t(0.);
 
+	engine_force=0;
+	brake=0;
+
+
 
-	mass=1;
 	friction=1;
 
 	ccd=false;
@@ -842,5 +1039,6 @@ VehicleBody::VehicleBody() : PhysicsBody(PhysicsServer::BODY_MODE_RIGID) {
 	exclude.insert(get_rid());
 	PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(),this,"_direct_state_changed");
 
+	set_mass(40);
 }
 
diff --git a/scene/3d/vehicle_body.h b/scene/3d/vehicle_body.h
index 97137da699..285cca142d 100644
--- a/scene/3d/vehicle_body.h
+++ b/scene/3d/vehicle_body.h
@@ -14,6 +14,8 @@ friend class VehicleBody;
 
 	Transform m_worldTransform;
 	Transform local_xform;
+	bool engine_traction;
+	bool steers;
 
 
 	Vector3	m_chassisConnectionPointCS; //const
@@ -39,7 +41,7 @@ friend class VehicleBody;
 	real_t	m_rotation;
 	real_t	m_deltaRotation;
 	real_t	m_rollInfluence;
-	real_t	m_engineForce;
+	//real_t	m_engineForce;
 	real_t	m_brake;
 
 	real_t	m_clippedInvContactDotSuspension;
@@ -69,6 +71,35 @@ protected:
 
 public:
 
+	void set_radius(float p_radius);
+	float get_radius() const;
+
+	void set_suspension_rest_length(float p_length);
+	float get_suspension_rest_length() const;
+
+	void set_suspension_travel(float p_length);
+	float get_suspension_travel() const;
+
+	void set_suspension_stiffness(float p_value);
+	float get_suspension_stiffness() const;
+
+	void set_suspension_max_force(float p_value);
+	float get_suspension_max_force() const;
+
+	void set_damping_compression(float p_value);
+	float get_damping_compression() const;
+
+	void set_damping_relaxation(float p_value);
+	float get_damping_relaxation() const;
+
+	void set_friction_slip(float p_value);
+	float get_friction_slip() const;
+
+	void set_use_as_traction(bool p_enable);
+	bool is_used_as_traction() const;
+
+	void set_use_as_steering(bool p_enabled);
+	bool is_used_as_steering() const;
 
 	VehicleWheel();
 
@@ -82,6 +113,9 @@ class VehicleBody : public PhysicsBody {
 	real_t mass;
 	real_t friction;
 
+	float engine_force;
+	float brake;
+
 	Vector3 linear_velocity;
 	Vector3  angular_velocity;
 	bool ccd;
@@ -135,6 +169,15 @@ public:
 	void set_friction(real_t p_friction);
 	real_t get_friction() const;
 
+	void set_engine_force(float p_engine_force);
+	float get_engine_force() const;
+
+	void set_brake(float p_force);
+	float get_brake() const;
+
+	void set_steering(float p_steering);
+	float get_steering() const;
+
 
 	VehicleBody();
 };
diff --git a/scene/gui/tree.cpp b/scene/gui/tree.cpp
index fb85f0c6b7..25f04379ef 100644
--- a/scene/gui/tree.cpp
+++ b/scene/gui/tree.cpp
@@ -2787,7 +2787,7 @@ int Tree::get_item_offset(TreeItem *p_item) const {
 
 		ofs+=compute_item_height(it)+cache.vseparation;
 
-		if (it->childs) {
+		if (it->childs && !it->collapsed) {
 
 			it=it->childs;
 
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index f7d6a246e6..02fed25883 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -401,6 +401,7 @@ void register_scene_types() {
 	ObjectTypeDB::register_virtual_type<CollisionObject>();
 	ObjectTypeDB::register_type<StaticBody>();
 	ObjectTypeDB::register_type<RigidBody>();
+	ObjectTypeDB::register_type<KinematicBody>();
 	ObjectTypeDB::register_type<CarBody>();
 	ObjectTypeDB::register_type<CarWheel>();
 	ObjectTypeDB::register_type<VehicleBody>();