-improved physics ccd

-html5 exporter works again
-disable repeat on image loader by default
-can change shape offset en tileset, texture offset was broken

2 files changed, 604 insertions, 604 deletions

diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 06ae34098c..094f56a421 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -198,7 +198,7 @@ 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_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) {
+	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;
 	}
 
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index b926a93773..f0f72b471c 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -26,606 +26,606 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#include "body_sw.h"

-#include "space_sw.h"

-#include "area_sw.h"

-

-void BodySW::_update_inertia() {

-

-	if (get_space() && !inertia_update_list.in_list())

-		get_space()->body_add_to_inertia_update_list(&inertia_update_list);

-

-}

-

-

-void BodySW::_update_inertia_tensor() {

-

-	Matrix3 tb = get_transform().basis;

-	tb.scale(_inv_inertia);

-	_inv_inertia_tensor = tb * get_transform().basis.transposed();

-

-}

-

-void BodySW::update_inertias() {

-

-	//update shapes and motions

-

-	switch(mode) {

-

-		case PhysicsServer::BODY_MODE_RIGID: {

-

-			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)

-			float total_area=0;

-

-			for (int i=0;i<get_shape_count();i++) {

-

-				total_area+=get_shape_aabb(i).get_area();

-			}

-

-			Vector3 _inertia;

-

-

-			for (int i=0;i<get_shape_count();i++) {

-

-				const ShapeSW* shape=get_shape(i);

-

-				float area=get_shape_aabb(i).get_area();

-

-				float mass = area * this->mass / total_area;

-

-				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();

-

-			}

-

-			if (_inertia!=Vector3())

-				_inv_inertia=_inertia.inverse();

-			else

-				_inv_inertia=Vector3();

-

-			if (mass)

-				_inv_mass=1.0/mass;

-			else

-				_inv_mass=0;

-

-		} break;

-

-		case PhysicsServer::BODY_MODE_STATIC_ACTIVE:

-		case PhysicsServer::BODY_MODE_STATIC: {

-

-			_inv_inertia=Vector3();

-			_inv_mass=0;

-		} break;

-		case PhysicsServer::BODY_MODE_CHARACTER: {

-

-			_inv_inertia=Vector3();

-			_inv_mass=1.0/mass;

-

-		} break;

-	}

-	_update_inertia_tensor();

-

-	//_update_shapes();

-

-}

-

-

-

-void BodySW::set_active(bool p_active) {

-

-	if (active==p_active)

-		return;

-

-	active=p_active;

-	if (!p_active) {

-		if (get_space())

-			get_space()->body_remove_from_active_list(&active_list);

-	} else {

-		if (mode==PhysicsServer::BODY_MODE_STATIC)

-			return; //static bodies can't become active

-		if (get_space())

-			get_space()->body_add_to_active_list(&active_list);

-

-		//still_time=0;

-	}

-/*

-	if (!space)

-		return;

-

-	for(int i=0;i<get_shape_count();i++) {

-		Shape &s=shapes[i];

-		if (s.bpid>0) {

-			get_space()->get_broadphase()->set_active(s.bpid,active);

-		}

-	}

-*/

-}

-

-

-

-void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {

-

-	switch(p_param) {

-		case PhysicsServer::BODY_PARAM_BOUNCE: {

-

-			bounce=p_value;

-		} break;

-		case PhysicsServer::BODY_PARAM_FRICTION: {

-

-			friction=p_value;

-		} break;

-		case PhysicsServer::BODY_PARAM_MASS: {

-			ERR_FAIL_COND(p_value<=0);

-			mass=p_value;

-			_update_inertia();

-

-		} break;

-		default:{}

-	}

-}

-

-float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {

-

-	switch(p_param) {

-		case PhysicsServer::BODY_PARAM_BOUNCE: {

-

-			return bounce;

-		} break;

-		case PhysicsServer::BODY_PARAM_FRICTION: {

-

-			return friction;

-		} break;

-		case PhysicsServer::BODY_PARAM_MASS: {

-			return mass;

-		} break;

-		default:{}

-	}

-

-	return 0;

-}

-

-void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {

-

-	mode=p_mode;

-

-	switch(p_mode) {

-		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!

-		case PhysicsServer::BODY_MODE_STATIC:

-		case PhysicsServer::BODY_MODE_STATIC_ACTIVE: {

-

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

-			linear_velocity=Vector3();

-			angular_velocity=Vector3();

-		} break;

-		case PhysicsServer::BODY_MODE_RIGID: {

-

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

-	}

-

-	_update_inertia();

-	//if (get_space())

-//		_update_queries();

-

-}

-PhysicsServer::BodyMode BodySW::get_mode() const {

-

-	return mode;

-}

-

-void BodySW::_shapes_changed() {

-

-	_update_inertia();

-}

-

-void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {

-

-	switch(p_state)	{

-		case PhysicsServer::BODY_STATE_TRANSFORM: {

-

-

-			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {

-				_set_transform(p_variant);

-				_set_inv_transform(get_transform().affine_inverse());

-				wakeup_neighbours();

-			} else {

-				Transform t = p_variant;

-				t.orthonormalize();

-				_set_transform(t);

-				_set_inv_transform(get_transform().inverse());

-

-			}

-

-		} break;

-		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {

-

-			//if (mode==PhysicsServer::BODY_MODE_STATIC)

-			//	break;

-			linear_velocity=p_variant;

-		} break;

-		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {

-			//if (mode!=PhysicsServer::BODY_MODE_RIGID)

-			//	break;

-			angular_velocity=p_variant;

-

-		} break;

-		case PhysicsServer::BODY_STATE_SLEEPING: {

-			//?

-			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

-				break;

-			bool do_sleep=p_variant;

-			if (do_sleep) {

-				linear_velocity=Vector3();

-				//biased_linear_velocity=Vector3();

-				angular_velocity=Vector3();

-				//biased_angular_velocity=Vector3();

-				set_active(false);

-			} else {

-				if (mode!=PhysicsServer::BODY_MODE_STATIC)

-					set_active(true);

-			}

-		} break;

-		case PhysicsServer::BODY_STATE_CAN_SLEEP: {

-			can_sleep=p_variant;

-			if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)

-				set_active(true);

-

-		} break;

-	}

-

-}

-Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {

-

-	switch(p_state)	{

-		case PhysicsServer::BODY_STATE_TRANSFORM: {

-			return get_transform();

-		} break;

-		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {

-			return linear_velocity;

-		} break;

-		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {

-			return angular_velocity;

-		} break;

-		case PhysicsServer::BODY_STATE_SLEEPING: {

-			return !is_active();

-		} break;

-		case PhysicsServer::BODY_STATE_CAN_SLEEP: {

-			return can_sleep;

-		} break;

-	}

-

-	return Variant();

-}

-

-

-void BodySW::set_space(SpaceSW *p_space){

-

-	if (get_space()) {

-

-		if (inertia_update_list.in_list())

-			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);

-		if (active_list.in_list())

-			get_space()->body_remove_from_active_list(&active_list);

-		if (direct_state_query_list.in_list())

-			get_space()->body_remove_from_state_query_list(&direct_state_query_list);

-

-	}

-

-	_set_space(p_space);

-

-	if (get_space()) {

-

-		_update_inertia();

-		if (active)

-			get_space()->body_add_to_active_list(&active_list);

-//		_update_queries();

-		//if (is_active()) {

-		//	active=false;

-		//	set_active(true);

-		//}

-

-	}

-

-}

-

-void BodySW::_compute_area_gravity(const AreaSW *p_area) {

-

-	if (p_area->is_gravity_point()) {

-

-		gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();

-

-	} else {

-		gravity = p_area->get_gravity_vector() * p_area->get_gravity();

-	}

-}

-

-void BodySW::integrate_forces(real_t p_step) {

-

-

-	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

-		return;

-

-	AreaSW *current_area = get_space()->get_default_area();

-	ERR_FAIL_COND(!current_area);

-

-	int prio = current_area->get_priority();

-	int ac = areas.size();

-	if (ac) {

-		const AreaCMP *aa = &areas[0];

-		for(int i=0;i<ac;i++) {

-			if (aa[i].area->get_priority() > prio) {

-				current_area=aa[i].area;

-				prio=current_area->get_priority();

-			}

-		}

-	}

-

-	_compute_area_gravity(current_area);

-	density=current_area->get_density();

-

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

-	}

-

-	applied_force=Vector3();

-	applied_torque=Vector3();

-

-	//motion=linear_velocity*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);

-

-	current_area=NULL; // clear the area, so it is set in the next frame

-	contact_count=0;

-

-}

-

-void BodySW::integrate_velocities(real_t p_step) {

-

-	if (mode==PhysicsServer::BODY_MODE_STATIC)

-		return;

-

-	if (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {

-		if (fi_callback)

-			get_space()->body_add_to_state_query_list(&direct_state_query_list);

-		return;

-	}

-

-	Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;

-

-

-

-	float ang_vel = total_angular_velocity.length();

-	Transform transform = get_transform();

-

-

-	if (ang_vel!=0.0) {

-		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;

-		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );

-		transform.basis = rot * transform.basis;

-		transform.orthonormalize();

-	}

-

-	Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;

-

-

-	transform.origin+=total_linear_velocity * p_step;

-

-	_set_transform(transform);

-	_set_inv_transform(get_transform().inverse());

-

-	_update_inertia_tensor();

-

-	if (fi_callback) {

-

-		get_space()->body_add_to_state_query_list(&direct_state_query_list);

-	}

-

-}

-

-

-void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {

-

-	Transform inv_xform = p_xform.affine_inverse();

-	if (!get_space()) {

-		_set_transform(p_xform);

-		_set_inv_transform(inv_xform);

-

-		return;

-	}

-

-	//compute a FAKE linear velocity - this is easy

-

-	linear_velocity=(p_xform.origin - get_transform().origin)/p_step;

-

-	//compute a FAKE angular velocity, not so easy

-	Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();

-	Vector3 axis;

-	float angle;

-

-	rot.get_axis_and_angle(axis,angle);

-	axis.normalize();

-	angular_velocity=axis.normalized() * (angle/p_step);

-	linear_velocity = (p_xform.origin - get_transform().origin)/p_step;

-

-	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))

-		get_space()->body_add_to_state_query_list(&direct_state_query_list);

-	simulated_motion=true;

-	_set_transform(p_xform);

-

-

-}

-

-void BodySW::wakeup_neighbours() {

-

-	for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {

-

-		const ConstraintSW *c=E->key();

-		BodySW **n = c->get_body_ptr();

-		int bc=c->get_body_count();

-

-		for(int i=0;i<bc;i++) {

-

-			if (i==E->get())

-				continue;

-			BodySW *b = n[i];

-			if (b->mode!=PhysicsServer::BODY_MODE_RIGID)

-				continue;

-

-			if (!b->is_active())

-				b->set_active(true);

-		}

-	}

-}

-

-void BodySW::call_queries() {

-

-

-	if (fi_callback) {

-

-		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;

-		dbs->body=this;

-

-		Variant v=dbs;

-

-		Object *obj = ObjectDB::get_instance(fi_callback->id);

-		if (!obj) {

-

-			set_force_integration_callback(0,StringName());

-		} else {

-			const Variant *vp[2]={&v,&fi_callback->udata};

-

-			Variant::CallError ce;

-			int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;

-			obj->call(fi_callback->method,vp,argc,ce);

-		}

-

-

-	}

-

-	if (simulated_motion) {

-

-	//	linear_velocity=Vector3();

-	//	angular_velocity=0;

-		simulated_motion=false;

-	}

-}

-

-

-bool BodySW::sleep_test(real_t p_step)  {

-

-	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)

-		return true; //

-	else if (mode==PhysicsServer::BODY_MODE_CHARACTER)

-		return !active; // characters don't sleep unless asked to sleep

-	else if (!can_sleep)

-		return false;

-

-

-

-

-	if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {

-

-		still_time+=p_step;

-

-		return still_time > get_space()->get_body_time_to_sleep();

-	} else {

-

-		still_time=0; //maybe this should be set to 0 on set_active?

-		return false;

-	}

-}

-

-

-void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {

-

-	if (fi_callback) {

-

-		memdelete(fi_callback);

-		fi_callback=NULL;

-	}

-

-

-	if (p_id!=0) {

-

-		fi_callback=memnew(ForceIntegrationCallback);

-		fi_callback->id=p_id;

-		fi_callback->method=p_method;

-		fi_callback->udata=p_udata;

-	}

-

-}

-

-BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {

-

-

-	mode=PhysicsServer::BODY_MODE_RIGID;

-	active=true;

-

-	mass=1;

-//	_inv_inertia=Transform();

-	_inv_mass=1;

-	bounce=0;

-	friction=1;

-	omit_force_integration=false;

-//	applied_torque=0;

-	island_step=0;

-	island_next=NULL;

-	island_list_next=NULL;

-	_set_static(false);

-	density=0;

-	contact_count=0;

-	simulated_motion=false;

-	still_time=0;

-	continuous_cd=false;

-	can_sleep=false;

-	fi_callback=NULL;

-

-}

-

-BodySW::~BodySW() {

-

-	if (fi_callback)

-		memdelete(fi_callback);

-}

-

-PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;

-

-PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {

-

-	return body->get_space()->get_direct_state();

-}

+#include "body_sw.h"
+#include "space_sw.h"
+#include "area_sw.h"
+
+void BodySW::_update_inertia() {
+
+	if (get_space() && !inertia_update_list.in_list())
+		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+
+}
+
+
+void BodySW::_update_inertia_tensor() {
+
+	Matrix3 tb = get_transform().basis;
+	tb.scale(_inv_inertia);
+	_inv_inertia_tensor = tb * get_transform().basis.transposed();
+
+}
+
+void BodySW::update_inertias() {
+
+	//update shapes and motions
+
+	switch(mode) {
+
+		case PhysicsServer::BODY_MODE_RIGID: {
+
+			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+			float total_area=0;
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				total_area+=get_shape_aabb(i).get_area();
+			}
+
+			Vector3 _inertia;
+
+
+			for (int i=0;i<get_shape_count();i++) {
+
+				const ShapeSW* shape=get_shape(i);
+
+				float area=get_shape_aabb(i).get_area();
+
+				float mass = area * this->mass / total_area;
+
+				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();
+
+			}
+
+			if (_inertia!=Vector3())
+				_inv_inertia=_inertia.inverse();
+			else
+				_inv_inertia=Vector3();
+
+			if (mass)
+				_inv_mass=1.0/mass;
+			else
+				_inv_mass=0;
+
+		} break;
+
+		case PhysicsServer::BODY_MODE_KINEMATIC:
+		case PhysicsServer::BODY_MODE_STATIC: {
+
+			_inv_inertia=Vector3();
+			_inv_mass=0;
+		} break;
+		case PhysicsServer::BODY_MODE_CHARACTER: {
+
+			_inv_inertia=Vector3();
+			_inv_mass=1.0/mass;
+
+		} break;
+	}
+	_update_inertia_tensor();
+
+	//_update_shapes();
+
+}
+
+
+
+void BodySW::set_active(bool p_active) {
+
+	if (active==p_active)
+		return;
+
+	active=p_active;
+	if (!p_active) {
+		if (get_space())
+			get_space()->body_remove_from_active_list(&active_list);
+	} else {
+		if (mode==PhysicsServer::BODY_MODE_STATIC)
+			return; //static bodies can't become active
+		if (get_space())
+			get_space()->body_add_to_active_list(&active_list);
+
+		//still_time=0;
+	}
+/*
+	if (!space)
+		return;
+
+	for(int i=0;i<get_shape_count();i++) {
+		Shape &s=shapes[i];
+		if (s.bpid>0) {
+			get_space()->get_broadphase()->set_active(s.bpid,active);
+		}
+	}
+*/
+}
+
+
+
+void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {
+
+	switch(p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+			bounce=p_value;
+		} break;
+		case PhysicsServer::BODY_PARAM_FRICTION: {
+
+			friction=p_value;
+		} break;
+		case PhysicsServer::BODY_PARAM_MASS: {
+			ERR_FAIL_COND(p_value<=0);
+			mass=p_value;
+			_update_inertia();
+
+		} break;
+		default:{}
+	}
+}
+
+float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
+
+	switch(p_param) {
+		case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+			return bounce;
+		} break;
+		case PhysicsServer::BODY_PARAM_FRICTION: {
+
+			return friction;
+		} break;
+		case PhysicsServer::BODY_PARAM_MASS: {
+			return mass;
+		} break;
+		default:{}
+	}
+
+	return 0;
+}
+
+void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
+
+	mode=p_mode;
+
+	switch(p_mode) {
+		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+		case PhysicsServer::BODY_MODE_STATIC:
+		case PhysicsServer::BODY_MODE_KINEMATIC: {
+
+			_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);
+			linear_velocity=Vector3();
+			angular_velocity=Vector3();
+		} break;
+		case PhysicsServer::BODY_MODE_RIGID: {
+
+			_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;
+	}
+
+	_update_inertia();
+	//if (get_space())
+//		_update_queries();
+
+}
+PhysicsServer::BodyMode BodySW::get_mode() const {
+
+	return mode;
+}
+
+void BodySW::_shapes_changed() {
+
+	_update_inertia();
+}
+
+void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {
+
+	switch(p_state)	{
+		case PhysicsServer::BODY_STATE_TRANSFORM: {
+
+
+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+				_set_transform(p_variant);
+				_set_inv_transform(get_transform().affine_inverse());
+				wakeup_neighbours();
+			} else {
+				Transform t = p_variant;
+				t.orthonormalize();
+				_set_transform(t);
+				_set_inv_transform(get_transform().inverse());
+
+			}
+
+		} break;
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+
+			//if (mode==PhysicsServer::BODY_MODE_STATIC)
+			//	break;
+			linear_velocity=p_variant;
+		} break;
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+			//if (mode!=PhysicsServer::BODY_MODE_RIGID)
+			//	break;
+			angular_velocity=p_variant;
+
+		} break;
+		case PhysicsServer::BODY_STATE_SLEEPING: {
+			//?
+			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+				break;
+			bool do_sleep=p_variant;
+			if (do_sleep) {
+				linear_velocity=Vector3();
+				//biased_linear_velocity=Vector3();
+				angular_velocity=Vector3();
+				//biased_angular_velocity=Vector3();
+				set_active(false);
+			} else {
+				if (mode!=PhysicsServer::BODY_MODE_STATIC)
+					set_active(true);
+			}
+		} break;
+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+			can_sleep=p_variant;
+			if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
+				set_active(true);
+
+		} break;
+	}
+
+}
+Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {
+
+	switch(p_state)	{
+		case PhysicsServer::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		} break;
+		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+			return linear_velocity;
+		} break;
+		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+			return angular_velocity;
+		} break;
+		case PhysicsServer::BODY_STATE_SLEEPING: {
+			return !is_active();
+		} break;
+		case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+			return can_sleep;
+		} break;
+	}
+
+	return Variant();
+}
+
+
+void BodySW::set_space(SpaceSW *p_space){
+
+	if (get_space()) {
+
+		if (inertia_update_list.in_list())
+			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (active_list.in_list())
+			get_space()->body_remove_from_active_list(&active_list);
+		if (direct_state_query_list.in_list())
+			get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+
+	}
+
+	_set_space(p_space);
+
+	if (get_space()) {
+
+		_update_inertia();
+		if (active)
+			get_space()->body_add_to_active_list(&active_list);
+//		_update_queries();
+		//if (is_active()) {
+		//	active=false;
+		//	set_active(true);
+		//}
+
+	}
+
+}
+
+void BodySW::_compute_area_gravity(const AreaSW *p_area) {
+
+	if (p_area->is_gravity_point()) {
+
+		gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();
+
+	} else {
+		gravity = p_area->get_gravity_vector() * p_area->get_gravity();
+	}
+}
+
+void BodySW::integrate_forces(real_t p_step) {
+
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+		return;
+
+	AreaSW *current_area = get_space()->get_default_area();
+	ERR_FAIL_COND(!current_area);
+
+	int prio = current_area->get_priority();
+	int ac = areas.size();
+	if (ac) {
+		const AreaCMP *aa = &areas[0];
+		for(int i=0;i<ac;i++) {
+			if (aa[i].area->get_priority() > prio) {
+				current_area=aa[i].area;
+				prio=current_area->get_priority();
+			}
+		}
+	}
+
+	_compute_area_gravity(current_area);
+	density=current_area->get_density();
+
+	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;
+	}
+
+	applied_force=Vector3();
+	applied_torque=Vector3();
+
+	//motion=linear_velocity*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);
+
+	current_area=NULL; // clear the area, so it is set in the next frame
+	contact_count=0;
+
+}
+
+void BodySW::integrate_velocities(real_t p_step) {
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC)
+		return;
+
+	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+		if (fi_callback)
+			get_space()->body_add_to_state_query_list(&direct_state_query_list);
+		return;
+	}
+
+	Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;
+
+
+
+	float ang_vel = total_angular_velocity.length();
+	Transform transform = get_transform();
+
+
+	if (ang_vel!=0.0) {
+		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
+		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );
+		transform.basis = rot * transform.basis;
+		transform.orthonormalize();
+	}
+
+	Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;
+
+
+	transform.origin+=total_linear_velocity * p_step;
+
+	_set_transform(transform);
+	_set_inv_transform(get_transform().inverse());
+
+	_update_inertia_tensor();
+
+	if (fi_callback) {
+
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	}
+
+}
+
+
+void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
+
+	Transform inv_xform = p_xform.affine_inverse();
+	if (!get_space()) {
+		_set_transform(p_xform);
+		_set_inv_transform(inv_xform);
+
+		return;
+	}
+
+	//compute a FAKE linear velocity - this is easy
+
+	linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
+
+	//compute a FAKE angular velocity, not so easy
+	Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
+	Vector3 axis;
+	float angle;
+
+	rot.get_axis_and_angle(axis,angle);
+	axis.normalize();
+	angular_velocity=axis.normalized() * (angle/p_step);
+	linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
+
+	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
+		get_space()->body_add_to_state_query_list(&direct_state_query_list);
+	simulated_motion=true;
+	_set_transform(p_xform);
+
+
+}
+
+void BodySW::wakeup_neighbours() {
+
+	for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+
+		const ConstraintSW *c=E->key();
+		BodySW **n = c->get_body_ptr();
+		int bc=c->get_body_count();
+
+		for(int i=0;i<bc;i++) {
+
+			if (i==E->get())
+				continue;
+			BodySW *b = n[i];
+			if (b->mode!=PhysicsServer::BODY_MODE_RIGID)
+				continue;
+
+			if (!b->is_active())
+				b->set_active(true);
+		}
+	}
+}
+
+void BodySW::call_queries() {
+
+
+	if (fi_callback) {
+
+		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
+		dbs->body=this;
+
+		Variant v=dbs;
+
+		Object *obj = ObjectDB::get_instance(fi_callback->id);
+		if (!obj) {
+
+			set_force_integration_callback(0,StringName());
+		} else {
+			const Variant *vp[2]={&v,&fi_callback->udata};
+
+			Variant::CallError ce;
+			int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;
+			obj->call(fi_callback->method,vp,argc,ce);
+		}
+
+
+	}
+
+	if (simulated_motion) {
+
+	//	linear_velocity=Vector3();
+	//	angular_velocity=0;
+		simulated_motion=false;
+	}
+}
+
+
+bool BodySW::sleep_test(real_t p_step)  {
+
+	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+		return true; //
+	else if (mode==PhysicsServer::BODY_MODE_CHARACTER)
+		return !active; // characters don't sleep unless asked to sleep
+	else if (!can_sleep)
+		return false;
+
+
+
+
+	if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
+
+		still_time+=p_step;
+
+		return still_time > get_space()->get_body_time_to_sleep();
+	} else {
+
+		still_time=0; //maybe this should be set to 0 on set_active?
+		return false;
+	}
+}
+
+
+void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+
+	if (fi_callback) {
+
+		memdelete(fi_callback);
+		fi_callback=NULL;
+	}
+
+
+	if (p_id!=0) {
+
+		fi_callback=memnew(ForceIntegrationCallback);
+		fi_callback->id=p_id;
+		fi_callback->method=p_method;
+		fi_callback->udata=p_udata;
+	}
+
+}
+
+BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
+
+
+	mode=PhysicsServer::BODY_MODE_RIGID;
+	active=true;
+
+	mass=1;
+//	_inv_inertia=Transform();
+	_inv_mass=1;
+	bounce=0;
+	friction=1;
+	omit_force_integration=false;
+//	applied_torque=0;
+	island_step=0;
+	island_next=NULL;
+	island_list_next=NULL;
+	_set_static(false);
+	density=0;
+	contact_count=0;
+	simulated_motion=false;
+	still_time=0;
+	continuous_cd=false;
+	can_sleep=false;
+	fi_callback=NULL;
+
+}
+
+BodySW::~BodySW() {
+
+	if (fi_callback)
+		memdelete(fi_callback);
+}
+
+PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;
+
+PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {
+
+	return body->get_space()->get_direct_state();
+}