Fixed inertia tensor computation and center of mass

14 files changed, 144 insertions, 70 deletions

diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 630d8e3d50..7defa87bb1 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -307,8 +307,8 @@ bool BodyPairSW::setup(float p_step) {
 		}
 #endif
 
-		c.rA = global_A;
-		c.rB = global_B-offset_B;
+		c.rA = global_A-A->get_center_of_mass();
+		c.rB = global_B-B->get_center_of_mass()-offset_B;
 
 		// contact query reporting...
 #if 0
@@ -364,12 +364,12 @@ bool BodyPairSW::setup(float p_step) {
 		c.depth=depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		A->apply_impulse( c.rA, -j_vec );
-		B->apply_impulse( c.rB, j_vec );
+		A->apply_impulse( c.rA+A->get_center_of_mass(), -j_vec );
+		B->apply_impulse( c.rB+B->get_center_of_mass(), j_vec );
 		c.acc_bias_impulse=0;
 		Vector3 jb_vec = c.normal * c.acc_bias_impulse;
-		A->apply_bias_impulse( c.rA, -jb_vec );
-		B->apply_bias_impulse( c.rB, jb_vec );
+		A->apply_bias_impulse( c.rA+A->get_center_of_mass(), -jb_vec );
+		B->apply_bias_impulse( c.rB+B->get_center_of_mass(), jb_vec );
 
 		c.bounce = MAX(A->get_bounce(),B->get_bounce());
 		if (c.bounce) {
@@ -418,8 +418,8 @@ void BodyPairSW::solve(float p_step) {
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
 
-			A->apply_bias_impulse(c.rA,-jb);
-			B->apply_bias_impulse(c.rB, jb);
+			A->apply_bias_impulse(c.rA+A->get_center_of_mass(),-jb);
+			B->apply_bias_impulse(c.rB+B->get_center_of_mass(), jb);
 
 			c.active=true;
 		}
@@ -442,8 +442,8 @@ void BodyPairSW::solve(float p_step) {
 			Vector3 j =c.normal * (c.acc_normal_impulse - jnOld);
 
 
-			A->apply_impulse(c.rA,-j);
-			B->apply_impulse(c.rB, j);
+			A->apply_impulse(c.rA+A->get_center_of_mass(),-j);
+			B->apply_impulse(c.rB+B->get_center_of_mass(), j);
 
 			c.active=true;
 		}
@@ -489,8 +489,8 @@ void BodyPairSW::solve(float p_step) {
 			jt = c.acc_tangent_impulse - jtOld;
 
 
-			A->apply_impulse( c.rA, -jt );
-			B->apply_impulse( c.rB, jt );
+			A->apply_impulse( c.rA+A->get_center_of_mass(), -jt );
+			B->apply_impulse( c.rB+B->get_center_of_mass(), jt );
 
 			c.active=true;
 
diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h
index 4962c78dac..a37f75d13c 100644
--- a/servers/physics/body_pair_sw.h
+++ b/servers/physics/body_pair_sw.h
@@ -65,7 +65,7 @@ class BodyPairSW : public ConstraintSW {
 
 		real_t depth;
 		bool active;
-		Vector3 rA,rB;
+		Vector3 rA,rB; // Offset in world orientation with respect to center of mass
 	};
 
 	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 74bba4f977..2f859a4ed4 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -37,12 +37,16 @@ void BodySW::_update_inertia() {
 
 }
 
-
-void BodySW::_update_inertia_tensor() {
-
-	Matrix3 tb = get_transform().basis;
+void BodySW::_update_transform_dependant() {
+	
+	center_of_mass = get_transform().basis.xform(center_of_mass_local);
+	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
+
+	// update inertia tensor
+	Matrix3 tb = principal_inertia_axes;
+	Matrix3 tbt = tb.transposed();
 	tb.scale(_inv_inertia);
-	_inv_inertia_tensor = tb * get_transform().basis.transposed();
+	_inv_inertia_tensor = tb * tbt;
 
 }
 
@@ -54,33 +58,56 @@ void BodySW::update_inertias() {
 
 		case PhysicsServer::BODY_MODE_RIGID: {
 
-			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+			//update tensor for all shapes, 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();
+				total_area+=get_shape_area(i);
 			}
 
-			Vector3 _inertia;
+			// We have to recompute the center of mass
+			center_of_mass_local.zero();
+
+			for (int i=0; i<get_shape_count(); i++) {
+				float area=get_shape_area(i);
+
+				float mass = area * this->mass / total_area;
+
+				// NOTE: we assume that the shape origin is also its center of mass
+				center_of_mass_local += mass * get_shape_transform(i).origin;
+			}
 
+			center_of_mass_local /= mass;
+
+			// Recompute the inertia tensor
+			Matrix3 inertia_tensor;
+			inertia_tensor.set_zero();
 
 			for (int i=0;i<get_shape_count();i++) {
 
 				const ShapeSW* shape=get_shape(i);
 
-				float area=get_shape_aabb(i).get_area();
+				float area=get_shape_area(i);
 
 				float mass = area * this->mass / total_area;
 
-				_inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();
+				Matrix3 shape_inertia_tensor=shape->get_moment_of_inertia(mass).to_diagonal_matrix();
+				Transform shape_transform=get_shape_transform(i);
+				Matrix3 shape_basis = shape_transform.basis.orthonormalized();
+
+				// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
+				shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
+
+				Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
+				inertia_tensor += shape_inertia_tensor + (Matrix3()*shape_origin.dot(shape_origin)-shape_origin.outer(shape_origin))*mass;
+
 
 			}
 
-			if (_inertia!=Vector3())
-				_inv_inertia=_inertia.inverse();
-			else
-				_inv_inertia=Vector3();
+			// Compute the principal axes of inertia
+			principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
+			_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
 
 			if (mass)
 				_inv_mass=1.0/mass;
@@ -92,20 +119,21 @@ void BodySW::update_inertias() {
 		case PhysicsServer::BODY_MODE_KINEMATIC:
 		case PhysicsServer::BODY_MODE_STATIC: {
 
-			_inv_inertia=Vector3();
+			_inv_inertia_tensor.set_zero();
 			_inv_mass=0;
 		} break;
 		case PhysicsServer::BODY_MODE_CHARACTER: {
 
-			_inv_inertia=Vector3();
+			_inv_inertia_tensor.set_zero();
 			_inv_mass=1.0/mass;
 
 		} break;
 	}
-	_update_inertia_tensor();
+
 
 	//_update_shapes();
 
+	_update_transform_dependant();
 }
 
 
@@ -582,6 +610,8 @@ void BodySW::integrate_velocities(real_t p_step) {
 	if (ang_vel!=0.0) {
 		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
 		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );
+		Matrix3 identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
+		transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
 		transform.basis = rot * transform.basis;
 		transform.orthonormalize();
 	}
@@ -598,7 +628,7 @@ void BodySW::integrate_velocities(real_t p_step) {
 	_set_transform(transform);
 	_set_inv_transform(get_transform().inverse());
 
-	_update_inertia_tensor();
+	_update_transform_dependant();
 
 	//if (fi_callback) {
 
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 9b28468b34..acdd84d259 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -57,8 +57,16 @@ class BodySW : public CollisionObjectSW {
 	PhysicsServer::BodyAxisLock axis_lock;
 
 	real_t _inv_mass;
-	Vector3 _inv_inertia;
+	Vector3 _inv_inertia; // Relative to the principal axes of inertia
+
+	// Relative to the local frame of reference
+	Matrix3 principal_inertia_axes_local;
+	Vector3 center_of_mass_local;
+
+	// In world orientation with local origin
 	Matrix3 _inv_inertia_tensor;
+	Matrix3 principal_inertia_axes;
+	Vector3 center_of_mass;
 
 	Vector3 gravity;
 
@@ -135,7 +143,7 @@ class BodySW : public CollisionObjectSW {
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const AreaSW *p_area);
 
-	_FORCE_INLINE_ void _update_inertia_tensor();
+	_FORCE_INLINE_ void _update_transform_dependant();
 
 friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
 
@@ -190,6 +198,10 @@ public:
 	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
 	_FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
 
+	_FORCE_INLINE_ Matrix3 get_principal_inertia_axes() const { return principal_inertia_axes; }
+	_FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
+	_FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3& p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } 
+
 	_FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }
 	_FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
 
@@ -202,18 +214,23 @@ public:
 	_FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) {
 
 		linear_velocity += p_j * _inv_mass;
-		angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+		angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) );
+	}
+
+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+
+		angular_velocity += _inv_inertia_tensor.xform(p_j);
 	}
 
 	_FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) {
 
 		biased_linear_velocity += p_j * _inv_mass;
-		biased_angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+		biased_angular_velocity += _inv_inertia_tensor.xform( (p_pos-center_of_mass).cross(p_j) );
 	}
 
-	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+	_FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3& p_j) {
 
-		angular_velocity += _inv_inertia_tensor.xform(p_j);
+		biased_angular_velocity += _inv_inertia_tensor.xform(p_j);
 	}
 
 	_FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) {
@@ -268,12 +285,12 @@ public:
 
 	_FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const {
 
-		return linear_velocity + angular_velocity.cross(rel_pos);
+		return linear_velocity + angular_velocity.cross(rel_pos-center_of_mass);
 	}
 
 	_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const {
 
-		 Vector3 r0 = p_pos - get_transform().origin;
+		 Vector3 r0 = p_pos - get_transform().origin - center_of_mass;
 
 		 Vector3 c0 = (r0).cross(p_normal);
 
@@ -363,6 +380,9 @@ public:
 	virtual float get_total_angular_damp() const {  return body->area_angular_damp;  } // get density of this body space/area
 	virtual float get_total_linear_damp() const {  return body->area_linear_damp;  } // get density of this body space/area
 
+	virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); }
+	virtual Matrix3 get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); }
+
 	virtual float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass
 	virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space
 	virtual Matrix3 get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor();   } // get density of this body space
@@ -378,6 +398,7 @@ public:
 
 	virtual void add_force(const Vector3& p_force, const Vector3& p_pos) {  body->add_force(p_force,p_pos); }
 	virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j) { body->apply_impulse(p_pos,p_j); }
+	virtual void apply_torque_impulse(const Vector3& p_j) { body->apply_torque_impulse(p_j); }
 
 	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
 	virtual bool is_sleeping() const {  return !body->is_active();  }
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index c45b76da22..2b2b73738a 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -144,6 +144,8 @@ void CollisionObjectSW::_update_shapes() {
 		s.aabb_cache=shape_aabb;
 		s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 );
 
+		Vector3 scale = xform.get_basis().get_scale();
+		s.area_cache=s.shape->get_area()*scale.x*scale.y*scale.z;
 
 		space->get_broadphase()->move(s.bpid,s.aabb_cache);
 	}
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index 5738a629dc..f3e43c98db 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -61,6 +61,7 @@ private:
 		Transform xform_inv;
 		BroadPhaseSW::ID bpid;
 		AABB aabb_cache; //for rayqueries
+		real_t area_cache;
 		ShapeSW *shape;
 		bool trigger;
 
@@ -123,6 +124,7 @@ public:
 	_FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
 	_FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
 	_FORCE_INLINE_ const AABB& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+	_FORCE_INLINE_ const real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }
 
 	_FORCE_INLINE_ Transform get_transform() const { return transform; }
 	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
index c94cc8bd8c..5036a1d8a3 100644
--- a/servers/physics/joints/cone_twist_joint_sw.cpp
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -128,10 +128,10 @@ bool	ConeTwistJointSW::setup(float p_step) {
 		for (int i=0;i<3;i++)
 		{
 			memnew_placement(&m_jac[i], JacobianEntrySW(
-				A->get_transform().basis.transposed(),
-				B->get_transform().basis.transposed(),
-				pivotAInW - A->get_transform().origin,
-				pivotBInW - B->get_transform().origin,
+				A->get_principal_inertia_axes().transposed(),
+				B->get_principal_inertia_axes().transposed(),
+				pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+				pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
 				normal[i],
 				A->get_inv_inertia(),
 				A->get_inv_mass(),
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index 4e25bebe55..3c0119971a 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -352,10 +352,10 @@ void Generic6DOFJointSW::buildLinearJacobian(
     const Vector3 & pivotAInW,const Vector3 & pivotBInW)
 {
    memnew_placement(&jacLinear, JacobianEntrySW(
-	A->get_transform().basis.transposed(),
-	B->get_transform().basis.transposed(),
-	pivotAInW - A->get_transform().origin,
-	pivotBInW - B->get_transform().origin,
+	A->get_principal_inertia_axes().transposed(),
+	B->get_principal_inertia_axes().transposed(),
+	pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+	pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
 	normalWorld,
 	A->get_inv_inertia(),
 	A->get_inv_mass(),
@@ -368,8 +368,8 @@ void Generic6DOFJointSW::buildAngularJacobian(
     JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
 {
     memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
-				      A->get_transform().basis.transposed(),
-				      B->get_transform().basis.transposed(),
+				      A->get_principal_inertia_axes().transposed(),
+				      B->get_principal_inertia_axes().transposed(),
 				      A->get_inv_inertia(),
 				      B->get_inv_inertia()));
 
@@ -440,7 +440,7 @@ bool Generic6DOFJointSW::setup(float p_step) {
 }
 
 
-void Generic6DOFJointSW::solve(real_t	timeStep)
+void Generic6DOFJointSW::solve(real_t timeStep)
 {
     m_timeStep = timeStep;
 
diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp
index f7e1df92b8..23e00a9e7f 100644
--- a/servers/physics/joints/hinge_joint_sw.cpp
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -173,10 +173,10 @@ bool HingeJointSW::setup(float p_step) {
 		for (int i=0;i<3;i++)
 		{
 			memnew_placement(&m_jac[i], JacobianEntrySW(
-				A->get_transform().basis.transposed(),
-				B->get_transform().basis.transposed(),
-				pivotAInW - A->get_transform().origin,
-				pivotBInW - B->get_transform().origin,
+				A->get_principal_inertia_axes().transposed(),
+				B->get_principal_inertia_axes().transposed(),
+				pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+				pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
 				normal[i],
 				A->get_inv_inertia(),
 				A->get_inv_mass(),
@@ -200,20 +200,20 @@ bool HingeJointSW::setup(float p_step) {
 	Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
 
 	memnew_placement(&m_jacAng[0],	JacobianEntrySW(jointAxis0,
-		A->get_transform().basis.transposed(),
-		B->get_transform().basis.transposed(),
+		A->get_principal_inertia_axes().transposed(),
+		B->get_principal_inertia_axes().transposed(),
 		A->get_inv_inertia(),
 		B->get_inv_inertia()));
 
 	memnew_placement(&m_jacAng[1],	JacobianEntrySW(jointAxis1,
-		A->get_transform().basis.transposed(),
-		B->get_transform().basis.transposed(),
+		A->get_principal_inertia_axes().transposed(),
+		B->get_principal_inertia_axes().transposed(),
 		A->get_inv_inertia(),
 		B->get_inv_inertia()));
 
 	memnew_placement(&m_jacAng[2],	JacobianEntrySW(hingeAxisWorld,
-		A->get_transform().basis.transposed(),
-		B->get_transform().basis.transposed(),
+		A->get_principal_inertia_axes().transposed(),
+		B->get_principal_inertia_axes().transposed(),
 		A->get_inv_inertia(),
 		B->get_inv_inertia()));
 
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
index 74d62e35fa..292d30443c 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -44,10 +44,10 @@ bool PinJointSW::setup(float p_step) {
 	{
 		normal[i] = 1;
 		memnew_placement(&m_jac[i],JacobianEntrySW(
-			A->get_transform().basis.transposed(),
-			B->get_transform().basis.transposed(),
-			A->get_transform().xform(m_pivotInA) - A->get_transform().origin,
-			B->get_transform().xform(m_pivotInB) - B->get_transform().origin,
+			A->get_principal_inertia_axes().transposed(),
+			B->get_principal_inertia_axes().transposed(),
+			A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+			B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
 			normal,
 			A->get_inv_inertia(),
 			A->get_inv_mass(),
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
index 7d1933e0df..bdcae08ca4 100644
--- a/servers/physics/joints/slider_joint_sw.cpp
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -132,10 +132,10 @@ bool SliderJointSW::setup(float p_step)
     {
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
 		memnew_placement(&m_jacLin[i], JacobianEntrySW(
-			A->get_transform().basis.transposed(),
-			B->get_transform().basis.transposed(),
-			m_relPosA,
-			m_relPosB,
+			A->get_principal_inertia_axes().transposed(),
+			B->get_principal_inertia_axes().transposed(),
+			m_relPosA - A->get_center_of_mass(),
+			m_relPosB - B->get_center_of_mass(),
 			normalWorld,
 			A->get_inv_inertia(),
 			A->get_inv_mass(),
@@ -152,8 +152,8 @@ bool SliderJointSW::setup(float p_step)
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
 		memnew_placement(&m_jacAng[i],	JacobianEntrySW(
 			normalWorld,
-	    A->get_transform().basis.transposed(),
-	    B->get_transform().basis.transposed(),
+	    A->get_principal_inertia_axes().transposed(),
+	    B->get_principal_inertia_axes().transposed(),
 	    A->get_inv_inertia(),
 	    B->get_inv_inertia()
 			));
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index ab54497eac..4069ccdccb 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -805,6 +805,15 @@ void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const
 	body->wakeup();
 };
 
+void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3& p_impulse) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->apply_torque_impulse(p_impulse);
+	body->wakeup();
+};
+
 void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) {
 
 	BodySW *body = body_owner.get(p_body);
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index bd9b5e4e30..a3f98392fc 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -193,6 +193,7 @@ public:
 	virtual Vector3 body_get_applied_torque(RID p_body) const;
 
 	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse);
+	virtual void body_apply_torque_impulse(RID p_body, const Vector3& p_impulse);
 	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity);
 
 	virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock);
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
index d031a9fb9b..e76237017d 100644
--- a/servers/physics/shape_sw.h
+++ b/servers/physics/shape_sw.h
@@ -73,6 +73,8 @@ public:
 		MAX_SUPPORTS=8
 	};
 
+	virtual real_t get_area() const { return aabb.get_area();}
+
 	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
 	_FORCE_INLINE_ RID get_self() const {return  self; }
 
@@ -128,6 +130,7 @@ public:
 
 	Plane get_plane() const;
 
+	virtual real_t get_area() const { return INFINITY; }
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
 	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3& p_normal) const;
@@ -152,6 +155,7 @@ public:
 
 	float get_length() const;
 
+	virtual real_t get_area() const { return 0.0; }
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
 	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3& p_normal) const;
@@ -176,6 +180,8 @@ public:
 
 	real_t get_radius() const;
 
+	virtual real_t get_area() const { return 4.0/3.0 * Math_PI * radius * radius * radius; }
+
 	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;
@@ -198,6 +204,7 @@ class BoxShapeSW : public ShapeSW {
 public:
 
 	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+	virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } 
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
 
@@ -226,6 +233,8 @@ public:
 	_FORCE_INLINE_ real_t get_height() const { return height; }
 	_FORCE_INLINE_ real_t get_radius() const { return radius; }
 
+	virtual real_t get_area() { return 4.0/3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
+
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
 
 	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;