1 files changed, 26 insertions, 2 deletions

diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 8923899278..6317186d5f 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -71,11 +71,12 @@ class BodySW : public CollisionObjectSW {
 	VSet<RID> exceptions;
 	bool omit_force_integration;
 	bool active;
-	bool simulated_motion;
+
 	bool continuous_cd;
 	bool can_sleep;
 	void _update_inertia();
 	virtual void _shapes_changed();
+	Transform new_transform;
 
 	Map<ConstraintSW*,int> constraint_map;
 
@@ -235,7 +236,29 @@ 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);
+	_FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const {
+
+		return linear_velocity + angular_velocity.cross(rel_pos);
+	}
+
+	_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const {
+
+		 Vector3 r0 = p_pos - get_transform().origin;
+
+		 Vector3 c0 = (r0).cross(p_normal);
+
+		 Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
+
+		 return _inv_mass + p_normal.dot(vec);
+
+	 }
+
+	_FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3& p_axis) const {
+
+		return p_axis.dot( _inv_inertia_tensor.xform_inv(p_axis) );
+	 }
+
+	//void simulate_motion(const Transform& p_xform,real_t p_step);
 	void call_queries();
 	void wakeup_neighbours();
 
@@ -323,6 +346,7 @@ public:
 	virtual Transform get_transform() const {  return body->get_transform();  }
 
 	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 set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
 	virtual bool is_sleeping() const {  return !body->is_active();  }