Bring that Whole New World to the Old Continent too

Applies the clang-format style to the 2.1 branch as done for master in
5dbf1809c6e3e905b94b8764e99491e608122261.

80 files changed, 9258 insertions, 11242 deletions

diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index 1131aa90d..2ea58c2f9 100644
--- a/servers/physics/area_pair_sw.cpp
+++ b/servers/physics/area_pair_sw.cpp
@@ -29,7 +29,6 @@
 #include "area_pair_sw.h"
 #include "collision_solver_sw.h"
 
-
 bool AreaPairSW::setup(float p_step) {
 
 	if (!area->test_collision_mask(body)) {
@@ -37,63 +36,55 @@ bool AreaPairSW::setup(float p_step) {
 		return false;
 	}
 
-	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),NULL,this);
+	bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this);
 
-	if (result!=colliding) {
+	if (result != colliding) {
 
 		if (result) {
 
-			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+			if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
 				body->add_area(area);
 			if (area->has_monitor_callback())
-				area->add_body_to_query(body,body_shape,area_shape);
+				area->add_body_to_query(body, body_shape, area_shape);
 
 		} else {
 
-			if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+			if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
 				body->remove_area(area);
 			if (area->has_monitor_callback())
-				area->remove_body_from_query(body,body_shape,area_shape);
-
+				area->remove_body_from_query(body, body_shape, area_shape);
 		}
 
-		colliding=result;
-
+		colliding = result;
 	}
 
 	return false; //never do any post solving
 }
 
 void AreaPairSW::solve(float p_step) {
-
-
 }
 
+AreaPairSW::AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape) {
 
-AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) {
-
-	body=p_body;
-	area=p_area;
-	body_shape=p_body_shape;
-	area_shape=p_area_shape;
-	colliding=false;
-	body->add_constraint(this,0);
+	body = p_body;
+	area = p_area;
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+	colliding = false;
+	body->add_constraint(this, 0);
 	area->add_constraint(this);
-	if (p_body->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC)
+	if (p_body->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC)
 		p_body->set_active(true);
-
 }
 
 AreaPairSW::~AreaPairSW() {
 
 	if (colliding) {
 
-		if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
+		if (area->get_space_override_mode() != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)
 			body->remove_area(area);
 		if (area->has_monitor_callback())
-			area->remove_body_from_query(body,body_shape,area_shape);
-
-
+			area->remove_body_from_query(body, body_shape, area_shape);
 	}
 	body->remove_constraint(this);
 	area->remove_constraint(this);
@@ -101,8 +92,6 @@ AreaPairSW::~AreaPairSW() {
 
 ////////////////////////////////////////////////////
 
-
-
 bool Area2PairSW::setup(float p_step) {
 
 	if (!area_a->test_collision_mask(area_b)) {
@@ -110,52 +99,46 @@ bool Area2PairSW::setup(float p_step) {
 		return false;
 	}
 
-//	bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
-	bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),NULL,this);
+	//	bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
+	bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this);
 
-	if (result!=colliding) {
+	if (result != colliding) {
 
 		if (result) {
 
 			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-				area_b->add_area_to_query(area_a,shape_a,shape_b);
+				area_b->add_area_to_query(area_a, shape_a, shape_b);
 
 			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-				area_a->add_area_to_query(area_b,shape_b,shape_a);
+				area_a->add_area_to_query(area_b, shape_b, shape_a);
 
 		} else {
 
 			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-				area_b->remove_area_from_query(area_a,shape_a,shape_b);
+				area_b->remove_area_from_query(area_a, shape_a, shape_b);
 
 			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-				area_a->remove_area_from_query(area_b,shape_b,shape_a);
+				area_a->remove_area_from_query(area_b, shape_b, shape_a);
 		}
 
-		colliding=result;
-
+		colliding = result;
 	}
 
 	return false; //never do any post solving
 }
 
 void Area2PairSW::solve(float p_step) {
-
-
 }
 
+Area2PairSW::Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b) {
 
-Area2PairSW::Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *p_area_b,int p_shape_b) {
-
-
-	area_a=p_area_a;
-	area_b=p_area_b;
-	shape_a=p_shape_a;
-	shape_b=p_shape_b;
-	colliding=false;
+	area_a = p_area_a;
+	area_b = p_area_b;
+	shape_a = p_shape_a;
+	shape_b = p_shape_b;
+	colliding = false;
 	area_a->add_constraint(this);
 	area_b->add_constraint(this);
-
 }
 
 Area2PairSW::~Area2PairSW() {
@@ -163,10 +146,10 @@ Area2PairSW::~Area2PairSW() {
 	if (colliding) {
 
 		if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-			area_b->remove_area_from_query(area_a,shape_a,shape_b);
+			area_b->remove_area_from_query(area_a, shape_a, shape_b);
 
 		if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-			area_a->remove_area_from_query(area_b,shape_b,shape_a);
+			area_a->remove_area_from_query(area_b, shape_b, shape_a);
 	}
 
 	area_a->remove_constraint(this);
diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h
index 17477dcbd..e05040a36 100644
--- a/servers/physics/area_pair_sw.h
+++ b/servers/physics/area_pair_sw.h
@@ -29,9 +29,9 @@
 #ifndef AREA_PAIR_SW_H
 #define AREA_PAIR_SW_H
 
-#include "constraint_sw.h"
-#include "body_sw.h"
 #include "area_sw.h"
+#include "body_sw.h"
+#include "constraint_sw.h"
 
 class AreaPairSW : public ConstraintSW {
 
@@ -40,16 +40,15 @@ class AreaPairSW : public ConstraintSW {
 	int body_shape;
 	int area_shape;
 	bool colliding;
-public:
 
+public:
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape);
+	AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape);
 	~AreaPairSW();
 };
 
-
 class Area2PairSW : public ConstraintSW {
 
 	AreaSW *area_a;
@@ -57,15 +56,13 @@ class Area2PairSW : public ConstraintSW {
 	int shape_a;
 	int shape_b;
 	bool colliding;
-public:
 
+public:
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *p_area_b,int p_shape_b);
+	Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b);
 	~Area2PairSW();
 };
 
-
 #endif // AREA_PAIR__SW_H
-
diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp
index 84389c9b7..2b7e87155 100644
--- a/servers/physics/area_sw.cpp
+++ b/servers/physics/area_sw.cpp
@@ -27,18 +27,26 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 #include "area_sw.h"
-#include "space_sw.h"
 #include "body_sw.h"
+#include "space_sw.h"
 
-AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
-AreaSW::BodyKey::BodyKey(AreaSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
+AreaSW::BodyKey::BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
 
 void AreaSW::_shapes_changed() {
-
-
 }
 
-void AreaSW::set_transform(const Transform& p_transform) {
+void AreaSW::set_transform(const Transform &p_transform) {
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
@@ -54,7 +62,6 @@ void AreaSW::set_space(SpaceSW *p_space) {
 			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
 		if (moved_list.in_list())
 			get_space()->area_remove_from_moved_list(&moved_list);
-
 	}
 
 	monitored_bodies.clear();
@@ -63,44 +70,38 @@ void AreaSW::set_space(SpaceSW *p_space) {
 	_set_space(p_space);
 }
 
+void AreaSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
 
-void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
-
-
-	if (p_id==monitor_callback_id) {
-		monitor_callback_method=p_method;
+	if (p_id == monitor_callback_id) {
+		monitor_callback_method = p_method;
 		return;
 	}
 
 	_unregister_shapes();
 
-	monitor_callback_id=p_id;
-	monitor_callback_method=p_method;
+	monitor_callback_id = p_id;
+	monitor_callback_method = p_method;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
 
-
 	_shape_changed();
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
-
-
 }
 
-void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) {
+void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) {
 
-
-	if (p_id==area_monitor_callback_id) {
-		area_monitor_callback_method=p_method;
+	if (p_id == area_monitor_callback_id) {
+		area_monitor_callback_method = p_method;
 		return;
 	}
 
 	_unregister_shapes();
 
-	area_monitor_callback_id=p_id;
-	area_monitor_callback_method=p_method;
+	area_monitor_callback_id = p_id;
+	area_monitor_callback_method = p_method;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
@@ -109,45 +110,63 @@ void AreaSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
-
-
 }
 
-
 void AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) {
-	bool do_override=p_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
-	if (do_override==(space_override_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED))
+	bool do_override = p_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override == (space_override_mode != PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED))
 		return;
 	_unregister_shapes();
-	space_override_mode=p_mode;
+	space_override_mode = p_mode;
 	_shape_changed();
 }
 
-void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value) {
+void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value) {
 
-	switch(p_param) {
-		case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_LINEAR_DAMP: linear_damp=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: angular_damp=p_value; ; break;
-		case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	switch (p_param) {
+		case PhysicsServer::AREA_PARAM_GRAVITY:
+			gravity = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR:
+			gravity_vector = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT:
+			gravity_is_point = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+			gravity_distance_scale = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			point_attenuation = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_LINEAR_DAMP:
+			linear_damp = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP:
+			angular_damp = p_value;
+			;
+			break;
+		case PhysicsServer::AREA_PARAM_PRIORITY:
+			priority = p_value;
+			;
+			break;
 	}
-
-
 }
 
 Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const {
 
-
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::AREA_PARAM_GRAVITY: return gravity;
 		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
 		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
 		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale;
-		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation;
 		case PhysicsServer::AREA_PARAM_LINEAR_DAMP: return linear_damp;
 		case PhysicsServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp;
 		case PhysicsServer::AREA_PARAM_PRIORITY: return priority;
@@ -156,23 +175,20 @@ Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const {
 	return Variant();
 }
 
-
 void AreaSW::_queue_monitor_update() {
 
 	ERR_FAIL_COND(!get_space());
 
 	if (!monitor_query_list.in_list())
 		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
-
-
 }
 
 void AreaSW::set_monitorable(bool p_monitorable) {
 
-	if (monitorable==p_monitorable)
+	if (monitorable == p_monitorable)
 		return;
 
-	monitorable=p_monitorable;
+	monitorable = p_monitorable;
 	_set_static(!monitorable);
 }
 
@@ -182,29 +198,29 @@ void AreaSW::call_queries() {
 
 		Variant res[5];
 		Variant *resptr[5];
-		for(int i=0;i<5;i++)
-			resptr[i]=&res[i];
+		for (int i = 0; i < 5; i++)
+			resptr[i] = &res[i];
 
 		Object *obj = ObjectDB::get_instance(monitor_callback_id);
 		if (!obj) {
 			monitored_bodies.clear();
-			monitor_callback_id=0;
+			monitor_callback_id = 0;
 			return;
 		}
 
-		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
 
-			if (E->get().state==0)
+			if (E->get().state == 0)
 				continue; //nothing happened
 
-			res[0]=E->get().state>0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
-			res[1]=E->key().rid;
-			res[2]=E->key().instance_id;
-			res[3]=E->key().body_shape;
-			res[4]=E->key().area_shape;
+			res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
+			res[1] = E->key().rid;
+			res[2] = E->key().instance_id;
+			res[3] = E->key().body_shape;
+			res[4] = E->key().area_shape;
 
 			Variant::CallError ce;
-			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
@@ -212,64 +228,56 @@ void AreaSW::call_queries() {
 
 	if (area_monitor_callback_id && !monitored_areas.empty()) {
 
-
 		Variant res[5];
 		Variant *resptr[5];
-		for(int i=0;i<5;i++)
-			resptr[i]=&res[i];
+		for (int i = 0; i < 5; i++)
+			resptr[i] = &res[i];
 
 		Object *obj = ObjectDB::get_instance(area_monitor_callback_id);
 		if (!obj) {
 			monitored_areas.clear();
-			area_monitor_callback_id=0;
+			area_monitor_callback_id = 0;
 			return;
 		}
 
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
 
-
-		for (Map<BodyKey,BodyState>::Element *E=monitored_areas.front();E;E=E->next()) {
-
-			if (E->get().state==0)
+			if (E->get().state == 0)
 				continue; //nothing happened
 
-			res[0]=E->get().state>0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
-			res[1]=E->key().rid;
-			res[2]=E->key().instance_id;
-			res[3]=E->key().body_shape;
-			res[4]=E->key().area_shape;
-
+			res[0] = E->get().state > 0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED;
+			res[1] = E->key().rid;
+			res[2] = E->key().instance_id;
+			res[3] = E->key().body_shape;
+			res[4] = E->key().area_shape;
 
 			Variant::CallError ce;
-			obj->call(area_monitor_callback_method,(const Variant**)resptr,5,ce);
+			obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
 	monitored_areas.clear();
 	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
-
 }
 
-AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this),  moved_list(this) {
+AreaSW::AreaSW()
+	: CollisionObjectSW(TYPE_AREA), monitor_query_list(this), moved_list(this) {
 
 	_set_static(true); //areas are never active
-	space_override_mode=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
-	gravity=9.80665;
-	gravity_vector=Vector3(0,-1,0);
-	gravity_is_point=false;
-	gravity_distance_scale=0;
-	point_attenuation=1;
-	angular_damp=1.0;
-	linear_damp=0.1;
-	priority=0;
+	space_override_mode = PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity = 9.80665;
+	gravity_vector = Vector3(0, -1, 0);
+	gravity_is_point = false;
+	gravity_distance_scale = 0;
+	point_attenuation = 1;
+	angular_damp = 1.0;
+	linear_damp = 0.1;
+	priority = 0;
 	set_ray_pickable(false);
-	monitor_callback_id=0;
-	area_monitor_callback_id=0;
-	monitorable=false;
-
+	monitor_callback_id = 0;
+	area_monitor_callback_id = 0;
+	monitorable = false;
 }
 
 AreaSW::~AreaSW() {
-
-
 }
-
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index 5ac698540..0bc6c2eab 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -29,17 +29,16 @@
 #ifndef AREA_SW_H
 #define AREA_SW_H
 
-#include "servers/physics_server.h"
 #include "collision_object_sw.h"
 #include "self_list.h"
+#include "servers/physics_server.h"
 //#include "servers/physics/query_sw.h"
 
 class SpaceSW;
 class BodySW;
 class ConstraintSW;
 
-class AreaSW : public CollisionObjectSW{
-
+class AreaSW : public CollisionObjectSW {
 
 	PhysicsServer::AreaSpaceOverrideMode space_override_mode;
 	float gravity;
@@ -68,24 +67,22 @@ class AreaSW : public CollisionObjectSW{
 		uint32_t body_shape;
 		uint32_t area_shape;
 
-		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+		_FORCE_INLINE_ bool operator<(const BodyKey &p_key) const {
 
-			if (rid==p_key.rid) {
+			if (rid == p_key.rid) {
 
-				if (body_shape==p_key.body_shape) {
+				if (body_shape == p_key.body_shape) {
 
 					return area_shape < p_key.area_shape;
 				} else
 					return body_shape < p_key.body_shape;
 			} else
 				return rid < p_key.rid;
-
 		}
 
 		_FORCE_INLINE_ BodyKey() {}
-		BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
-		BodyKey(AreaSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
-
+		BodyKey(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 	};
 
 	struct BodyState {
@@ -93,125 +90,111 @@ class AreaSW : public CollisionObjectSW{
 		int state;
 		_FORCE_INLINE_ void inc() { state++; }
 		_FORCE_INLINE_ void dec() { state--; }
-		_FORCE_INLINE_ BodyState() { state=0; }
+		_FORCE_INLINE_ BodyState() { state = 0; }
 	};
 
-	Map<BodyKey,BodyState> monitored_bodies;
-	Map<BodyKey,BodyState> monitored_areas;
+	Map<BodyKey, BodyState> monitored_bodies;
+	Map<BodyKey, BodyState> monitored_areas;
 
 	//virtual void shape_changed_notify(ShapeSW *p_shape);
 	//virtual void shape_deleted_notify(ShapeSW *p_shape);
 
-	Set<ConstraintSW*> constraints;
-
+	Set<ConstraintSW *> constraints;
 
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
 public:
-
 	//_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; }
 	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
 
-	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
 
-	void set_area_monitor_callback(ObjectID p_id, const StringName& p_method);
+	void set_area_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id; }
 
-	_FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
-	_FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 
-	_FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
-	_FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
+	_FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
 
-	void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value);
+	void set_param(PhysicsServer::AreaParameter p_param, const Variant &p_value);
 	Variant get_param(PhysicsServer::AreaParameter p_param) const;
 
 	void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode);
 	PhysicsServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
 
-	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity = p_gravity; }
 	_FORCE_INLINE_ float get_gravity() const { return gravity; }
 
-	_FORCE_INLINE_ void set_gravity_vector(const Vector3& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ void set_gravity_vector(const Vector3 &p_gravity) { gravity_vector = p_gravity; }
 	_FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; }
 
-	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; }
 	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
 
-	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale=scale; }
+	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale = scale; }
 	_FORCE_INLINE_ float get_gravity_distance_scale() const { return gravity_distance_scale; }
 
-	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation = p_point_attenuation; }
 	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
 
-	_FORCE_INLINE_ void set_linear_damp(float p_linear_damp) { linear_damp=p_linear_damp; }
+	_FORCE_INLINE_ void set_linear_damp(float p_linear_damp) { linear_damp = p_linear_damp; }
 	_FORCE_INLINE_ float get_linear_damp() const { return linear_damp; }
 
-	_FORCE_INLINE_ void set_angular_damp(float p_angular_damp) { angular_damp=p_angular_damp; }
+	_FORCE_INLINE_ void set_angular_damp(float p_angular_damp) { angular_damp = p_angular_damp; }
 	_FORCE_INLINE_ float get_angular_damp() const { return angular_damp; }
 
-	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
-	_FORCE_INLINE_ void add_constraint( ConstraintSW* p_constraint) { constraints.insert(p_constraint); }
-	_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
-	_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<ConstraintSW *> &get_constraints() const { return constraints; }
 
 	void set_monitorable(bool p_monitorable);
 	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 
-	void set_transform(const Transform& p_transform);
+	void set_transform(const Transform &p_transform);
 
 	void set_space(SpaceSW *p_space);
 
-
 	void call_queries();
 
 	AreaSW();
 	~AreaSW();
 };
 
-void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 
-	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].inc();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
 }
-void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 
-	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].dec();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
 }
 
+void AreaSW::add_area_to_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 
-void AreaSW::add_area_to_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
-
-
-	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].inc();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
-
-
 }
-void AreaSW::remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
-
+void AreaSW::remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 
-	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].dec();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
-
 }
 
-
-
-
-
-
 #endif // AREA__SW_H
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 630d8e3d5..b232bae1e 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -28,8 +28,8 @@
 /*************************************************************************/
 #include "body_pair_sw.h"
 #include "collision_solver_sw.h"
-#include "space_sw.h"
 #include "os/os.h"
+#include "space_sw.h"
 
 /*
 #define NO_ACCUMULATE_IMPULSES
@@ -41,19 +41,17 @@
 #define NO_TANGENTIALS
 /* BODY PAIR */
 
-
 //#define ALLOWED_PENETRATION 0.01
 #define RELAXATION_TIMESTEPS 3
 #define MIN_VELOCITY 0.0001
 
-void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
-
-	BodyPairSW* pair  = (BodyPairSW*)p_userdata;
-	pair->contact_added_callback(p_point_A,p_point_B);
+void BodyPairSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
 
+	BodyPairSW *pair = (BodyPairSW *)p_userdata;
+	pair->contact_added_callback(p_point_A, p_point_B);
 }
 
-void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) {
+void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) {
 
 	// check if we already have the contact
 
@@ -61,40 +59,36 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3&
 	//Vector3 local_B = B->get_inv_transform().xform(p_point_B);
 
 	Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
-	Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B-offset_B);
-
-
+	Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
 
 	int new_index = contact_count;
 
-	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+	ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
 
 	Contact contact;
 
-	contact.acc_normal_impulse=0;
-	contact.acc_bias_impulse=0;
-	contact.acc_tangent_impulse=Vector3();
-	contact.local_A=local_A;
-	contact.local_B=local_B;
-	contact.normal=(p_point_A-p_point_B).normalized();
-
-
+	contact.acc_normal_impulse = 0;
+	contact.acc_bias_impulse = 0;
+	contact.acc_tangent_impulse = Vector3();
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.normal = (p_point_A - p_point_B).normalized();
 
 	// attempt to determine if the contact will be reused
-	real_t contact_recycle_radius=space->get_contact_recycle_radius();
+	real_t contact_recycle_radius = space->get_contact_recycle_radius();
 
-	for (int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 		if (
-		c.local_A.distance_squared_to( local_A ) < (contact_recycle_radius*contact_recycle_radius) &&
-		c.local_B.distance_squared_to( local_B ) < (contact_recycle_radius*contact_recycle_radius) ) {
+				c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
+				c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
 
-				contact.acc_normal_impulse=c.acc_normal_impulse;
-				contact.acc_bias_impulse=c.acc_bias_impulse;
-				contact.acc_tangent_impulse=c.acc_tangent_impulse;
-				new_index=i;
-				break;
+			contact.acc_normal_impulse = c.acc_normal_impulse;
+			contact.acc_bias_impulse = c.acc_bias_impulse;
+			contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			new_index = i;
+			break;
 		}
 	}
 
@@ -104,66 +98,63 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3&
 
 		// remove the contact with the minimum depth
 
-		int least_deep=-1;
-		float min_depth=1e10;
+		int least_deep = -1;
+		float min_depth = 1e10;
 
-		for (int i=0;i<=contact_count;i++) {
+		for (int i = 0; i <= contact_count; i++) {
 
-			Contact& c = (i==contact_count)?contact:contacts[i];
+			Contact &c = (i == contact_count) ? contact : contacts[i];
 			Vector3 global_A = A->get_transform().basis.xform(c.local_A);
-			Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+			Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
 
 			Vector3 axis = global_A - global_B;
-			float depth = axis.dot( c.normal );
+			float depth = axis.dot(c.normal);
 
-			if (depth<min_depth) {
+			if (depth < min_depth) {
 
-				min_depth=depth;
-				least_deep=i;
+				min_depth = depth;
+				least_deep = i;
 			}
 		}
 
-		ERR_FAIL_COND(least_deep==-1);
+		ERR_FAIL_COND(least_deep == -1);
 
 		if (least_deep < contact_count) { //replace the last deep contact by the new one
 
-			contacts[least_deep]=contact;
+			contacts[least_deep] = contact;
 		}
 
 		return;
 	}
 
-	contacts[new_index]=contact;
+	contacts[new_index] = contact;
 
-	if (new_index==contact_count) {
+	if (new_index == contact_count) {
 
 		contact_count++;
 	}
-
 }
 
 void BodyPairSW::validate_contacts() {
 
 	//make sure to erase contacts that are no longer valid
 
-	real_t contact_max_separation=space->get_contact_max_separation();
-	for (int i=0;i<contact_count;i++) {
+	real_t contact_max_separation = space->get_contact_max_separation();
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 
 		Vector3 global_A = A->get_transform().basis.xform(c.local_A);
-		Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B;
+		Vector3 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
 		Vector3 axis = global_A - global_B;
-		float depth = axis.dot( c.normal );
+		float depth = axis.dot(c.normal);
 
 		if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
 			// contact no longer needed, remove
 
-
-			if ((i+1) < contact_count) {
+			if ((i + 1) < contact_count) {
 				// swap with the last one
-				SWAP( contacts[i], contacts[ contact_count-1 ] );
-
+				SWAP(contacts[i], contacts[contact_count - 1]);
 			}
 
 			i--;
@@ -172,21 +163,18 @@ void BodyPairSW::validate_contacts() {
 	}
 }
 
+bool BodyPairSW::_test_ccd(float p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B) {
 
-bool BodyPairSW::_test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B) {
-
-
-
-	Vector3 motion = p_A->get_linear_velocity()*p_step;
+	Vector3 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
-	if (mlen<CMP_EPSILON)
+	if (mlen < CMP_EPSILON)
 		return false;
 
 	Vector3 mnormal = motion / mlen;
 
-	real_t min,max;
-	p_A->get_shape(p_shape_A)->project_range(mnormal,p_xform_A,min,max);
-	bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+	real_t min, max;
+	p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
+	bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
 
 	if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
 		return false;
@@ -194,35 +182,34 @@ bool BodyPairSW::_test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transfo
 
 	//cast a segment from support in motion normal, in the same direction of motion by motion length
 	//support is the worst case collision point, so real collision happened before
-	Vector3 s=p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
+	Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
 	Vector3 from = p_xform_A.xform(s);
 	Vector3 to = from + motion;
 
 	Transform from_inv = p_xform_B.affine_inverse();
 
-	Vector3 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+	Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
 	Vector3 local_to = from_inv.xform(to);
 
-	Vector3 rpos,rnorm;
-	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) {
+	Vector3 rpos, rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
 		return false;
 	}
 
 	//shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
 	Vector3 hitpos = p_xform_B.xform(rpos);
 
-	float newlen = hitpos.distance_to(from)-(max-min)*0.01;
-	p_A->set_linear_velocity((mnormal*newlen)/p_step);
+	float newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+	p_A->set_linear_velocity((mnormal * newlen) / p_step);
 
 	return true;
 }
 
-
 bool BodyPairSW::setup(float p_step) {
 
 	//cannot collide
-	if (!A->test_collision_mask(B) || 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 && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
-		collided=false;
+	if (!A->test_collision_mask(B) || 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 && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+		collided = false;
 		return false;
 	}
 
@@ -231,86 +218,79 @@ bool BodyPairSW::setup(float p_step) {
 	validate_contacts();
 
 	Vector3 offset_A = A->get_transform().get_origin();
-	Transform xform_Au = Transform(A->get_transform().basis,Vector3());
+	Transform xform_Au = Transform(A->get_transform().basis, Vector3());
 	Transform xform_A = xform_Au * A->get_shape_transform(shape_A);
 
 	Transform xform_Bu = B->get_transform();
-	xform_Bu.origin-=offset_A;
+	xform_Bu.origin -= offset_A;
 	Transform xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
-	ShapeSW *shape_A_ptr=A->get_shape(shape_A);
-	ShapeSW *shape_B_ptr=B->get_shape(shape_B);
-
-	bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis);
-	this->collided=collided;
+	ShapeSW *shape_A_ptr = A->get_shape(shape_A);
+	ShapeSW *shape_B_ptr = B->get_shape(shape_B);
 
+	bool collided = CollisionSolverSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+	this->collided = collided;
 
 	if (!collided) {
 
 		//test ccd (currently just a raycast)
 
-		if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
-			_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B);
+		if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
+			_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
 		}
 
-		if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
-			_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A);
+		if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
+			_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
 		}
 
 		return false;
 	}
 
-
-
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	float bias = 0.3f;
 
 	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
 
-		if (shape_A_ptr->get_custom_bias()==0)
-			bias=shape_B_ptr->get_custom_bias();
-		else if (shape_B_ptr->get_custom_bias()==0)
-			bias=shape_A_ptr->get_custom_bias();
+		if (shape_A_ptr->get_custom_bias() == 0)
+			bias = shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias() == 0)
+			bias = shape_A_ptr->get_custom_bias();
 		else
-			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+			bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
 	}
 
+	real_t inv_dt = 1.0 / p_step;
 
-
-	real_t inv_dt = 1.0/p_step;
-
-	for(int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
 		Contact &c = contacts[i];
-		c.active=false;
+		c.active = false;
 
 		Vector3 global_A = xform_Au.xform(c.local_A);
 		Vector3 global_B = xform_Bu.xform(c.local_B);
 
-
 		real_t depth = c.normal.dot(global_A - global_B);
 
-		if (depth<=0) {
-			c.active=false;
+		if (depth <= 0) {
+			c.active = false;
 			continue;
 		}
 
-		c.active=true;
+		c.active = true;
 
 #ifdef DEBUG_ENABLED
 
-
 		if (space->is_debugging_contacts()) {
-			space->add_debug_contact(global_A+offset_A);
-			space->add_debug_contact(global_B+offset_A);
+			space->add_debug_contact(global_A + offset_A);
+			space->add_debug_contact(global_B + offset_A);
 		}
 #endif
 
 		c.rA = global_A;
-		c.rB = global_B-offset_B;
+		c.rB = global_B - offset_B;
 
-		// contact query reporting...
+// contact query reporting...
 #if 0
 		if (A->get_body_type() == PhysicsServer::BODY_CHARACTER)
 			static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B );
@@ -323,30 +303,28 @@ bool BodyPairSW::setup(float p_step) {
 #endif
 
 		if (A->can_report_contacts()) {
-			Vector3 crA = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity();
-			A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crA);
+			Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
+			A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA);
 		}
 
 		if (B->can_report_contacts()) {
-			Vector3 crB = B->get_angular_velocity().cross( c.rB ) + B->get_linear_velocity();
-			B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crB);
+			Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
+			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
 		}
 
-		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
-			c.active=false;
-			collided=false;
+		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
+			c.active = false;
+			collided = false;
 			continue;
-
 		}
 
-
-		c.active=true;
+		c.active = true;
 
 		// Precompute normal mass, tangent mass, and bias.
-		Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) );
-		Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) );
+		Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
+		Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal));
 		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
-		kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB ));
+		kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
 		c.mass_normal = 1.0f / kNormal;
 
 #if 1
@@ -354,34 +332,32 @@ bool BodyPairSW::setup(float p_step) {
 
 #else
 		if (depth > max_penetration) {
-			c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS)));
+			c.bias = (depth - max_penetration) * (1.0 / (p_step * (1.0 / RELAXATION_TIMESTEPS)));
 		} else {
 			float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
-			approach = CLAMP( approach, CMP_EPSILON, 1.0 );
-			c.bias = approach * (depth - max_penetration) * (1.0/p_step);
+			approach = CLAMP(approach, CMP_EPSILON, 1.0);
+			c.bias = approach * (depth - max_penetration) * (1.0 / p_step);
 		}
 #endif
-		c.depth=depth;
+		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 );
-		c.acc_bias_impulse=0;
+		A->apply_impulse(c.rA, -j_vec);
+		B->apply_impulse(c.rB, 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, -jb_vec);
+		B->apply_bias_impulse(c.rB, jb_vec);
 
-		c.bounce = MAX(A->get_bounce(),B->get_bounce());
+		c.bounce = MAX(A->get_bounce(), B->get_bounce());
 		if (c.bounce) {
 
-			Vector3 crA = A->get_angular_velocity().cross( c.rA );
-			Vector3 crB = B->get_angular_velocity().cross( c.rB );
+			Vector3 crA = A->get_angular_velocity().cross(c.rA);
+			Vector3 crB = B->get_angular_velocity().cross(c.rB);
 			Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 			//normal impule
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}
-
-
 	}
 
 	return true;
@@ -392,68 +368,63 @@ void BodyPairSW::solve(float p_step) {
 	if (!collided)
 		return;
 
-
-	for(int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
 		Contact &c = contacts[i];
 		if (!c.active)
 			continue;
 
-		c.active=false; //try to deactivate, will activate itself if still needed
+		c.active = false; //try to deactivate, will activate itself if still needed
 
 		//bias impule
 
-		Vector3 crbA = A->get_biased_angular_velocity().cross( c.rA );
-		Vector3 crbB = B->get_biased_angular_velocity().cross( c.rB );
+		Vector3 crbA = A->get_biased_angular_velocity().cross(c.rA);
+		Vector3 crbB = B->get_biased_angular_velocity().cross(c.rB);
 		Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
 
 		real_t vbn = dbv.dot(c.normal);
 
-		if (Math::abs(-vbn+c.bias)>MIN_VELOCITY) {
+		if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
 
-			real_t jbn = (-vbn + c.bias)*c.mass_normal;
+			real_t jbn = (-vbn + c.bias) * c.mass_normal;
 			real_t jbnOld = c.acc_bias_impulse;
 			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-
-			A->apply_bias_impulse(c.rA,-jb);
+			A->apply_bias_impulse(c.rA, -jb);
 			B->apply_bias_impulse(c.rB, jb);
 
-			c.active=true;
+			c.active = true;
 		}
 
-
-		Vector3 crA = A->get_angular_velocity().cross( c.rA );
-		Vector3 crB = B->get_angular_velocity().cross( c.rB );
+		Vector3 crA = A->get_angular_velocity().cross(c.rA);
+		Vector3 crB = B->get_angular_velocity().cross(c.rB);
 		Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 
 		//normal impule
 		real_t vn = dv.dot(c.normal);
 
-		if (Math::abs(vn)>MIN_VELOCITY) {
+		if (Math::abs(vn) > MIN_VELOCITY) {
 
-			real_t jn = -(c.bounce + vn)*c.mass_normal;
+			real_t jn = -(c.bounce + vn) * c.mass_normal;
 			real_t jnOld = c.acc_normal_impulse;
 			c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
 
+			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			Vector3 j =c.normal * (c.acc_normal_impulse - jnOld);
-
-
-			A->apply_impulse(c.rA,-j);
+			A->apply_impulse(c.rA, -j);
 			B->apply_impulse(c.rB, j);
 
-			c.active=true;
+			c.active = true;
 		}
 
 		//friction impule
 
 		real_t friction = A->get_friction() * B->get_friction();
 
-		Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross( c.rA );
-		Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross( c.rB );
+		Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross(c.rA);
+		Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross(c.rB);
 
 		Vector3 dtv = lvB - lvA;
 		real_t tn = c.normal.dot(dtv);
@@ -466,15 +437,14 @@ void BodyPairSW::solve(float p_step) {
 
 			tv /= tvl;
 
-			Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) );
-			Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) );
+			Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv));
+			Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv));
 
 			real_t t = -tvl /
-				(A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+					   (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
 
 			Vector3 jt = t * tv;
 
-
 			Vector3 jtOld = c.acc_tangent_impulse;
 			c.acc_tangent_impulse += jt;
 
@@ -483,46 +453,35 @@ void BodyPairSW::solve(float p_step) {
 
 			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
 
-				c.acc_tangent_impulse*=jtMax / fi_len;
+				c.acc_tangent_impulse *= jtMax / fi_len;
 			}
 
 			jt = c.acc_tangent_impulse - jtOld;
 
+			A->apply_impulse(c.rA, -jt);
+			B->apply_impulse(c.rB, jt);
 
-			A->apply_impulse( c.rA, -jt );
-			B->apply_impulse( c.rB, jt );
-
-			c.active=true;
-
+			c.active = true;
 		}
-
-
 	}
-
 }
 
+BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B)
+	: ConstraintSW(_arr, 2) {
 
-
-
-
-BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_arr,2) {
-
-	A=p_A;
-	B=p_B;
-	shape_A=p_shape_A;
-	shape_B=p_shape_B;
-	space=A->get_space();
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
-	contact_count=0;
-	collided=false;
-
+	A = p_A;
+	B = p_B;
+	shape_A = p_shape_A;
+	shape_B = p_shape_B;
+	space = A->get_space();
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+	contact_count = 0;
+	collided = false;
 }
 
-
 BodyPairSW::~BodyPairSW() {
 
 	A->remove_constraint(this);
 	B->remove_constraint(this);
-
 }
diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h
index 4962c78da..d33583545 100644
--- a/servers/physics/body_pair_sw.h
+++ b/servers/physics/body_pair_sw.h
@@ -35,7 +35,7 @@
 class BodyPairSW : public ConstraintSW {
 	enum {
 
-		MAX_CONTACTS=4
+		MAX_CONTACTS = 4
 	};
 
 	union {
@@ -50,22 +50,21 @@ class BodyPairSW : public ConstraintSW {
 	int shape_A;
 	int shape_B;
 
-
 	struct Contact {
 
 		Vector3 position;
 		Vector3 normal;
 		Vector3 local_A, local_B;
-		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
-		Vector3 acc_tangent_impulse;	// accumulated tangent impulse (Pt)
-		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t acc_normal_impulse; // accumulated normal impulse (Pn)
+		Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
 		real_t mass_normal;
 		real_t bias;
 		real_t bounce;
 
 		real_t depth;
 		bool active;
-		Vector3 rA,rB;
+		Vector3 rA, rB;
 	};
 
 	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
@@ -76,24 +75,21 @@ class BodyPairSW : public ConstraintSW {
 	bool collided;
 	int cc;
 
+	static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
 
-	static void _contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
-
-	void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B);
+	void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B);
 
 	void validate_contacts();
-	bool _test_ccd(float p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B);
+	bool _test_ccd(float p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B);
 
 	SpaceSW *space;
 
 public:
-
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B);
+	BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B);
 	~BodyPairSW();
-
 };
 
 #endif // BODY_PAIR__SW_H
diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 74bba4f97..935328a16 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -27,107 +27,99 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 #include "body_sw.h"
-#include "space_sw.h"
 #include "area_sw.h"
+#include "space_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) {
+	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;
+			float total_area = 0;
 
-			for (int i=0;i<get_shape_count();i++) {
+			for (int i = 0; i < get_shape_count(); i++) {
 
-				total_area+=get_shape_aabb(i).get_area();
+				total_area += get_shape_aabb(i).get_area();
 			}
 
 			Vector3 _inertia;
 
+			for (int i = 0; i < get_shape_count(); i++) {
 
-			for (int i=0;i<get_shape_count();i++) {
-
-				const ShapeSW* shape=get_shape(i);
+				const ShapeSW *shape = get_shape(i);
 
-				float area=get_shape_aabb(i).get_area();
+				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();
+			if (_inertia != Vector3())
+				_inv_inertia = _inertia.inverse();
 			else
-				_inv_inertia=Vector3();
+				_inv_inertia = Vector3();
 
 			if (mass)
-				_inv_mass=1.0/mass;
+				_inv_mass = 1.0 / mass;
 			else
-				_inv_mass=0;
+				_inv_mass = 0;
 
 		} break;
 
 		case PhysicsServer::BODY_MODE_KINEMATIC:
 		case PhysicsServer::BODY_MODE_STATIC: {
 
-			_inv_inertia=Vector3();
-			_inv_mass=0;
+			_inv_inertia = Vector3();
+			_inv_mass = 0;
 		} break;
 		case PhysicsServer::BODY_MODE_CHARACTER: {
 
-			_inv_inertia=Vector3();
-			_inv_mass=1.0/mass;
+			_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)
+	if (active == p_active)
 		return;
 
-	active=p_active;
+	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)
+		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;
 
@@ -140,43 +132,41 @@ void BodySW::set_active(bool p_active) {
 */
 }
 
-
-
 void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::BODY_PARAM_BOUNCE: {
 
-			bounce=p_value;
+			bounce = p_value;
 		} break;
 		case PhysicsServer::BODY_PARAM_FRICTION: {
 
-			friction=p_value;
+			friction = p_value;
 		} break;
 		case PhysicsServer::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value<=0);
-			mass=p_value;
+			ERR_FAIL_COND(p_value <= 0);
+			mass = p_value;
 			_update_inertia();
 
 		} break;
 		case PhysicsServer::BODY_PARAM_GRAVITY_SCALE: {
-			gravity_scale=p_value;
+			gravity_scale = p_value;
 		} break;
 		case PhysicsServer::BODY_PARAM_LINEAR_DAMP: {
 
-			linear_damp=p_value;
+			linear_damp = p_value;
 		} break;
 		case PhysicsServer::BODY_PARAM_ANGULAR_DAMP: {
 
-			angular_damp=p_value;
+			angular_damp = p_value;
 		} break;
-		default:{}
+		default: {}
 	}
 }
 
 float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::BODY_PARAM_BOUNCE: {
 
 			return bounce;
@@ -200,7 +190,7 @@ float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
 			return angular_damp;
 		} break;
 
-		default:{}
+		default: {}
 	}
 
 	return 0;
@@ -208,43 +198,42 @@ float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
 
 void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 
-	PhysicsServer::BodyMode prev=mode;
-	mode=p_mode;
+	PhysicsServer::BodyMode prev = mode;
+	mode = p_mode;
 
-	switch(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);
+			_inv_mass = 0;
+			_set_static(p_mode == PhysicsServer::BODY_MODE_STATIC);
 			//set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC);
-			set_active(p_mode==PhysicsServer::BODY_MODE_KINEMATIC && contacts.size());
-			linear_velocity=Vector3();
-			angular_velocity=Vector3();
-			if (mode==PhysicsServer::BODY_MODE_KINEMATIC && prev!=mode) {
-				first_time_kinematic=true;
+			set_active(p_mode == PhysicsServer::BODY_MODE_KINEMATIC && contacts.size());
+			linear_velocity = Vector3();
+			angular_velocity = Vector3();
+			if (mode == PhysicsServer::BODY_MODE_KINEMATIC && prev != mode) {
+				first_time_kinematic = true;
 			}
 
 		} break;
 		case PhysicsServer::BODY_MODE_RIGID: {
 
-			_inv_mass=mass>0?(1.0/mass):0;
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 
 		} break;
 		case PhysicsServer::BODY_MODE_CHARACTER: {
 
-			_inv_mass=mass>0?(1.0/mass):0;
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 		} break;
 	}
 
 	_update_inertia();
 	//if (get_space())
-//		_update_queries();
-
+	//		_update_queries();
 }
 PhysicsServer::BodyMode BodySW::get_mode() const {
 
@@ -256,35 +245,33 @@ void BodySW::_shapes_changed() {
 	_update_inertia();
 }
 
-void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {
+void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) {
 
-	switch(p_state)	{
+	switch (p_state) {
 		case PhysicsServer::BODY_STATE_TRANSFORM: {
 
-
-			if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
-				new_transform=p_variant;
+			if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
+				new_transform = p_variant;
 				//wakeup_neighbours();
 				set_active(true);
 				if (first_time_kinematic) {
 					_set_transform(p_variant);
 					_set_inv_transform(get_transform().affine_inverse());
-					first_time_kinematic=false;
+					first_time_kinematic = false;
 				}
 
-			} else if (mode==PhysicsServer::BODY_MODE_STATIC) {
+			} else if (mode == PhysicsServer::BODY_MODE_STATIC) {
 				_set_transform(p_variant);
 				_set_inv_transform(get_transform().affine_inverse());
 				wakeup_neighbours();
 			} else {
 				Transform t = p_variant;
 				t.orthonormalize();
-				new_transform=get_transform(); //used as old to compute motion
-				if (new_transform==t)
+				new_transform = get_transform(); //used as old to compute motion
+				if (new_transform == t)
 					break;
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
-
 			}
 			wakeup();
 
@@ -293,44 +280,43 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
 
 			//if (mode==PhysicsServer::BODY_MODE_STATIC)
 			//	break;
-			linear_velocity=p_variant;
+			linear_velocity = p_variant;
 			wakeup();
 		} break;
 		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
 			//if (mode!=PhysicsServer::BODY_MODE_RIGID)
 			//	break;
-			angular_velocity=p_variant;
+			angular_velocity = p_variant;
 			wakeup();
 
 		} break;
 		case PhysicsServer::BODY_STATE_SLEEPING: {
 			//?
-			if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+			if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
 				break;
-			bool do_sleep=p_variant;
+			bool do_sleep = p_variant;
 			if (do_sleep) {
-				linear_velocity=Vector3();
+				linear_velocity = Vector3();
 				//biased_linear_velocity=Vector3();
-				angular_velocity=Vector3();
+				angular_velocity = Vector3();
 				//biased_angular_velocity=Vector3();
 				set_active(false);
 			} else {
-				if (mode!=PhysicsServer::BODY_MODE_STATIC)
+				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)
+			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)	{
+	switch (p_state) {
 		case PhysicsServer::BODY_STATE_TRANSFORM: {
 			return get_transform();
 		} break;
@@ -351,8 +337,7 @@ Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {
 	return Variant();
 }
 
-
-void BodySW::set_space(SpaceSW *p_space){
+void BodySW::set_space(SpaceSW *p_space) {
 
 	if (get_space()) {
 
@@ -362,7 +347,6 @@ void BodySW::set_space(SpaceSW *p_space){
 			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);
@@ -372,24 +356,22 @@ void BodySW::set_space(SpaceSW *p_space){
 		_update_inertia();
 		if (active)
 			get_space()->body_add_to_active_list(&active_list);
-//		_update_queries();
+		//		_update_queries();
 		//if (is_active()) {
 		//	active=false;
 		//	set_active(true);
 		//}
-
 	}
 
-	first_integration=true;
-
+	first_integration = true;
 }
 
 void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) {
 
 	if (p_area->is_gravity_point()) {
-		if(p_area->get_gravity_distance_scale() > 0) {
+		if (p_area->get_gravity_distance_scale() > 0) {
 			Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
-			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) );
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
 		} else {
 			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
 		}
@@ -403,8 +385,7 @@ void BodySW::_compute_area_gravity_and_dampenings(const AreaSW *p_area) {
 
 void BodySW::integrate_forces(real_t p_step) {
 
-
-	if (mode==PhysicsServer::BODY_MODE_STATIC)
+	if (mode == PhysicsServer::BODY_MODE_STATIC)
 		return;
 
 	AreaSW *def_area = get_space()->get_default_area();
@@ -414,186 +395,173 @@ void BodySW::integrate_forces(real_t p_step) {
 
 	int ac = areas.size();
 	bool stopped = false;
-	gravity = Vector3(0,0,0);
+	gravity = Vector3(0, 0, 0);
 	area_linear_damp = 0;
 	area_angular_damp = 0;
 	if (ac) {
 		areas.sort();
 		const AreaCMP *aa = &areas[0];
 		// damp_area = aa[ac-1].area;
-		for(int i=ac-1;i>=0 && !stopped;i--) {
-			PhysicsServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode();
+		for (int i = ac - 1; i >= 0 && !stopped; i--) {
+			PhysicsServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
 			switch (mode) {
 				case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE:
 				case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
 					_compute_area_gravity_and_dampenings(aa[i].area);
-					stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+					stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
 				} break;
 				case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE:
 				case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
-					gravity = Vector3(0,0,0);
+					gravity = Vector3(0, 0, 0);
 					area_angular_damp = 0;
 					area_linear_damp = 0;
 					_compute_area_gravity_and_dampenings(aa[i].area);
-					stopped = mode==PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE;
+					stopped = mode == PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE;
 				} break;
 				default: {}
 			}
 		}
 	}
 
-	if( !stopped ) {
+	if (!stopped) {
 		_compute_area_gravity_and_dampenings(def_area);
 	}
 
-	gravity*=gravity_scale;
+	gravity *= gravity_scale;
 
 	// If less than 0, override dampenings with that of the Body
-	if (angular_damp>=0)
-		area_angular_damp=angular_damp;
+	if (angular_damp >= 0)
+		area_angular_damp = angular_damp;
 	//else
 	//	area_angular_damp=damp_area->get_angular_damp();
 
-	if (linear_damp>=0)
-		area_linear_damp=linear_damp;
+	if (linear_damp >= 0)
+		area_linear_damp = linear_damp;
 	//else
 	//	area_linear_damp=damp_area->get_linear_damp();
 
-
 	Vector3 motion;
-	bool do_motion=false;
+	bool do_motion = false;
 
-	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
 
 		//compute motion, angular and etc. velocities from prev transform
-		linear_velocity = (new_transform.origin - get_transform().origin)/p_step;
+		linear_velocity = (new_transform.origin - get_transform().origin) / p_step;
 
 		//compute a FAKE angular velocity, not so easy
-		Matrix3 rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
+		Matrix3 rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
 		Vector3 axis;
 		float angle;
 
-		rot.get_axis_and_angle(axis,angle);
+		rot.get_axis_and_angle(axis, angle);
 		axis.normalize();
-		angular_velocity=axis.normalized() * (angle/p_step);
+		angular_velocity = axis.normalized() * (angle / p_step);
 
 		motion = new_transform.origin - get_transform().origin;
-		do_motion=true;
+		do_motion = true;
 
 	} else {
 		if (!omit_force_integration && !first_integration) {
 			//overriden by direct state query
 
-			Vector3 force=gravity*mass;
-			force+=applied_force;
-			Vector3 torque=applied_torque;
+			Vector3 force = gravity * mass;
+			force += applied_force;
+			Vector3 torque = applied_torque;
 
 			real_t damp = 1.0 - p_step * area_linear_damp;
 
-			if (damp<0) // reached zero in the given time
-				damp=0;
+			if (damp < 0) // reached zero in the given time
+				damp = 0;
 
 			real_t angular_damp = 1.0 - p_step * area_angular_damp;
 
-			if (angular_damp<0) // reached zero in the given time
-				angular_damp=0;
+			if (angular_damp < 0) // reached zero in the given time
+				angular_damp = 0;
 
-			linear_velocity*=damp;
-			angular_velocity*=angular_damp;
+			linear_velocity *= damp;
+			angular_velocity *= angular_damp;
 
-			linear_velocity+=_inv_mass * force * p_step;
-			angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
+			linear_velocity += _inv_mass * force * p_step;
+			angular_velocity += _inv_inertia_tensor.xform(torque) * p_step;
 		}
 
 		if (continuous_cd) {
-			motion=linear_velocity*p_step;
-			do_motion=true;
+			motion = linear_velocity * p_step;
+			do_motion = true;
 		}
-
 	}
 
-	applied_force=Vector3();
-	applied_torque=Vector3();
-	first_integration=false;
+	applied_force = Vector3();
+	applied_torque = Vector3();
+	first_integration = false;
 
 	//motion=linear_velocity*p_step;
 
-	biased_angular_velocity=Vector3();
-	biased_linear_velocity=Vector3();
-
+	biased_angular_velocity = Vector3();
+	biased_linear_velocity = Vector3();
 
-	if (do_motion) {//shapes temporarily extend for raycast
+	if (do_motion) { //shapes temporarily extend for raycast
 		_update_shapes_with_motion(motion);
 	}
 
-
-	def_area=NULL; // clear the area, so it is set in the next frame
-	contact_count=0;
-
+	def_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)
+	if (mode == PhysicsServer::BODY_MODE_STATIC)
 		return;
 
 	if (fi_callback)
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 
-	if (mode==PhysicsServer::BODY_MODE_KINEMATIC) {
+	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
 
-		_set_transform(new_transform,false);
+		_set_transform(new_transform, false);
 		_set_inv_transform(new_transform.affine_inverse());
-		if (contacts.size()==0 && linear_velocity==Vector3() && angular_velocity==Vector3())
+		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
 			set_active(false); //stopped moving, deactivate
 
 		return;
 	}
 
-
-
 	//apply axis lock
-	if (axis_lock!=PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
-
+	if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
 
-		int axis=axis_lock-1;
-		for(int i=0;i<3;i++) {
-			if (i==axis) {
-				linear_velocity[i]=0;
-				biased_linear_velocity[i]=0;
+		int axis = axis_lock - 1;
+		for (int i = 0; i < 3; i++) {
+			if (i == axis) {
+				linear_velocity[i] = 0;
+				biased_linear_velocity[i] = 0;
 			} else {
 
-				angular_velocity[i]=0;
-				biased_angular_velocity[i]=0;
+				angular_velocity[i] = 0;
+				biased_angular_velocity[i] = 0;
 			}
 		}
-
 	}
 
-
-	Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;
-
-
+	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) {
+	if (ang_vel != 0.0) {
 		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
-		Matrix3 rot( ang_vel_axis, -ang_vel*p_step );
+		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;
+	Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
 	/*for(int i=0;i<3;i++) {
 		if (axis_lock&(1<<i)) {
 			transform.origin[i]=0.0;
 		}
 	}*/
 
-	transform.origin+=total_linear_velocity * p_step;
+	transform.origin += total_linear_velocity * p_step;
 
 	_set_transform(transform);
 	_set_inv_transform(get_transform().inverse());
@@ -642,18 +610,18 @@ void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
 
 void BodySW::wakeup_neighbours() {
 
-	for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+	for (Map<ConstraintSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) {
 
-		const ConstraintSW *c=E->key();
+		const ConstraintSW *c = E->key();
 		BodySW **n = c->get_body_ptr();
-		int bc=c->get_body_count();
+		int bc = c->get_body_count();
 
-		for(int i=0;i<bc;i++) {
+		for (int i = 0; i < bc; i++) {
 
-			if (i==E->get())
+			if (i == E->get())
 				continue;
 			BodySW *b = n[i];
-			if (b->mode!=PhysicsServer::BODY_MODE_RIGID)
+			if (b->mode != PhysicsServer::BODY_MODE_RIGID)
 				continue;
 
 			if (!b->is_active())
@@ -664,109 +632,96 @@ void BodySW::wakeup_neighbours() {
 
 void BodySW::call_queries() {
 
-
 	if (fi_callback) {
 
 		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
-		dbs->body=this;
+		dbs->body = this;
 
-		Variant v=dbs;
+		Variant v = dbs;
 
 		Object *obj = ObjectDB::get_instance(fi_callback->id);
 		if (!obj) {
 
-			set_force_integration_callback(0,StringName());
+			set_force_integration_callback(0, StringName());
 		} else {
-			const Variant *vp[2]={&v,&fi_callback->udata};
+			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);
+			int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
+			obj->call(fi_callback->method, vp, argc, ce);
 		}
-
-
 	}
-
-
 }
 
+bool BodySW::sleep_test(real_t p_step) {
 
-bool BodySW::sleep_test(real_t p_step)  {
-
-	if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+	if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
 		return true; //
-	else if (mode==PhysicsServer::BODY_MODE_CHARACTER)
+	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()) {
 
-
-
-	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;
+		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?
+		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) {
+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;
+		fi_callback = NULL;
 	}
 
+	if (p_id != 0) {
 
-	if (p_id!=0) {
-
-		fi_callback=memnew(ForceIntegrationCallback);
-		fi_callback->id=p_id;
-		fi_callback->method=p_method;
-		fi_callback->udata=p_udata;
+		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) {
-
+BodySW::BodySW()
+	: CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
 
-	mode=PhysicsServer::BODY_MODE_RIGID;
-	active=true;
+	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;
-	first_time_kinematic=false;
-	first_integration=false;
+	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;
+	first_time_kinematic = false;
+	first_integration = false;
 	_set_static(false);
 
-	contact_count=0;
-	gravity_scale=1.0;
+	contact_count = 0;
+	gravity_scale = 1.0;
 
-	area_angular_damp=0;
-	area_linear_damp=0;
-
-	still_time=0;
-	continuous_cd=false;
-	can_sleep=false;
-	fi_callback=NULL;
-	axis_lock=PhysicsServer::BODY_AXIS_LOCK_DISABLED;
+	area_angular_damp = 0;
+	area_linear_damp = 0;
 
+	still_time = 0;
+	continuous_cd = false;
+	can_sleep = false;
+	fi_callback = NULL;
+	axis_lock = PhysicsServer::BODY_AXIS_LOCK_DISABLED;
 }
 
 BodySW::~BodySW() {
@@ -775,9 +730,9 @@ BodySW::~BodySW() {
 		memdelete(fi_callback);
 }
 
-PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;
+PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = NULL;
 
-PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {
+PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() {
 
 	return body->get_space()->get_direct_state();
 }
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 2855e03a8..cb91f93fe 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -29,16 +29,14 @@
 #ifndef BODY_SW_H
 #define BODY_SW_H
 
+#include "area_sw.h"
 #include "collision_object_sw.h"
 #include "vset.h"
-#include "area_sw.h"
 
 class ConstraintSW;
 
-
 class BodySW : public CollisionObjectSW {
 
-
 	PhysicsServer::BodyMode mode;
 
 	Vector3 linear_velocity;
@@ -70,7 +68,6 @@ class BodySW : public CollisionObjectSW {
 	float area_angular_damp;
 	float area_linear_damp;
 
-
 	SelfList<BodySW> active_list;
 	SelfList<BodySW> inertia_update_list;
 	SelfList<BodySW> direct_state_query_list;
@@ -88,23 +85,25 @@ class BodySW : public CollisionObjectSW {
 	virtual void _shapes_changed();
 	Transform new_transform;
 
-	Map<ConstraintSW*,int> constraint_map;
+	Map<ConstraintSW *, int> constraint_map;
 
 	struct AreaCMP {
 
 		AreaSW *area;
 		int refCount;
-		_FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();}
-		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();}
+		_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
+		_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
 		_FORCE_INLINE_ AreaCMP() {}
-		_FORCE_INLINE_ AreaCMP(AreaSW *p_area) { area=p_area; refCount=1;}
+		_FORCE_INLINE_ AreaCMP(AreaSW *p_area) {
+			area = p_area;
+			refCount = 1;
+		}
 	};
 
 	Vector<AreaCMP> areas;
 
 	struct Contact {
 
-
 		Vector3 local_pos;
 		Vector3 local_normal;
 		float depth;
@@ -128,7 +127,6 @@ class BodySW : public CollisionObjectSW {
 
 	ForceIntegrationCallback *fi_callback;
 
-
 	uint64_t island_step;
 	BodySW *island_next;
 	BodySW *island_list_next;
@@ -137,16 +135,14 @@ class BodySW : public CollisionObjectSW {
 
 	_FORCE_INLINE_ void _update_inertia_tensor();
 
-friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
+	friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
 
 public:
-
-
-	void set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata=Variant());
+	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
 
 	_FORCE_INLINE_ void add_area(AreaSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
-		if( index > -1 ) {
+		if (index > -1) {
 			areas[index].refCount += 1;
 		} else {
 			areas.ordered_insert(AreaCMP(p_area));
@@ -155,68 +151,71 @@ public:
 
 	_FORCE_INLINE_ void remove_area(AreaSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
-		if( index > -1 ) {
+		if (index > -1) {
 			areas[index].refCount -= 1;
-			if( areas[index].refCount < 1 )
+			if (areas[index].refCount < 1)
 				areas.remove(index);
 		}
 	}
 
-	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==PhysicsServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
+		contacts.resize(p_size);
+		contact_count = 0;
+		if (mode == PhysicsServer::BODY_MODE_KINEMATIC && p_size) set_active(true);
+	}
 	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
 
 	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
-	_FORCE_INLINE_ void add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos);
-
+	_FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, float p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos);
 
-	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
-	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
-	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
-	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
+	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
+	_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
+	_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
+	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
 
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
-	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
-	_FORCE_INLINE_ BodySW* get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(BodySW* p_next) { island_next=p_next; }
+	_FORCE_INLINE_ BodySW *get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(BodySW *p_next) { island_next = p_next; }
 
-	_FORCE_INLINE_ BodySW* get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(BodySW* p_next) { island_list_next=p_next; }
+	_FORCE_INLINE_ BodySW *get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(BodySW *p_next) { island_list_next = p_next; }
 
-	_FORCE_INLINE_ void add_constraint(ConstraintSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
-	_FORCE_INLINE_ void remove_constraint(ConstraintSW* p_constraint) { constraint_map.erase(p_constraint); }
-	const Map<ConstraintSW*,int>& get_constraint_map() const { return constraint_map; }
+	_FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
+	_FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraint_map.erase(p_constraint); }
+	const Map<ConstraintSW *, int> &get_constraint_map() const { return constraint_map; }
 
-	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+	_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_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
 
-	_FORCE_INLINE_ void set_angular_velocity(const Vector3& p_velocity) { angular_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_angular_velocity(const Vector3 &p_velocity) { angular_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
 
-	_FORCE_INLINE_ const Vector3& get_biased_linear_velocity() const { return biased_linear_velocity; }
-	_FORCE_INLINE_ const Vector3& get_biased_angular_velocity() const { return biased_angular_velocity; }
+	_FORCE_INLINE_ const Vector3 &get_biased_linear_velocity() const { return biased_linear_velocity; }
+	_FORCE_INLINE_ const Vector3 &get_biased_angular_velocity() const { return biased_angular_velocity; }
 
-	_FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) {
+	_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.cross(p_j));
 	}
 
-	_FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& 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.cross(p_j));
 	}
 
-	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) {
 
 		angular_velocity += _inv_inertia_tensor.xform(p_j);
 	}
 
-	_FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) {
+	_FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) {
 
 		applied_force += p_force;
 		applied_torque += p_pos.cross(p_force);
@@ -226,7 +225,7 @@ public:
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
 	_FORCE_INLINE_ void wakeup() {
-		if ((!get_space()) || mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_KINEMATIC)
+		if ((!get_space()) || mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
 			return;
 		set_active(true);
 	}
@@ -237,16 +236,16 @@ public:
 	void set_mode(PhysicsServer::BodyMode p_mode);
 	PhysicsServer::BodyMode get_mode() const;
 
-	void set_state(PhysicsServer::BodyState p_state, const Variant& p_variant);
+	void set_state(PhysicsServer::BodyState p_state, const Variant &p_variant);
 	Variant get_state(PhysicsServer::BodyState p_state) const;
 
-	void set_applied_force(const Vector3& p_force) { applied_force=p_force; }
+	void set_applied_force(const Vector3 &p_force) { applied_force = p_force; }
 	Vector3 get_applied_force() const { return applied_force; }
 
-	void set_applied_torque(const Vector3& p_torque) { applied_torque=p_torque; }
+	void set_applied_torque(const Vector3 &p_torque) { applied_torque = p_torque; }
 	Vector3 get_applied_torque() const { return applied_torque; }
 
-	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
+	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
 	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
 
 	void set_space(SpaceSW *p_space);
@@ -260,33 +259,32 @@ public:
 	_FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }
 	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
 
-	_FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock=p_lock; }
+	_FORCE_INLINE_ void set_axis_lock(PhysicsServer::BodyAxisLock p_lock) { axis_lock = p_lock; }
 	_FORCE_INLINE_ PhysicsServer::BodyAxisLock get_axis_lock() const { return axis_lock; }
 
 	void integrate_forces(real_t p_step);
 	void integrate_velocities(real_t p_step);
 
-	_FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const {
+	_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 {
+	_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;
 
-		 Vector3 c0 = (r0).cross(p_normal);
+		Vector3 c0 = (r0).cross(p_normal);
 
-		 Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
+		Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
 
-		 return _inv_mass + p_normal.dot(vec);
-
-	 }
+		return _inv_mass + p_normal.dot(vec);
+	}
 
-	_FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3& p_axis) const {
+	_FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3 &p_axis) const {
 
-		return p_axis.dot( _inv_inertia_tensor.xform_inv(p_axis) );
-	 }
+		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();
@@ -296,113 +294,129 @@ public:
 
 	BodySW();
 	~BodySW();
-
 };
 
-
 //add contact inline
 
-void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos) {
+void BodySW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, float p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
 
-	int c_max=contacts.size();
+	int c_max = contacts.size();
 
-	if (c_max==0)
+	if (c_max == 0)
 		return;
 
 	Contact *c = &contacts[0];
 
+	int idx = -1;
 
-	int idx=-1;
-
-	if (contact_count<c_max) {
-		idx=contact_count++;
+	if (contact_count < c_max) {
+		idx = contact_count++;
 	} else {
 
-		float least_depth=1e20;
-		int least_deep=-1;
-		for(int i=0;i<c_max;i++) {
+		float least_depth = 1e20;
+		int least_deep = -1;
+		for (int i = 0; i < c_max; i++) {
 
-			if (i==0 || c[i].depth<least_depth) {
-				least_deep=i;
-				least_depth=c[i].depth;
+			if (i == 0 || c[i].depth < least_depth) {
+				least_deep = i;
+				least_depth = c[i].depth;
 			}
 		}
 
-		if (least_deep>=0 && least_depth<p_depth) {
+		if (least_deep >= 0 && least_depth < p_depth) {
 
-			idx=least_deep;
+			idx = least_deep;
 		}
-		if (idx==-1)
+		if (idx == -1)
 			return; //none least deepe than this
 	}
 
-	c[idx].local_pos=p_local_pos;
-	c[idx].local_normal=p_local_normal;
-	c[idx].depth=p_depth;
-	c[idx].local_shape=p_local_shape;
-	c[idx].collider_pos=p_collider_pos;
-	c[idx].collider_shape=p_collider_shape;
-	c[idx].collider_instance_id=p_collider_instance_id;
-	c[idx].collider=p_collider;
-	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
-
+	c[idx].local_pos = p_local_pos;
+	c[idx].local_normal = p_local_normal;
+	c[idx].depth = p_depth;
+	c[idx].local_shape = p_local_shape;
+	c[idx].collider_pos = p_collider_pos;
+	c[idx].collider_shape = p_collider_shape;
+	c[idx].collider_instance_id = p_collider_instance_id;
+	c[idx].collider = p_collider;
+	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
 }
 
-
 class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState {
 
-	OBJ_TYPE( PhysicsDirectBodyStateSW, PhysicsDirectBodyState );
+	OBJ_TYPE(PhysicsDirectBodyStateSW, PhysicsDirectBodyState);
 
 public:
-
 	static PhysicsDirectBodyStateSW *singleton;
 	BodySW *body;
 	real_t step;
 
-	virtual Vector3 get_total_gravity() const {  return body->gravity;  } // get gravity vector working on this body space/area
-	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_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area
+	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 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
+	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
 
-	virtual void set_linear_velocity(const Vector3& p_velocity) {  body->set_linear_velocity(p_velocity);  }
-	virtual Vector3 get_linear_velocity() const {  return body->get_linear_velocity();  }
+	virtual void set_linear_velocity(const Vector3 &p_velocity) { body->set_linear_velocity(p_velocity); }
+	virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); }
 
-	virtual void set_angular_velocity(const Vector3& p_velocity) {  body->set_angular_velocity(p_velocity);  }
-	virtual Vector3 get_angular_velocity() const {  return body->get_angular_velocity();  }
+	virtual void set_angular_velocity(const Vector3 &p_velocity) { body->set_angular_velocity(p_velocity); }
+	virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); }
 
-	virtual void set_transform(const Transform& p_transform) {  body->set_state(PhysicsServer::BODY_STATE_TRANSFORM,p_transform);  }
-	virtual Transform get_transform() const {  return body->get_transform();  }
+	virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); }
+	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 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();  }
+	virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
+	virtual bool is_sleeping() const { return !body->is_active(); }
 
-	virtual int get_contact_count() const {  return body->contact_count;  }
+	virtual int get_contact_count() const { return body->contact_count; }
 
 	virtual Vector3 get_contact_local_pos(int p_contact_idx) const {
-		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
 		return body->contacts[p_contact_idx].local_pos;
 	}
-	virtual Vector3 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].local_normal;   }
-	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }
-
-	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }
-	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());   return body->contacts[p_contact_idx].collider_pos;  }
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }
-	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }
-	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+		return body->contacts[p_contact_idx].local_normal;
+	}
+	virtual int get_contact_local_shape(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
+		return body->contacts[p_contact_idx].local_shape;
+	}
 
-	virtual PhysicsDirectSpaceState* get_space_state();
+	virtual RID get_contact_collider(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
+		return body->contacts[p_contact_idx].collider;
+	}
+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+		return body->contacts[p_contact_idx].collider_pos;
+	}
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+		return body->contacts[p_contact_idx].collider_instance_id;
+	}
+	virtual int get_contact_collider_shape(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+		return body->contacts[p_contact_idx].collider_shape;
+	}
+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+		return body->contacts[p_contact_idx].collider_velocity_at_pos;
+	}
 
+	virtual PhysicsDirectSpaceState *get_space_state();
 
 	virtual real_t get_step() const { return step; }
-	PhysicsDirectBodyStateSW() { singleton=this; body=NULL; }
+	PhysicsDirectBodyStateSW() {
+		singleton = this;
+		body = NULL;
+	}
 };
 
-
 #endif // BODY__SW_H
diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp
index 30b597859..d6cca6710 100644
--- a/servers/physics/broad_phase_basic.cpp
+++ b/servers/physics/broad_phase_basic.cpp
@@ -31,117 +31,111 @@
 #include "print_string.h"
 BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object_, int p_subindex) {
 
-	if (p_object_==NULL) {
+	if (p_object_ == NULL) {
 
-		ERR_FAIL_COND_V(p_object_==NULL,0);
+		ERR_FAIL_COND_V(p_object_ == NULL, 0);
 	}
 
 	current++;
 
 	Element e;
-	e.owner=p_object_;
-	e._static=false;
-	e.subindex=p_subindex;
+	e.owner = p_object_;
+	e._static = false;
+	e.subindex = p_subindex;
 
-	element_map[current]=e;
+	element_map[current] = e;
 	return current;
 }
 
-void BroadPhaseBasic::move(ID p_id, const AABB& p_aabb) {
+void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	E->get().aabb=p_aabb;
-
+	E->get().aabb = p_aabb;
 }
 void BroadPhaseBasic::set_static(ID p_id, bool p_static) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	E->get()._static=p_static;
-
+	E->get()._static = p_static;
 }
 void BroadPhaseBasic::remove(ID p_id) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 	List<PairKey> to_erase;
 	//unpair must be done immediately on removal to avoid potential invalid pointers
-	for (Map<PairKey,void*>::Element *F=pair_map.front();F;F=F->next()) {
+	for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) {
 
-		if (F->key().a==p_id || F->key().b==p_id) {
+		if (F->key().a == p_id || F->key().b == p_id) {
 
 			if (unpair_callback) {
-				Element *elem_A=&element_map[F->key().a];
-				Element *elem_B=&element_map[F->key().b];
-				unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,F->get(),unpair_userdata);
+				Element *elem_A = &element_map[F->key().a];
+				Element *elem_B = &element_map[F->key().b];
+				unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata);
 			}
 			to_erase.push_back(F->key());
 		}
 	}
-	while(to_erase.size()) {
+	while (to_erase.size()) {
 
 		pair_map.erase(to_erase.front()->get());
 		to_erase.pop_front();
 	}
 	element_map.erase(E);
-
 }
 
 CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,NULL);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, NULL);
 	return E->get().owner;
-
 }
 bool BroadPhaseBasic::is_static(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,false);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, false);
 	return E->get()._static;
-
 }
 int BroadPhaseBasic::get_subindex(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,-1);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, -1);
 	return E->get().subindex;
 }
 
-int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhaseBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
-	int rc=0;
+	int rc = 0;
 
-	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
 
-		const AABB aabb=E->get().aabb;
-		if (aabb.intersects_segment(p_from,p_to)) {
+		const AABB aabb = E->get().aabb;
+		if (aabb.intersects_segment(p_from, p_to)) {
 
-			p_results[rc]=E->get().owner;
-			p_result_indices[rc]=E->get().subindex;
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
 			rc++;
-			if (rc>=p_max_results)
+			if (rc >= p_max_results)
 				break;
 		}
 	}
 
 	return rc;
-
 }
-int BroadPhaseBasic::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhaseBasic::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
-	int rc=0;
+	int rc = 0;
 
-	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
 
-		const AABB aabb=E->get().aabb;
+		const AABB aabb = E->get().aabb;
 		if (aabb.intersects(p_aabb)) {
 
-			p_results[rc]=E->get().owner;
-			p_result_indices[rc]=E->get().subindex;
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
 			rc++;
-			if (rc>=p_max_results)
+			if (rc >= p_max_results)
 				break;
 		}
 	}
@@ -149,68 +143,63 @@ int BroadPhaseBasic::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,
 	return rc;
 }
 
-void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
-
-	pair_userdata=p_userdata;
-	pair_callback=p_pair_callback;
+void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 
+	pair_userdata = p_userdata;
+	pair_callback = p_pair_callback;
 }
-void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
-
-	unpair_userdata=p_userdata;
-	unpair_callback=p_pair_callback;
+void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback, void *p_userdata) {
 
+	unpair_userdata = p_userdata;
+	unpair_callback = p_pair_callback;
 }
 
 void BroadPhaseBasic::update() {
 
 	// recompute pairs
-	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+	for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
 
-		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+		for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
 
-			Element *elem_A=&I->get();
-			Element *elem_B=&J->get();
+			Element *elem_A = &I->get();
+			Element *elem_B = &J->get();
 
 			if (elem_A->owner == elem_B->owner)
 				continue;
 
+			bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
 
-			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
-
-			PairKey key(I->key(),J->key());
+			PairKey key(I->key(), J->key());
 
-			Map<PairKey,void*>::Element *E=pair_map.find(key);
+			Map<PairKey, void *>::Element *E = pair_map.find(key);
 
 			if (!pair_ok && E) {
 				if (unpair_callback)
-					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+					unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
 				pair_map.erase(key);
 			}
 
 			if (pair_ok && !E) {
 
-				void *data=NULL;
+				void *data = NULL;
 				if (pair_callback)
-					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
-				pair_map.insert(key,data);
+					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
+				pair_map.insert(key, data);
 			}
 		}
 	}
-
 }
 
 BroadPhaseSW *BroadPhaseBasic::_create() {
 
-	return memnew( BroadPhaseBasic );
+	return memnew(BroadPhaseBasic);
 }
 
 BroadPhaseBasic::BroadPhaseBasic() {
 
-	current=1;
-	unpair_callback=NULL;
-	unpair_userdata=NULL;
-	pair_callback=NULL;
-	pair_userdata=NULL;
-
+	current = 1;
+	unpair_callback = NULL;
+	unpair_userdata = NULL;
+	pair_callback = NULL;
+	pair_userdata = NULL;
 }
diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h
index 423ff0a6a..6c9309cec 100644
--- a/servers/physics/broad_phase_basic.h
+++ b/servers/physics/broad_phase_basic.h
@@ -42,8 +42,7 @@ class BroadPhaseBasic : public BroadPhaseSW {
 		int subindex;
 	};
 
-
-	Map<ID,Element> element_map;
+	Map<ID, Element> element_map;
 
 	ID current;
 
@@ -57,17 +56,23 @@ class BroadPhaseBasic : public BroadPhaseSW {
 			uint64_t key;
 		};
 
-		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+		_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
 			return key < p_key.key;
 		}
 
-		PairKey() { key=0; }
-		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
-
+		PairKey() { key = 0; }
+		PairKey(ID p_a, ID p_b) {
+			if (p_a > p_b) {
+				a = p_b;
+				b = p_a;
+			} else {
+				a = p_a;
+				b = p_b;
+			}
+		}
 	};
 
-	Map<PairKey,void*> pair_map;
-
+	Map<PairKey, void *> pair_map;
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -75,10 +80,9 @@ class BroadPhaseBasic : public BroadPhaseSW {
 	void *unpair_userdata;
 
 public:
-
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
-	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 
@@ -86,11 +90,11 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
-	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
-	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
+	virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
 
 	virtual void update();
 
diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp
index e747ea93a..5adf0811e 100644
--- a/servers/physics/broad_phase_octree.cpp
+++ b/servers/physics/broad_phase_octree.cpp
@@ -31,85 +31,77 @@
 
 ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) {
 
-	ID oid = octree.create(p_object,AABB(),p_subindex,false,1<<p_object->get_type(),0);
+	ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
 	return oid;
 }
 
-void BroadPhaseOctree::move(ID p_id, const AABB& p_aabb){
+void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
 
-	octree.move(p_id,p_aabb);
+	octree.move(p_id, p_aabb);
 }
 
-void BroadPhaseOctree::set_static(ID p_id, bool p_static){
+void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
 
 	CollisionObjectSW *it = octree.get(p_id);
-	octree.set_pairable(p_id,p_static?false:true,1<<it->get_type(),p_static?0:0xFFFFF); //pair everything, don't care 1?
-
+	octree.set_pairable(p_id, p_static ? false : true, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
 }
-void BroadPhaseOctree::remove(ID p_id){
+void BroadPhaseOctree::remove(ID p_id) {
 
 	octree.erase(p_id);
 }
 
-CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const{
+CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const {
 
 	CollisionObjectSW *it = octree.get(p_id);
-	ERR_FAIL_COND_V(!it,NULL);
+	ERR_FAIL_COND_V(!it, NULL);
 	return it;
 }
-bool BroadPhaseOctree::is_static(ID p_id) const{
+bool BroadPhaseOctree::is_static(ID p_id) const {
 
 	return !octree.is_pairable(p_id);
 }
-int BroadPhaseOctree::get_subindex(ID p_id) const{
+int BroadPhaseOctree::get_subindex(ID p_id) const {
 
 	return octree.get_subindex(p_id);
 }
 
-int BroadPhaseOctree::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices){
+int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
-	return octree.cull_segment(p_from,p_to,p_results,p_max_results,p_result_indices);
+	return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
 }
 
-int BroadPhaseOctree::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
-
-	return octree.cull_AABB(p_aabb,p_results,p_max_results,p_result_indices);
+int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices) {
 
+	return octree.cull_AABB(p_aabb, p_results, p_max_results, p_result_indices);
 }
 
+void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B) {
 
-void* BroadPhaseOctree::_pair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B) {
-
-	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
 	if (!bpo->pair_callback)
 		return NULL;
 
-	return bpo->pair_callback(p_object_A,subindex_A,p_object_B,subindex_B,bpo->pair_userdata);
-
+	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
 }
 
-void BroadPhaseOctree::_unpair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B,void*pairdata) {
+void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) {
 
-	BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self);
+	BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
 	if (!bpo->unpair_callback)
 		return;
 
-	bpo->unpair_callback(p_object_A,subindex_A,p_object_B,subindex_B,pairdata,bpo->unpair_userdata);
-
+	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
 }
 
+void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 
-void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback,void *p_userdata){
-
-	pair_callback=p_pair_callback;
-	pair_userdata=p_userdata;
-
+	pair_callback = p_pair_callback;
+	pair_userdata = p_userdata;
 }
-void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata){
-
-	unpair_callback=p_unpair_callback;
-	unpair_userdata=p_userdata;
+void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
 
+	unpair_callback = p_unpair_callback;
+	unpair_userdata = p_userdata;
 }
 
 void BroadPhaseOctree::update() {
@@ -118,16 +110,14 @@ void BroadPhaseOctree::update() {
 
 BroadPhaseSW *BroadPhaseOctree::_create() {
 
-	return memnew( BroadPhaseOctree );
+	return memnew(BroadPhaseOctree);
 }
 
 BroadPhaseOctree::BroadPhaseOctree() {
-	octree.set_pair_callback(_pair_callback,this);
-	octree.set_unpair_callback(_unpair_callback,this);
-	pair_callback=NULL;
-	pair_userdata=NULL;
-	pair_callback=NULL;
-	unpair_userdata=NULL;
+	octree.set_pair_callback(_pair_callback, this);
+	octree.set_unpair_callback(_unpair_callback, this);
+	pair_callback = NULL;
+	pair_userdata = NULL;
+	pair_callback = NULL;
+	unpair_userdata = NULL;
 }
-
-
diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h
index 43005812e..188d0f9c7 100644
--- a/servers/physics/broad_phase_octree.h
+++ b/servers/physics/broad_phase_octree.h
@@ -34,12 +34,10 @@
 
 class BroadPhaseOctree : public BroadPhaseSW {
 
+	Octree<CollisionObjectSW, true> octree;
 
-	Octree<CollisionObjectSW,true> octree;
-
-	static void* _pair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int);
-	static void _unpair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int,void*);
-
+	static void *_pair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int);
+	static void _unpair_callback(void *, OctreeElementID, CollisionObjectSW *, int, OctreeElementID, CollisionObjectSW *, int, void *);
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -47,10 +45,9 @@ class BroadPhaseOctree : public BroadPhaseSW {
 	void *unpair_userdata;
 
 public:
-
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0);
-	virtual void move(ID p_id, const AABB& p_aabb);
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 
@@ -58,11 +55,11 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
-	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
-	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
+	virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
 
 	virtual void update();
 
diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp
index 2a897dea2..f9a19e558 100644
--- a/servers/physics/broad_phase_sw.cpp
+++ b/servers/physics/broad_phase_sw.cpp
@@ -28,8 +28,7 @@
 /*************************************************************************/
 #include "broad_phase_sw.h"
 
-BroadPhaseSW::CreateFunction BroadPhaseSW::create_func=NULL;
+BroadPhaseSW::CreateFunction BroadPhaseSW::create_func = NULL;
 
-BroadPhaseSW::~BroadPhaseSW()
-{
+BroadPhaseSW::~BroadPhaseSW() {
 }
diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h
index 736b67418..c45f3c494 100644
--- a/servers/physics/broad_phase_sw.h
+++ b/servers/physics/broad_phase_sw.h
@@ -29,45 +29,42 @@
 #ifndef BROAD_PHASE_SW_H
 #define BROAD_PHASE_SW_H
 
-#include "math_funcs.h"
 #include "aabb.h"
+#include "math_funcs.h"
 
 class CollisionObjectSW;
 
-
 class BroadPhaseSW {
 
 public:
-	typedef BroadPhaseSW* (*CreateFunction)();
+	typedef BroadPhaseSW *(*CreateFunction)();
 
 	static CreateFunction create_func;
 
 	typedef uint32_t ID;
 
-
-	typedef void* (*PairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_userdata);
-	typedef void (*UnpairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+	typedef void *(*PairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0)=0;
-	virtual void move(ID p_id, const AABB& p_aabb)=0;
-	virtual void set_static(ID p_id, bool p_static)=0;
-	virtual void remove(ID p_id)=0;
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0) = 0;
+	virtual void move(ID p_id, const AABB &p_aabb) = 0;
+	virtual void set_static(ID p_id, bool p_static) = 0;
+	virtual void remove(ID p_id) = 0;
 
-	virtual CollisionObjectSW *get_object(ID p_id) const=0;
-	virtual bool is_static(ID p_id) const=0;
-	virtual int get_subindex(ID p_id) const=0;
+	virtual CollisionObjectSW *get_object(ID p_id) const = 0;
+	virtual bool is_static(ID p_id) const = 0;
+	virtual int get_subindex(ID p_id) const = 0;
 
-	virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
-	virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
+	virtual int cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
 
-	virtual void update()=0;
+	virtual void update() = 0;
 
 	virtual ~BroadPhaseSW();
-
 };
 
 #endif // BROAD_PHASE__SW_H
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index c45b76da2..52c04e86c 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -29,37 +29,35 @@
 #include "collision_object_sw.h"
 #include "space_sw.h"
 
-void CollisionObjectSW::add_shape(ShapeSW *p_shape,const Transform& p_transform) {
+void CollisionObjectSW::add_shape(ShapeSW *p_shape, const Transform &p_transform) {
 
 	Shape s;
-	s.shape=p_shape;
-	s.xform=p_transform;
-	s.xform_inv=s.xform.affine_inverse();
-	s.bpid=0; //needs update
+	s.shape = p_shape;
+	s.xform = p_transform;
+	s.xform_inv = s.xform.affine_inverse();
+	s.bpid = 0; //needs update
 	shapes.push_back(s);
 	p_shape->add_owner(this);
 	_update_shapes();
 	_shapes_changed();
-
 }
 
-void CollisionObjectSW::set_shape(int p_index,ShapeSW *p_shape){
+void CollisionObjectSW::set_shape(int p_index, ShapeSW *p_shape) {
 
-	ERR_FAIL_INDEX(p_index,shapes.size());
+	ERR_FAIL_INDEX(p_index, shapes.size());
 	shapes[p_index].shape->remove_owner(this);
-	shapes[p_index].shape=p_shape;
+	shapes[p_index].shape = p_shape;
 
 	p_shape->add_owner(this);
 	_update_shapes();
 	_shapes_changed();
-
 }
-void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_transform){
+void CollisionObjectSW::set_shape_transform(int p_index, const Transform &p_transform) {
 
-	ERR_FAIL_INDEX(p_index,shapes.size());
+	ERR_FAIL_INDEX(p_index, shapes.size());
 
-	shapes[p_index].xform=p_transform;
-	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	shapes[p_index].xform = p_transform;
+	shapes[p_index].xform_inv = p_transform.affine_inverse();
 	_update_shapes();
 	_shapes_changed();
 }
@@ -67,61 +65,58 @@ void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_trans
 void CollisionObjectSW::remove_shape(ShapeSW *p_shape) {
 
 	//remove a shape, all the times it appears
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		if (shapes[i].shape==p_shape) {
+		if (shapes[i].shape == p_shape) {
 			remove_shape(i);
 			i--;
 		}
 	}
 }
 
-void CollisionObjectSW::remove_shape(int p_index){
+void CollisionObjectSW::remove_shape(int p_index) {
 
 	//remove anything from shape to be erased to end, so subindices don't change
-	ERR_FAIL_INDEX(p_index,shapes.size());
-	for(int i=p_index;i<shapes.size();i++) {
+	ERR_FAIL_INDEX(p_index, shapes.size());
+	for (int i = p_index; i < shapes.size(); i++) {
 
-		if (shapes[i].bpid==0)
+		if (shapes[i].bpid == 0)
 			continue;
 		//should never get here with a null owner
 		space->get_broadphase()->remove(shapes[i].bpid);
-		shapes[i].bpid=0;
+		shapes[i].bpid = 0;
 	}
 	shapes[p_index].shape->remove_owner(this);
 	shapes.remove(p_index);
 
 	_shapes_changed();
-
 }
 
 void CollisionObjectSW::_set_static(bool p_static) {
-	if (_static==p_static)
+	if (_static == p_static)
 		return;
-	_static=p_static;
+	_static = p_static;
 
 	if (!space)
 		return;
-	for(int i=0;i<get_shape_count();i++) {
-		Shape &s=shapes[i];
-		if (s.bpid>0) {
-			space->get_broadphase()->set_static(s.bpid,_static);
+	for (int i = 0; i < get_shape_count(); i++) {
+		Shape &s = shapes[i];
+		if (s.bpid > 0) {
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 	}
-
 }
 
 void CollisionObjectSW::_unregister_shapes() {
 
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		Shape &s=shapes[i];
-		if (s.bpid>0) {
+		Shape &s = shapes[i];
+		if (s.bpid > 0) {
 			space->get_broadphase()->remove(s.bpid);
-			s.bpid=0;
+			s.bpid = 0;
 		}
 	}
-
 }
 
 void CollisionObjectSW::_update_shapes() {
@@ -129,52 +124,47 @@ void CollisionObjectSW::_update_shapes() {
 	if (!space)
 		return;
 
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		Shape &s=shapes[i];
-		if (s.bpid==0) {
-			s.bpid=space->get_broadphase()->create(this,i);
-			space->get_broadphase()->set_static(s.bpid,_static);
+		Shape &s = shapes[i];
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i);
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 
 		//not quite correct, should compute the next matrix..
-		AABB shape_aabb=s.shape->get_aabb();
+		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
-		shape_aabb=xform.xform(shape_aabb);
-		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 );
-
+		shape_aabb = xform.xform(shape_aabb);
+		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);
 
-		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+		space->get_broadphase()->move(s.bpid, s.aabb_cache);
 	}
-
 }
 
-void CollisionObjectSW::_update_shapes_with_motion(const Vector3& p_motion) {
-
+void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 
 	if (!space)
 		return;
 
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		Shape &s=shapes[i];
-		if (s.bpid==0) {
-			s.bpid=space->get_broadphase()->create(this,i);
-			space->get_broadphase()->set_static(s.bpid,_static);
+		Shape &s = shapes[i];
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i);
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 
 		//not quite correct, should compute the next matrix..
-		AABB shape_aabb=s.shape->get_aabb();
+		AABB shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
-		shape_aabb=xform.xform(shape_aabb);
-		shape_aabb=shape_aabb.merge(AABB( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
-		s.aabb_cache=shape_aabb;
+		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb = shape_aabb.merge(AABB(shape_aabb.pos + p_motion, shape_aabb.size)); //use motion
+		s.aabb_cache = shape_aabb;
 
-		space->get_broadphase()->move(s.bpid,shape_aabb);
+		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
-
-
 }
 
 void CollisionObjectSW::_set_space(SpaceSW *p_space) {
@@ -183,25 +173,23 @@ void CollisionObjectSW::_set_space(SpaceSW *p_space) {
 
 		space->remove_object(this);
 
-		for(int i=0;i<shapes.size();i++) {
+		for (int i = 0; i < shapes.size(); i++) {
 
-			Shape &s=shapes[i];
+			Shape &s = shapes[i];
 			if (s.bpid) {
 				space->get_broadphase()->remove(s.bpid);
-				s.bpid=0;
+				s.bpid = 0;
 			}
 		}
-
 	}
 
-	space=p_space;
+	space = p_space;
 
 	if (space) {
 
 		space->add_object(this);
 		_update_shapes();
 	}
-
 }
 
 void CollisionObjectSW::_shape_changed() {
@@ -212,11 +200,11 @@ void CollisionObjectSW::_shape_changed() {
 
 CollisionObjectSW::CollisionObjectSW(Type p_type) {
 
-	_static=true;
-	type=p_type;
-	space=NULL;
-	instance_id=0;
-	layer_mask=1;
-	collision_mask=1;
-	ray_pickable=true;
+	_static = true;
+	type = p_type;
+	space = NULL;
+	instance_id = 0;
+	layer_mask = 1;
+	collision_mask = 1;
+	ray_pickable = true;
 }
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index 5738a629d..ed6b2bdc6 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -29,14 +29,14 @@
 #ifndef COLLISION_OBJECT_SW_H
 #define COLLISION_OBJECT_SW_H
 
-#include "shape_sw.h"
-#include "servers/physics_server.h"
-#include "self_list.h"
 #include "broad_phase_sw.h"
+#include "self_list.h"
+#include "servers/physics_server.h"
+#include "shape_sw.h"
 
 #ifdef DEBUG_ENABLED
 #define MAX_OBJECT_DISTANCE 10000000.0
-#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE*MAX_OBJECT_DISTANCE)
+#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE)
 #endif
 
 class SpaceSW;
@@ -47,8 +47,8 @@ public:
 		TYPE_AREA,
 		TYPE_BODY
 	};
-private:
 
+private:
 	Type type;
 	RID self;
 	ObjectID instance_id;
@@ -64,7 +64,7 @@ private:
 		ShapeSW *shape;
 		bool trigger;
 
-		Shape() { trigger=false; }
+		Shape() { trigger = false; }
 	};
 
 	Vector<Shape> shapes;
@@ -76,83 +76,79 @@ private:
 	void _update_shapes();
 
 protected:
-
-
-	void _update_shapes_with_motion(const Vector3& p_motion);
+	void _update_shapes_with_motion(const Vector3 &p_motion);
 	void _unregister_shapes();
 
-	_FORCE_INLINE_ void _set_transform(const Transform& p_transform,bool p_update_shapes=true) {
+	_FORCE_INLINE_ void _set_transform(const Transform &p_transform, bool p_update_shapes = true) {
 
 #ifdef DEBUG_ENABLED
 
 		if (p_transform.origin.length_squared() > MAX_OBJECT_DISTANCE_X2) {
-			ERR_EXPLAIN("Object went too far away (more than "+itos(MAX_OBJECT_DISTANCE)+"mts from origin).");
+			ERR_EXPLAIN("Object went too far away (more than " + itos(MAX_OBJECT_DISTANCE) + "mts from origin).");
 			ERR_FAIL();
 		}
 #endif
 
-		transform=p_transform; if (p_update_shapes) _update_shapes();
-
+		transform = p_transform;
+		if (p_update_shapes) _update_shapes();
 	}
-	_FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; }
+	_FORCE_INLINE_ void _set_inv_transform(const Transform &p_transform) { inv_transform = p_transform; }
 	void _set_static(bool p_static);
 
-	virtual void _shapes_changed()=0;
+	virtual void _shapes_changed() = 0;
 	void _set_space(SpaceSW *space);
 
 	bool ray_pickable;
 
-
 	CollisionObjectSW(Type p_type);
-public:
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
 	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
 
 	void _shape_changed();
 
 	_FORCE_INLINE_ Type get_type() const { return type; }
-	void add_shape(ShapeSW *p_shape,const Transform& p_transform=Transform());
-	void set_shape(int p_index,ShapeSW *p_shape);
-	void set_shape_transform(int p_index,const Transform& p_transform);
+	void add_shape(ShapeSW *p_shape, const Transform &p_transform = Transform());
+	void set_shape(int p_index, ShapeSW *p_shape);
+	void set_shape_transform(int p_index, const Transform &p_transform);
 	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
 	_FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; }
-	_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 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_ Transform get_transform() const { return transform; }
 	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
-	_FORCE_INLINE_ SpaceSW* get_space() const { return space; }
+	_FORCE_INLINE_ SpaceSW *get_space() const { return space; }
 
-	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; }
+	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
-	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx, bool p_enable) { shapes[p_idx].trigger = p_enable; }
 	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
 
-	_FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask=p_mask; }
+	_FORCE_INLINE_ void set_layer_mask(uint32_t p_mask) { layer_mask = p_mask; }
 	_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
 
-	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask=p_mask; }
+	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW* p_other) const {
-		return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask;
+	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectSW *p_other) const {
+		return layer_mask & p_other->collision_mask || p_other->layer_mask & collision_mask;
 	}
 
 	void remove_shape(ShapeSW *p_shape);
 	void remove_shape(int p_index);
 
-	virtual void set_space(SpaceSW *p_space)=0;
+	virtual void set_space(SpaceSW *p_space) = 0;
 
-	_FORCE_INLINE_ bool is_static() const { return _static;  }
+	_FORCE_INLINE_ bool is_static() const { return _static; }
 
 	virtual ~CollisionObjectSW() {}
-
 };
 
 #endif // COLLISION_OBJECT_SW_H
diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp
index 08490795f..2513a9fe6 100644
--- a/servers/physics/collision_solver_sat.cpp
+++ b/servers/physics/collision_solver_sat.cpp
@@ -40,76 +40,68 @@ struct _CollectorCallback {
 	Vector3 normal;
 	Vector3 *prev_axis;
 
-	_FORCE_INLINE_ void call(const Vector3& p_point_A, const Vector3& p_point_B) {
+	_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
 
 		//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
 		//	return;
-//		print_line("** A: "+p_point_A+" B: "+p_point_B+" D: "+rtos(p_point_A.distance_to(p_point_B)));
+		//		print_line("** A: "+p_point_A+" B: "+p_point_B+" D: "+rtos(p_point_A.distance_to(p_point_B)));
 
 		if (swap)
-			callback(p_point_B,p_point_A,userdata);
+			callback(p_point_B, p_point_A, userdata);
 		else
-			callback(p_point_A,p_point_B,userdata);
+			callback(p_point_A, p_point_B, userdata);
 	}
-
 };
 
-typedef void (*GenerateContactsFunc)(const Vector3 *,int, const Vector3 *,int ,_CollectorCallback *);
-
+typedef void (*GenerateContactsFunc)(const Vector3 *, int, const Vector3 *, int, _CollectorCallback *);
 
-static void _generate_contacts_point_point(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 1 );
-	ERR_FAIL_COND( p_point_count_B != 1 );
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 1);
 #endif
 
-	p_callback->call(*p_points_A,*p_points_B);
+	p_callback->call(*p_points_A, *p_points_B);
 }
 
-static void _generate_contacts_point_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 1 );
-	ERR_FAIL_COND( p_point_count_B != 2 );
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 2);
 #endif
 
-	Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B );
-	p_callback->call(*p_points_A,closest_B);
-
+	Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
+	p_callback->call(*p_points_A, closest_B);
 }
 
-static void _generate_contacts_point_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 1 );
-	ERR_FAIL_COND( p_point_count_B < 3 );
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B < 3);
 #endif
 
+	Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A);
 
-	Vector3 closest_B=Plane(p_points_B[0],p_points_B[1],p_points_B[2]).project( *p_points_A );
-
-	p_callback->call(*p_points_A,closest_B);
-
+	p_callback->call(*p_points_A, closest_B);
 }
 
-
-static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 2 );
-	ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix
+	ERR_FAIL_COND(p_point_count_A != 2);
+	ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
 #endif
 
+	Vector3 rel_A = p_points_A[1] - p_points_A[0];
+	Vector3 rel_B = p_points_B[1] - p_points_B[0];
 
+	Vector3 c = rel_A.cross(rel_B).cross(rel_B);
 
-	Vector3 rel_A=p_points_A[1]-p_points_A[0];
-	Vector3 rel_B=p_points_B[1]-p_points_B[0];
-
-	Vector3 c=rel_A.cross(rel_B).cross(rel_B);
-
-//	if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
-	if ( Math::abs(rel_A.dot(c) )<CMP_EPSILON ) {
+	//	if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
+	if (Math::abs(rel_A.dot(c)) < CMP_EPSILON) {
 
 		// should handle somehow..
 		//ERR_PRINT("TODO FIX");
@@ -120,156 +112,146 @@ static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_
 		Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
 
 		//sort all 4 points in axis
-		float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
+		float dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
 
 		SortArray<float> sa;
-		sa.sort(dvec,4);
+		sa.sort(dvec, 4);
 
 		//use the middle ones as contacts
-		p_callback->call(base_A+axis*dvec[1],base_B+axis*dvec[1]);
-		p_callback->call(base_A+axis*dvec[2],base_B+axis*dvec[2]);
+		p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]);
+		p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]);
 
 		return;
-
 	}
 
-	real_t d = (c.dot( p_points_B[0] ) - p_points_A[0].dot(c))/rel_A.dot(c);
+	real_t d = (c.dot(p_points_B[0]) - p_points_A[0].dot(c)) / rel_A.dot(c);
 
-	if (d<0.0)
-		d=0.0;
-	else if (d>1.0)
-		d=1.0;
-
-	Vector3 closest_A=p_points_A[0]+rel_A*d;
-	Vector3 closest_B=Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
-	p_callback->call(closest_A,closest_B);
+	if (d < 0.0)
+		d = 0.0;
+	else if (d > 1.0)
+		d = 1.0;
 
+	Vector3 closest_A = p_points_A[0] + rel_A * d;
+	Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
+	p_callback->call(closest_A, closest_B);
 }
 
-static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
+static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A <2 );
-	ERR_FAIL_COND( p_point_count_B <3 );
+	ERR_FAIL_COND(p_point_count_A < 2);
+	ERR_FAIL_COND(p_point_count_B < 3);
 #endif
 
-	static const int max_clip=32;
+	static const int max_clip = 32;
 
 	Vector3 _clipbuf1[max_clip];
 	Vector3 _clipbuf2[max_clip];
-	Vector3 *clipbuf_src=_clipbuf1;
-	Vector3 *clipbuf_dst=_clipbuf2;
-	int clipbuf_len=p_point_count_A;
+	Vector3 *clipbuf_src = _clipbuf1;
+	Vector3 *clipbuf_dst = _clipbuf2;
+	int clipbuf_len = p_point_count_A;
 
 	// copy A points to clipbuf_src
-	for (int i=0;i<p_point_count_A;i++) {
+	for (int i = 0; i < p_point_count_A; i++) {
 
-		clipbuf_src[i]=p_points_A[i];
+		clipbuf_src[i] = p_points_A[i];
 	}
 
-	Plane plane_B(p_points_B[0],p_points_B[1],p_points_B[2]);
+	Plane plane_B(p_points_B[0], p_points_B[1], p_points_B[2]);
 
 	// go through all of B points
-	for (int i=0;i<p_point_count_B;i++) {
+	for (int i = 0; i < p_point_count_B; i++) {
 
-		int i_n=(i+1)%p_point_count_B;
+		int i_n = (i + 1) % p_point_count_B;
 
-		Vector3 edge0_B=p_points_B[i];
-		Vector3 edge1_B=p_points_B[i_n];
+		Vector3 edge0_B = p_points_B[i];
+		Vector3 edge1_B = p_points_B[i_n];
 
-		Vector3 clip_normal = (edge0_B - edge1_B).cross( plane_B.normal ).normalized();
+		Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized();
 		// make a clip plane
 
-
-		Plane clip(edge0_B,clip_normal);
+		Plane clip(edge0_B, clip_normal);
 		// avoid double clip if A is edge
-		int dst_idx=0;
-		bool edge = clipbuf_len==2;
-		for (int j=0;j<clipbuf_len;j++) {
+		int dst_idx = 0;
+		bool edge = clipbuf_len == 2;
+		for (int j = 0; j < clipbuf_len; j++) {
 
-			int j_n=(j+1)%clipbuf_len;
+			int j_n = (j + 1) % clipbuf_len;
 
-			Vector3 edge0_A=clipbuf_src[j];
-			Vector3 edge1_A=clipbuf_src[j_n];
+			Vector3 edge0_A = clipbuf_src[j];
+			Vector3 edge1_A = clipbuf_src[j_n];
 
 			real_t dist0 = clip.distance_to(edge0_A);
 			real_t dist1 = clip.distance_to(edge1_A);
 
+			if (dist0 <= 0) { // behind plane
 
-			if ( dist0 <= 0 ) { // behind plane
-
-				ERR_FAIL_COND( dst_idx >= max_clip );
-				clipbuf_dst[dst_idx++]=clipbuf_src[j];
-
+				ERR_FAIL_COND(dst_idx >= max_clip);
+				clipbuf_dst[dst_idx++] = clipbuf_src[j];
 			}
 
-
 			// check for different sides and non coplanar
-//			if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) {
-			if ( (dist0*dist1) < 0 && !(edge && j)) {
+			//			if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) {
+			if ((dist0 * dist1) < 0 && !(edge && j)) {
 
 				// calculate intersection
 				Vector3 rel = edge1_A - edge0_A;
-				real_t den=clip.normal.dot( rel );
-				real_t dist=-(clip.normal.dot( edge0_A )-clip.d)/den;
-				Vector3 inters = edge0_A+rel*dist;
+				real_t den = clip.normal.dot(rel);
+				real_t dist = -(clip.normal.dot(edge0_A) - clip.d) / den;
+				Vector3 inters = edge0_A + rel * dist;
 
-				ERR_FAIL_COND( dst_idx >= max_clip );
-				clipbuf_dst[dst_idx]=inters;
+				ERR_FAIL_COND(dst_idx >= max_clip);
+				clipbuf_dst[dst_idx] = inters;
 				dst_idx++;
 			}
 		}
 
-		clipbuf_len=dst_idx;
-		SWAP(clipbuf_src,clipbuf_dst);
+		clipbuf_len = dst_idx;
+		SWAP(clipbuf_src, clipbuf_dst);
 	}
 
-
 	// generate contacts
 	//Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]);
 
-	int added=0;
+	int added = 0;
 
-	for (int i=0;i<clipbuf_len;i++) {
+	for (int i = 0; i < clipbuf_len; i++) {
 
 		float d = plane_B.distance_to(clipbuf_src[i]);
 		//if (d>CMP_EPSILON)
 		//	continue;
 
-		Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d;
+		Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d;
 
 		if (p_callback->normal.dot(clipbuf_src[i]) >= p_callback->normal.dot(closest_B))
 			continue;
 
-		p_callback->call(clipbuf_src[i],closest_B);
+		p_callback->call(clipbuf_src[i], closest_B);
 		added++;
-
 	}
-
 }
 
-
-static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) {
-
+static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A <1 );
-	ERR_FAIL_COND( p_point_count_B <1 );
+	ERR_FAIL_COND(p_point_count_A < 1);
+	ERR_FAIL_COND(p_point_count_B < 1);
 #endif
 
-
-	static const GenerateContactsFunc generate_contacts_func_table[3][3]={
+	static const GenerateContactsFunc generate_contacts_func_table[3][3] = {
+		{
+				_generate_contacts_point_point,
+				_generate_contacts_point_edge,
+				_generate_contacts_point_face,
+		},
 		{
-			_generate_contacts_point_point,
-			_generate_contacts_point_edge,
-			_generate_contacts_point_face,
-		},{
-			0,
-			_generate_contacts_edge_edge,
-			_generate_contacts_face_face,
-		},{
-			0,0,
-			_generate_contacts_face_face,
+				0,
+				_generate_contacts_edge_edge,
+				_generate_contacts_face_face,
+		},
+		{
+				0, 0,
+				_generate_contacts_face_face,
 		}
 	};
 
@@ -285,28 +267,25 @@ static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_po
 
 		pointcount_B = p_point_count_A;
 		pointcount_A = p_point_count_B;
-		points_A=p_points_B;
-		points_B=p_points_A;
+		points_A = p_points_B;
+		points_B = p_points_A;
 	} else {
 
 		pointcount_B = p_point_count_B;
 		pointcount_A = p_point_count_A;
-		points_A=p_points_A;
-		points_B=p_points_B;
+		points_A = p_points_A;
+		points_B = p_points_B;
 	}
 
-	int version_A = (pointcount_A > 3 ?  3 : pointcount_A) -1;
-	int version_B = (pointcount_B > 3 ?  3 : pointcount_B) -1;
+	int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1;
+	int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1;
 
 	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
 	ERR_FAIL_COND(!contacts_func);
-	contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_callback);
-
+	contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback);
 }
 
-
-
-template<class ShapeA, class ShapeB, bool withMargin=false>
+template <class ShapeA, class ShapeB, bool withMargin = false>
 class SeparatorAxisTest {
 
 	const ShapeA *shape_A;
@@ -321,46 +300,45 @@ class SeparatorAxisTest {
 	Vector3 separator_axis;
 
 public:
-
 	_FORCE_INLINE_ bool test_previous_axis() {
 
-		if (callback && callback->prev_axis && *callback->prev_axis!=Vector3())
+		if (callback && callback->prev_axis && *callback->prev_axis != Vector3())
 			return test_axis(*callback->prev_axis);
 		else
 			return true;
 	}
 
-	_FORCE_INLINE_ bool test_axis(const Vector3& p_axis) {
+	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
 
-		Vector3 axis=p_axis;
+		Vector3 axis = p_axis;
 
-		if (	Math::abs(axis.x)<CMP_EPSILON &&
-			Math::abs(axis.y)<CMP_EPSILON &&
-			Math::abs(axis.z)<CMP_EPSILON ) {
+		if (Math::abs(axis.x) < CMP_EPSILON &&
+				Math::abs(axis.y) < CMP_EPSILON &&
+				Math::abs(axis.z) < CMP_EPSILON) {
 			// strange case, try an upwards separator
-			axis=Vector3(0.0,1.0,0.0);
+			axis = Vector3(0.0, 1.0, 0.0);
 		}
 
-		real_t min_A,max_A,min_B,max_B;
+		real_t min_A, max_A, min_B, max_B;
 
-		shape_A->project_range(axis,*transform_A,min_A,max_A);
-		shape_B->project_range(axis,*transform_B,min_B,max_B);
+		shape_A->project_range(axis, *transform_A, min_A, max_A);
+		shape_B->project_range(axis, *transform_B, min_B, max_B);
 
 		if (withMargin) {
-			min_A-=margin_A;
-			max_A+=margin_A;
-			min_B-=margin_B;
-			max_B+=margin_B;
+			min_A -= margin_A;
+			max_A += margin_A;
+			min_B -= margin_B;
+			max_B += margin_B;
 		}
 
-		min_B -= ( max_A - min_A ) * 0.5;
-		max_B += ( max_A - min_A ) * 0.5;
+		min_B -= (max_A - min_A) * 0.5;
+		max_B += (max_A - min_A) * 0.5;
 
-		real_t dmin = min_B - ( min_A + max_A ) * 0.5;
-		real_t dmax = max_B - ( min_A + max_A ) * 0.5;
+		real_t dmin = min_B - (min_A + max_A) * 0.5;
+		real_t dmax = max_B - (min_A + max_A) * 0.5;
 
 		if (dmin > 0.0 || dmax < 0.0) {
-			separator_axis=axis;
+			separator_axis = axis;
 			return false; // doesn't contain 0
 		}
 
@@ -368,68 +346,65 @@ public:
 
 		dmin = Math::abs(dmin);
 
-		if ( dmax < dmin ) {
-			if ( dmax < best_depth ) {
-				best_depth=dmax;
-				best_axis=axis;
+		if (dmax < dmin) {
+			if (dmax < best_depth) {
+				best_depth = dmax;
+				best_axis = axis;
 			}
 		} else {
-			if ( dmin < best_depth ) {
-				best_depth=dmin;
-				best_axis=-axis; // keep it as A axis
+			if (dmin < best_depth) {
+				best_depth = dmin;
+				best_axis = -axis; // keep it as A axis
 			}
 		}
 
 		return true;
 	}
 
-
 	_FORCE_INLINE_ void generate_contacts() {
 
 		// nothing to do, don't generate
-		if (best_axis==Vector3(0.0,0.0,0.0))
+		if (best_axis == Vector3(0.0, 0.0, 0.0))
 			return;
 
 		if (!callback->callback) {
 			//just was checking intersection?
-			callback->collided=true;
+			callback->collided = true;
 			if (callback->prev_axis)
-				*callback->prev_axis=best_axis;
+				*callback->prev_axis = best_axis;
 			return;
 		}
 
-		static const int max_supports=16;
+		static const int max_supports = 16;
 
 		Vector3 supports_A[max_supports];
 		int support_count_A;
-		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(),max_supports,supports_A,support_count_A);
-		for(int i=0;i<support_count_A;i++) {
+		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A);
+		for (int i = 0; i < support_count_A; i++) {
 			supports_A[i] = transform_A->xform(supports_A[i]);
 		}
 
 		if (withMargin) {
 
-			for(int i=0;i<support_count_A;i++) {
-				supports_A[i]+=-best_axis*margin_A;
+			for (int i = 0; i < support_count_A; i++) {
+				supports_A[i] += -best_axis * margin_A;
 			}
-
 		}
 
-
 		Vector3 supports_B[max_supports];
 		int support_count_B;
-		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(),max_supports,supports_B,support_count_B);
-		for(int i=0;i<support_count_B;i++) {
+		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B);
+		for (int i = 0; i < support_count_B; i++) {
 			supports_B[i] = transform_B->xform(supports_B[i]);
 		}
 
 		if (withMargin) {
 
-			for(int i=0;i<support_count_B;i++) {
-				supports_B[i]+=best_axis*margin_B;
+			for (int i = 0; i < support_count_B; i++) {
+				supports_B[i] += best_axis * margin_B;
 			}
 		}
-/*
+		/*
 		print_line("best depth: "+rtos(best_depth));
 		print_line("best axis: "+(best_axis));
 		for(int i=0;i<support_count_A;i++) {
@@ -441,29 +416,26 @@ public:
 			print_line("B-"+itos(i)+": "+supports_B[i]);
 		}
 */
-		callback->normal=best_axis;
+		callback->normal = best_axis;
 		if (callback->prev_axis)
-			*callback->prev_axis=best_axis;
-		_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
+			*callback->prev_axis = best_axis;
+		_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
 
-		callback->collided=true;
+		callback->collided = true;
 		//CollisionSolverSW::CallbackResult cbk=NULL;
 		//cbk(Vector3(),Vector3(),NULL);
-
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback,real_t p_margin_A=0,real_t p_margin_B=0) {
-		best_depth=1e15;
-		shape_A=p_shape_A;
-		shape_B=p_shape_B;
-		transform_A=&p_transform_A;
-		transform_B=&p_transform_B;
-		callback=p_callback;
-		margin_A=p_margin_A;
-		margin_B=p_margin_B;
-
+	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform &p_transform_A, const ShapeB *p_shape_B, const Transform &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) {
+		best_depth = 1e15;
+		shape_A = p_shape_A;
+		shape_B = p_shape_B;
+		transform_A = &p_transform_A;
+		transform_B = &p_transform_B;
+		callback = p_callback;
+		margin_A = p_margin_A;
+		margin_B = p_margin_B;
 	}
-
 };
 
 /****** SAT TESTS *******/
@@ -471,92 +443,84 @@ public:
 /****** SAT TESTS *******/
 /****** SAT TESTS *******/
 
+typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, float, float);
 
-typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback,float,float);
-
-
-template<bool withMargin>
-static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_sphere_sphere(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
+	const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW *>(p_b);
 
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b);
-
-	SeparatorAxisTest<SphereShapeSW,SphereShapeSW,withMargin> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<SphereShapeSW, SphereShapeSW, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	// previous axis
 
 	if (!separator.test_previous_axis())
 		return;
 
-	if (!separator.test_axis( (p_transform_a.origin-p_transform_b.origin).normalized() ))
+	if (!separator.test_axis((p_transform_a.origin - p_transform_b.origin).normalized()))
 		return;
 
 	separator.generate_contacts();
 }
 
-template<bool withMargin>
-static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
+template <bool withMargin>
+static void _collision_sphere_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
+	const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,BoxShapeSW,withMargin> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<SphereShapeSW, BoxShapeSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
 	// test faces
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
-
 	}
 
 	// calculate closest point to sphere
 
-	Vector3 cnormal=p_transform_b.xform_inv( p_transform_a.origin );
+	Vector3 cnormal = p_transform_b.xform_inv(p_transform_a.origin);
 
-	Vector3 cpoint=p_transform_b.xform( Vector3(
+	Vector3 cpoint = p_transform_b.xform(Vector3(
 
-		(cnormal.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
-		(cnormal.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
-		(cnormal.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z
-	) );
+			(cnormal.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
+			(cnormal.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
+			(cnormal.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z));
 
 	// use point to test axis
 	Vector3 point_axis = (p_transform_a.origin - cpoint).normalized();
 
-	if (!separator.test_axis( point_axis  ))
+	if (!separator.test_axis(point_axis))
 		return;
 
 	// test edges
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		Vector3 axis = point_axis.cross( p_transform_b.basis.get_axis(i) ).cross( p_transform_b.basis.get_axis(i) ).normalized();
+		Vector3 axis = point_axis.cross(p_transform_b.basis.get_axis(i)).cross(p_transform_b.basis.get_axis(i)).normalized();
 
-		if (!separator.test_axis( axis  ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	separator.generate_contacts();
-
-
 }
 
-template<bool withMargin>
-static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b);
 
-	SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW,withMargin> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<SphereShapeSW, CapsuleShapeSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -567,38 +531,35 @@ static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_tran
 
 	Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis;
 
-	if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) )
+	if (!separator.test_axis((capsule_ball_1 - p_transform_a.origin).normalized()))
 		return;
 
 	//capsule sphere 2, sphere
 
 	Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis;
 
-	if (!separator.test_axis( (capsule_ball_2 - p_transform_a.origin).normalized() ) )
+	if (!separator.test_axis((capsule_ball_2 - p_transform_a.origin).normalized()))
 		return;
 
 	//capsule edge, sphere
 
 	Vector3 b2a = p_transform_a.origin - p_transform_b.origin;
 
-	Vector3 axis = b2a.cross( capsule_axis ).cross( capsule_axis ).normalized();
-
+	Vector3 axis = b2a.cross(capsule_axis).cross(capsule_axis).normalized();
 
-	if (!separator.test_axis( axis ))
+	if (!separator.test_axis(axis))
 		return;
 
 	separator.generate_contacts();
 }
 
-template<bool withMargin>
-static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_sphere_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b);
 
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
-
-	SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW,withMargin> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
-
+	SeparatorAxisTest<SphereShapeSW, ConvexPolygonShapeSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -612,146 +573,127 @@ static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform
 	const Vector3 *vertices = mesh.vertices.ptr();
 	int vertex_count = mesh.vertices.size();
 
-
 	// faces of B
-	for (int i=0;i<face_count;i++) {
+	for (int i = 0; i < face_count; i++) {
 
-		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
-
 	// edges of B
-	for(int i=0;i<edge_count;i++) {
+	for (int i = 0; i < edge_count; i++) {
 
+		Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]);
+		Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]);
+		Vector3 v3 = p_transform_a.origin;
 
-		Vector3 v1=p_transform_b.xform( vertices[ edges[i].a ] );
-		Vector3 v2=p_transform_b.xform( vertices[ edges[i].b ] );
-		Vector3 v3=p_transform_a.origin;
-
-
-		Vector3 n1=v2-v1;
-		Vector3 n2=v2-v3;
+		Vector3 n1 = v2 - v1;
+		Vector3 n2 = v2 - v3;
 
 		Vector3 axis = n1.cross(n2).cross(n1).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
-
 	}
 
 	// vertices of B
-	for(int i=0;i<vertex_count;i++) {
-
+	for (int i = 0; i < vertex_count; i++) {
 
-		Vector3 v1=p_transform_b.xform( vertices[i] );
-		Vector3 v2=p_transform_a.origin;
+		Vector3 v1 = p_transform_b.xform(vertices[i]);
+		Vector3 v2 = p_transform_a.origin;
 
-		Vector3 axis = (v2-v1).normalized();
+		Vector3 axis = (v2 - v1).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
-
 	}
 
 	separator.generate_contacts();
-
-
 }
 
-template<bool withMargin>
-static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+template <bool withMargin>
+static void _collision_sphere_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
+	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b);
 
+	SeparatorAxisTest<SphereShapeSW, FaceShapeSW, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
-	SeparatorAxisTest<SphereShapeSW,FaceShapeSW,withMargin> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
-
-
-	Vector3 vertex[3]={
-		p_transform_b.xform( face_B->vertex[0] ),
-		p_transform_b.xform( face_B->vertex[1] ),
-		p_transform_b.xform( face_B->vertex[2] ),
+	Vector3 vertex[3] = {
+		p_transform_b.xform(face_B->vertex[0]),
+		p_transform_b.xform(face_B->vertex[1]),
+		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
 		return;
 
 	// edges and points of B
-	for(int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
+		Vector3 n1 = vertex[i] - p_transform_a.origin;
 
-		Vector3 n1=vertex[i]-p_transform_a.origin;
-
-		if (!separator.test_axis( n1.normalized() )) {
+		if (!separator.test_axis(n1.normalized())) {
 			return;
 		}
 
-		Vector3 n2=vertex[(i+1)%3]-vertex[i];
+		Vector3 n2 = vertex[(i + 1) % 3] - vertex[i];
 
 		Vector3 axis = n1.cross(n2).cross(n2).normalized();
 
-		if (!separator.test_axis( axis )) {
+		if (!separator.test_axis(axis)) {
 			return;
 		}
-
 	}
 
 	separator.generate_contacts();
 }
 
+template <bool withMargin>
+static void _collision_box_box(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
+	const BoxShapeSW *box_B = static_cast<const BoxShapeSW *>(p_b);
 
-
-	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
-	const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b);
-
-	SeparatorAxisTest<BoxShapeSW,BoxShapeSW,withMargin> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<BoxShapeSW, BoxShapeSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
 	// test faces of A
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
-
 	}
 
 	// test faces of B
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_b.basis.get_axis(i).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
-
 	}
 
 	// test combined edges
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		for (int j=0;j<3;j++) {
+		for (int j = 0; j < 3; j++) {
 
-			Vector3 axis = p_transform_a.basis.get_axis(i).cross( p_transform_b.basis.get_axis(j) );
+			Vector3 axis = p_transform_a.basis.get_axis(i).cross(p_transform_b.basis.get_axis(j));
 
-			if (axis.length_squared()<CMP_EPSILON)
+			if (axis.length_squared() < CMP_EPSILON)
 				continue;
 			axis.normalize();
 
-
-			if (!separator.test_axis( axis  )) {
+			if (!separator.test_axis(axis)) {
 				return;
 			}
 		}
@@ -764,110 +706,103 @@ static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a
 
 		Vector3 ab_vec = p_transform_b.origin - p_transform_a.origin;
 
-		Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
-
-		Vector3 support_a=p_transform_a.xform( Vector3(
+		Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-			(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
-			(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
-			(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
-		) );
+		Vector3 support_a = p_transform_a.xform(Vector3(
 
+				(cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+				(cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+				(cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
 
-		Vector3 cnormal_b=p_transform_b.basis.xform_inv( -ab_vec );
+		Vector3 cnormal_b = p_transform_b.basis.xform_inv(-ab_vec);
 
-		Vector3 support_b=p_transform_b.xform( Vector3(
+		Vector3 support_b = p_transform_b.xform(Vector3(
 
-			(cnormal_b.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
-			(cnormal_b.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
-			(cnormal_b.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z
-		) );
+				(cnormal_b.x < 0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x,
+				(cnormal_b.y < 0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y,
+				(cnormal_b.z < 0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z));
 
-		Vector3 axis_ab = (support_a-support_b);
+		Vector3 axis_ab = (support_a - support_b);
 
-		if (!separator.test_axis( axis_ab.normalized()  )) {
+		if (!separator.test_axis(axis_ab.normalized())) {
 			return;
 		}
 
 		//now try edges, which become cylinders!
 
-		for(int i=0;i<3;i++) {
+		for (int i = 0; i < 3; i++) {
 
 			//a ->b
 			Vector3 axis_a = p_transform_a.basis.get_axis(i);
 
-			if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+			if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
 				return;
 
 			//b ->a
 			Vector3 axis_b = p_transform_b.basis.get_axis(i);
 
-			if (!separator.test_axis( axis_ab.cross(axis_b).cross(axis_b).normalized() ))
+			if (!separator.test_axis(axis_ab.cross(axis_b).cross(axis_b).normalized()))
 				return;
-
 		}
 	}
 
 	separator.generate_contacts();
-
-
 }
 
-template<bool withMargin>
-static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
-	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b);
 
-	SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW,withMargin> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<BoxShapeSW, CapsuleShapeSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
 	// faces of A
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_a.basis.get_axis(i);
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
-
 	Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized();
 
 	// edges of A, capsule cylinder
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		// cylinder
 		Vector3 box_axis = p_transform_a.basis.get_axis(i);
-		Vector3 axis = box_axis.cross( cyl_axis );
+		Vector3 axis = box_axis.cross(cyl_axis);
 		if (axis.length_squared() < CMP_EPSILON)
 			continue;
 
-		if (!separator.test_axis( axis.normalized() ))
+		if (!separator.test_axis(axis.normalized()))
 			return;
 	}
 
 	// points of A, capsule cylinder
 	// this sure could be made faster somehow..
 
-	for (int i=0;i<2;i++) {
-		for (int j=0;j<2;j++) {
-			for (int k=0;k<2;k++) {
+	for (int i = 0; i < 2; i++) {
+		for (int j = 0; j < 2; j++) {
+			for (int k = 0; k < 2; k++) {
 				Vector3 he = box_A->get_half_extents();
-				he.x*=(i*2-1);
-				he.y*=(j*2-1);
-				he.z*=(k*2-1);
-				Vector3 point=p_transform_a.origin;
-				for(int l=0;l<3;l++)
-					point+=p_transform_a.basis.get_axis(l)*he[l];
+				he.x *= (i * 2 - 1);
+				he.y *= (j * 2 - 1);
+				he.z *= (k * 2 - 1);
+				Vector3 point = p_transform_a.origin;
+				for (int l = 0; l < 3; l++)
+					point += p_transform_a.basis.get_axis(l) * he[l];
 
 				//Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized();
-				Vector3 axis = Plane(cyl_axis,0).project(point).normalized();
+				Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
 
-				if (!separator.test_axis( axis ))
+				if (!separator.test_axis(axis))
 					return;
 			}
 		}
@@ -875,58 +810,51 @@ static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transfo
 
 	// capsule balls, edges of A
 
-	for (int i=0;i<2;i++) {
-
+	for (int i = 0; i < 2; i++) {
 
-		Vector3 capsule_axis = p_transform_b.basis.get_axis(2)*(capsule_B->get_height()*0.5);
+		Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5);
 
-		Vector3 sphere_pos = p_transform_b.origin + ((i==0)?capsule_axis:-capsule_axis);
+		Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
+		Vector3 cnormal = p_transform_a.xform_inv(sphere_pos);
 
-		Vector3 cnormal=p_transform_a.xform_inv( sphere_pos );
+		Vector3 cpoint = p_transform_a.xform(Vector3(
 
-		Vector3 cpoint=p_transform_a.xform( Vector3(
-
-			(cnormal.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
-			(cnormal.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
-			(cnormal.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
-		) );
+				(cnormal.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+				(cnormal.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+				(cnormal.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
 
 		// use point to test axis
 		Vector3 point_axis = (sphere_pos - cpoint).normalized();
 
-		if (!separator.test_axis( point_axis  ))
+		if (!separator.test_axis(point_axis))
 			return;
 
 		// test edges of A
 
-		for (int i=0;i<3;i++) {
+		for (int i = 0; i < 3; i++) {
 
-			Vector3 axis = point_axis.cross( p_transform_a.basis.get_axis(i) ).cross( p_transform_a.basis.get_axis(i) ).normalized();
+			Vector3 axis = point_axis.cross(p_transform_a.basis.get_axis(i)).cross(p_transform_a.basis.get_axis(i)).normalized();
 
-			if (!separator.test_axis( axis  ))
+			if (!separator.test_axis(axis))
 				return;
 		}
 	}
 
-
 	separator.generate_contacts();
 }
 
-template<bool withMargin>
-static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_box_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b);
 
-
-	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
-	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
-
-	SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW,withMargin> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<BoxShapeSW, ConvexPolygonShapeSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
 
-
 	const Geometry::MeshData &mesh = convex_polygon_B->get_mesh();
 
 	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
@@ -937,97 +865,92 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_
 	int vertex_count = mesh.vertices.size();
 
 	// faces of A
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// faces of B
-	for (int i=0;i<face_count;i++) {
+	for (int i = 0; i < face_count; i++) {
 
-		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// A<->B edges
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 e1 = p_transform_a.basis.get_axis(i);
 
-		for (int j=0;j<edge_count;j++) {
+		for (int j = 0; j < edge_count; j++) {
 
-			Vector3 e2=p_transform_b.basis.xform(vertices[edges[j].a]) -  p_transform_b.basis.xform(vertices[edges[j].b]);
+			Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
 
-			Vector3 axis=e1.cross( e2 ).normalized();
+			Vector3 axis = e1.cross(e2).normalized();
 
-			if (!separator.test_axis( axis ))
+			if (!separator.test_axis(axis))
 				return;
-
 		}
 	}
 
 	if (withMargin) {
 
 		// calculate closest points between vertices and box edges
-		for(int v=0;v<vertex_count;v++) {
-
+		for (int v = 0; v < vertex_count; v++) {
 
 			Vector3 vtxb = p_transform_b.xform(vertices[v]);
 			Vector3 ab_vec = vtxb - p_transform_a.origin;
 
-			Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
-
-			Vector3 support_a=p_transform_a.xform( Vector3(
+			Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-				(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
-				(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
-				(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
-			) );
+			Vector3 support_a = p_transform_a.xform(Vector3(
 
+					(cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+					(cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+					(cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
 
-			Vector3 axis_ab = support_a-vtxb;
+			Vector3 axis_ab = support_a - vtxb;
 
-			if (!separator.test_axis( axis_ab.normalized()  )) {
+			if (!separator.test_axis(axis_ab.normalized())) {
 				return;
 			}
 
 			//now try edges, which become cylinders!
 
-			for(int i=0;i<3;i++) {
+			for (int i = 0; i < 3; i++) {
 
 				//a ->b
 				Vector3 axis_a = p_transform_a.basis.get_axis(i);
 
-				if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+				if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
 					return;
 			}
 		}
 
 		//convex edges and box points
-		for (int i=0;i<2;i++) {
-			for (int j=0;j<2;j++) {
-				for (int k=0;k<2;k++) {
+		for (int i = 0; i < 2; i++) {
+			for (int j = 0; j < 2; j++) {
+				for (int k = 0; k < 2; k++) {
 					Vector3 he = box_A->get_half_extents();
-					he.x*=(i*2-1);
-					he.y*=(j*2-1);
-					he.z*=(k*2-1);
-					Vector3 point=p_transform_a.origin;
-					for(int l=0;l<3;l++)
-						point+=p_transform_a.basis.get_axis(l)*he[l];
+					he.x *= (i * 2 - 1);
+					he.y *= (j * 2 - 1);
+					he.z *= (k * 2 - 1);
+					Vector3 point = p_transform_a.origin;
+					for (int l = 0; l < 3; l++)
+						point += p_transform_a.basis.get_axis(l) * he[l];
 
-					for(int e=0;e<edge_count;e++) {
+					for (int e = 0; e < edge_count; e++) {
 
-						Vector3 p1=p_transform_b.xform(vertices[edges[e].a]);
-						Vector3 p2=p_transform_b.xform(vertices[edges[e].b]);
-						Vector3 n = (p2-p1);
+						Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]);
+						Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]);
+						Vector3 n = (p2 - p1);
 
-
-						if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() ))
+						if (!separator.test_axis((point - p2).cross(n).cross(n).normalized()))
 							return;
 					}
 				}
@@ -1036,130 +959,119 @@ static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_
 	}
 
 	separator.generate_contacts();
-
-
 }
 
+template <bool withMargin>
+static void _collision_box_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
+	const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b);
 
-	const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a);
-	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
+	SeparatorAxisTest<BoxShapeSW, FaceShapeSW, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
-	SeparatorAxisTest<BoxShapeSW,FaceShapeSW,withMargin> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
-
-	Vector3 vertex[3]={
-		p_transform_b.xform( face_B->vertex[0] ),
-		p_transform_b.xform( face_B->vertex[1] ),
-		p_transform_b.xform( face_B->vertex[2] ),
+	Vector3 vertex[3] = {
+		p_transform_b.xform(face_B->vertex[0]),
+		p_transform_b.xform(face_B->vertex[1]),
+		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
 		return;
 
 	// faces of A
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// combined edges
 
-	for(int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		Vector3 e=vertex[i]-vertex[(i+1)%3];
+		Vector3 e = vertex[i] - vertex[(i + 1) % 3];
 
-		for (int j=0;j<3;j++) {
+		for (int j = 0; j < 3; j++) {
 
 			Vector3 axis = p_transform_a.basis.get_axis(j);
 
-			if (!separator.test_axis( e.cross(axis).normalized() ))
+			if (!separator.test_axis(e.cross(axis).normalized()))
 				return;
 		}
-
 	}
 
 	if (withMargin) {
 
 		// calculate closest points between vertices and box edges
-		for(int v=0;v<3;v++) {
-
+		for (int v = 0; v < 3; v++) {
 
 			Vector3 ab_vec = vertex[v] - p_transform_a.origin;
 
-			Vector3 cnormal_a=p_transform_a.basis.xform_inv( ab_vec );
-
-			Vector3 support_a=p_transform_a.xform( Vector3(
+			Vector3 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-				(cnormal_a.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
-				(cnormal_a.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
-				(cnormal_a.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z
-			) );
+			Vector3 support_a = p_transform_a.xform(Vector3(
 
+					(cnormal_a.x < 0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x,
+					(cnormal_a.y < 0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y,
+					(cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
 
-			Vector3 axis_ab = support_a-vertex[v];
+			Vector3 axis_ab = support_a - vertex[v];
 
-			if (!separator.test_axis( axis_ab.normalized()  )) {
+			if (!separator.test_axis(axis_ab.normalized())) {
 				return;
 			}
 
 			//now try edges, which become cylinders!
 
-			for(int i=0;i<3;i++) {
+			for (int i = 0; i < 3; i++) {
 
 				//a ->b
 				Vector3 axis_a = p_transform_a.basis.get_axis(i);
 
-				if (!separator.test_axis( axis_ab.cross(axis_a).cross(axis_a).normalized() ))
+				if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized()))
 					return;
 			}
 		}
 
 		//convex edges and box points, there has to be a way to speed up this (get closest point?)
-		for (int i=0;i<2;i++) {
-			for (int j=0;j<2;j++) {
-				for (int k=0;k<2;k++) {
+		for (int i = 0; i < 2; i++) {
+			for (int j = 0; j < 2; j++) {
+				for (int k = 0; k < 2; k++) {
 					Vector3 he = box_A->get_half_extents();
-					he.x*=(i*2-1);
-					he.y*=(j*2-1);
-					he.z*=(k*2-1);
-					Vector3 point=p_transform_a.origin;
-					for(int l=0;l<3;l++)
-						point+=p_transform_a.basis.get_axis(l)*he[l];
+					he.x *= (i * 2 - 1);
+					he.y *= (j * 2 - 1);
+					he.z *= (k * 2 - 1);
+					Vector3 point = p_transform_a.origin;
+					for (int l = 0; l < 3; l++)
+						point += p_transform_a.basis.get_axis(l) * he[l];
 
-					for(int e=0;e<3;e++) {
+					for (int e = 0; e < 3; e++) {
 
-						Vector3 p1=vertex[e];
-						Vector3 p2=vertex[(e+1)%3];
+						Vector3 p1 = vertex[e];
+						Vector3 p2 = vertex[(e + 1) % 3];
 
-						Vector3 n = (p2-p1);
+						Vector3 n = (p2 - p1);
 
-						if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() ))
+						if (!separator.test_axis((point - p2).cross(n).cross(n).normalized()))
 							return;
 					}
 				}
 			}
 		}
-
 	}
 
 	separator.generate_contacts();
-
 }
 
+template <bool withMargin>
+static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a);
+	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW *>(p_b);
 
-	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
-	const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b);
-
-	SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<CapsuleShapeSW, CapsuleShapeSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1176,49 +1088,45 @@ static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_tra
 
 	//balls-balls
 
-	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).normalized()))
 		return;
-	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).normalized()))
 		return;
 
-	if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_1 ).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_1).normalized()))
 		return;
-	if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_2 ).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_2 - capsule_B_ball_2).normalized()))
 		return;
 
-
 	// edges-balls
 
-	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_1).cross(capsule_A_axis).cross(capsule_A_axis).normalized()))
 		return;
 
-	if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) )
+	if (!separator.test_axis((capsule_A_ball_1 - capsule_B_ball_2).cross(capsule_A_axis).cross(capsule_A_axis).normalized()))
 		return;
 
-	if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_1 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) )
+	if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_1).cross(capsule_B_axis).cross(capsule_B_axis).normalized()))
 		return;
 
-	if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_2 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) )
+	if (!separator.test_axis((capsule_B_ball_1 - capsule_A_ball_2).cross(capsule_B_axis).cross(capsule_B_axis).normalized()))
 		return;
 
 	// edges
 
-	if (!separator.test_axis( capsule_A_axis.cross(capsule_B_axis).normalized() ) )
+	if (!separator.test_axis(capsule_A_axis.cross(capsule_B_axis).normalized()))
 		return;
 
-
 	separator.generate_contacts();
-
 }
 
-template<bool withMargin>
-static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
+template <bool withMargin>
+static void _collision_capsule_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b);
 
-	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
-	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
-
-	SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW,withMargin> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<CapsuleShapeSW, ConvexPolygonShapeSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1232,127 +1140,113 @@ static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform
 	const Vector3 *vertices = mesh.vertices.ptr();
 
 	// faces of B
-	for (int i=0;i<face_count;i++) {
+	for (int i = 0; i < face_count; i++) {
 
-		Vector3 axis = p_transform_b.xform( faces[i].plane ).normal;
+		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// edges of B, capsule cylinder
 
-	for (int i=0;i<edge_count;i++) {
+	for (int i = 0; i < edge_count; i++) {
 
 		// cylinder
-		Vector3 edge_axis = p_transform_b.basis.xform( vertices[ edges[i].a] ) - p_transform_b.basis.xform( vertices[ edges[i].b] );
-		Vector3 axis = edge_axis.cross( p_transform_a.basis.get_axis(2) ).normalized();
-
+		Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]);
+		Vector3 axis = edge_axis.cross(p_transform_a.basis.get_axis(2)).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// capsule balls, edges of B
 
-	for (int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
 		// edges of B, capsule cylinder
 
-		Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5);
+		Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5);
 
-		Vector3 sphere_pos = p_transform_a.origin + ((i==0)?capsule_axis:-capsule_axis);
+		Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
-		for (int j=0;j<edge_count;j++) {
+		for (int j = 0; j < edge_count; j++) {
 
-
-			Vector3 n1=sphere_pos - p_transform_b.xform( vertices[ edges[j].a] );
-			Vector3 n2=p_transform_b.basis.xform( vertices[ edges[j].a] ) - p_transform_b.basis.xform( vertices[ edges[j].b] );
+			Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]);
+			Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
 
 			Vector3 axis = n1.cross(n2).cross(n2).normalized();
 
-			if (!separator.test_axis( axis ))
+			if (!separator.test_axis(axis))
 				return;
 		}
 	}
 
-
 	separator.generate_contacts();
-
 }
 
+template <bool withMargin>
+static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
-	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a);
-	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
-
-	SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW,withMargin> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
-
+	const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b);
 
+	SeparatorAxisTest<CapsuleShapeSW, FaceShapeSW, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
-	Vector3 vertex[3]={
-		p_transform_b.xform( face_B->vertex[0] ),
-		p_transform_b.xform( face_B->vertex[1] ),
-		p_transform_b.xform( face_B->vertex[2] ),
+	Vector3 vertex[3] = {
+		p_transform_b.xform(face_B->vertex[0]),
+		p_transform_b.xform(face_B->vertex[1]),
+		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
 		return;
 
 	// edges of B, capsule cylinder
 
-	Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5);
+	Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5);
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		// edge-cylinder
-		Vector3 edge_axis = vertex[i]-vertex[(i+1)%3];
-		Vector3 axis = edge_axis.cross( capsule_axis ).normalized();
+		Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
+		Vector3 axis = edge_axis.cross(capsule_axis).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 
-		if (!separator.test_axis( (p_transform_a.origin-vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized() ))
+		if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized()))
 			return;
 
-		for (int j=0;j<2;j++) {
+		for (int j = 0; j < 2; j++) {
 
 			// point-spheres
-			Vector3 sphere_pos = p_transform_a.origin + ( (j==0) ? capsule_axis : -capsule_axis );
+			Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis);
 
-			Vector3 n1=sphere_pos - vertex[i];
+			Vector3 n1 = sphere_pos - vertex[i];
 
-			if (!separator.test_axis( n1.normalized() ))
+			if (!separator.test_axis(n1.normalized()))
 				return;
 
-			Vector3 n2=edge_axis;
+			Vector3 n2 = edge_axis;
 
 			axis = n1.cross(n2).cross(n2);
 
-			if (!separator.test_axis( axis.normalized() ))
+			if (!separator.test_axis(axis.normalized()))
 				return;
-
-
 		}
-
 	}
 
-
 	separator.generate_contacts();
-
 }
 
+template <bool withMargin>
+static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
+	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a);
+	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b);
 
-	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
-	const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b);
-
-	SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<ConvexPolygonShapeSW, ConvexPolygonShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1376,107 +1270,97 @@ static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Tr
 	int vertex_count_B = mesh_B.vertices.size();
 
 	// faces of A
-	for (int i=0;i<face_count_A;i++) {
+	for (int i = 0; i < face_count_A; i++) {
 
-		Vector3 axis = p_transform_a.xform( faces_A[i].plane ).normal;
-//		Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
+		Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal;
+		//		Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// faces of B
-	for (int i=0;i<face_count_B;i++) {
-
-		Vector3 axis = p_transform_b.xform( faces_B[i].plane ).normal;
-//		Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
+	for (int i = 0; i < face_count_B; i++) {
 
+		Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal;
+		//		Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
 	// A<->B edges
-	for (int i=0;i<edge_count_A;i++) {
+	for (int i = 0; i < edge_count_A; i++) {
 
-		Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ) -p_transform_a.basis.xform( vertices_A[ edges_A[i].b] );
+		Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
 
-		for (int j=0;j<edge_count_B;j++) {
+		for (int j = 0; j < edge_count_B; j++) {
 
-			Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[j].a] ) -p_transform_b.basis.xform( vertices_B[ edges_B[j].b] );
+			Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]);
 
-			Vector3 axis=e1.cross( e2 ).normalized();
+			Vector3 axis = e1.cross(e2).normalized();
 
-			if (!separator.test_axis( axis ))
+			if (!separator.test_axis(axis))
 				return;
-
 		}
 	}
 
 	if (withMargin) {
 
 		//vertex-vertex
-		for(int i=0;i<vertex_count_A;i++) {
+		for (int i = 0; i < vertex_count_A; i++) {
 
 			Vector3 va = p_transform_a.xform(vertices_A[i]);
 
-			for(int j=0;j<vertex_count_B;j++) {
+			for (int j = 0; j < vertex_count_B; j++) {
 
-				if (!separator.test_axis( (va-p_transform_b.xform(vertices_B[j])).normalized() ))
+				if (!separator.test_axis((va - p_transform_b.xform(vertices_B[j])).normalized()))
 					return;
-
 			}
 		}
 		//edge-vertex( hsell)
 
-		for (int i=0;i<edge_count_A;i++) {
+		for (int i = 0; i < edge_count_A; i++) {
 
-			Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] );
-			Vector3 e2=p_transform_a.basis.xform( vertices_A[ edges_A[i].b] );
-			Vector3 n = (e2-e1);
+			Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]);
+			Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
+			Vector3 n = (e2 - e1);
 
-			for(int j=0;j<vertex_count_B;j++) {
+			for (int j = 0; j < vertex_count_B; j++) {
 
-				Vector3 e3=p_transform_b.xform(vertices_B[j]);
+				Vector3 e3 = p_transform_b.xform(vertices_B[j]);
 
-
-				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized()))
 					return;
 			}
 		}
 
-		for (int i=0;i<edge_count_B;i++) {
-
-			Vector3 e1=p_transform_b.basis.xform( vertices_B[ edges_B[i].a] );
-			Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[i].b] );
-			Vector3 n = (e2-e1);
+		for (int i = 0; i < edge_count_B; i++) {
 
-			for(int j=0;j<vertex_count_A;j++) {
+			Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]);
+			Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]);
+			Vector3 n = (e2 - e1);
 
-				Vector3 e3=p_transform_a.xform(vertices_A[j]);
+			for (int j = 0; j < vertex_count_A; j++) {
 
+				Vector3 e3 = p_transform_a.xform(vertices_A[j]);
 
-				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized()))
 					return;
 			}
 		}
-
-
 	}
 
 	separator.generate_contacts();
-
 }
 
+template <bool withMargin>
+static void _collision_convex_polygon_face(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, float p_margin_a, float p_margin_b) {
 
-template<bool withMargin>
-static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b,_CollectorCallback *p_collector,float p_margin_a,float p_margin_b) {
-
+	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW *>(p_a);
+	const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b);
 
-	const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a);
-	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b);
-
-	SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW,withMargin> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector,p_margin_a,p_margin_b);
+	SeparatorAxisTest<ConvexPolygonShapeSW, FaceShapeSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	const Geometry::MeshData &mesh = convex_polygon_A->get_mesh();
 
@@ -1487,207 +1371,192 @@ static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p
 	const Vector3 *vertices = mesh.vertices.ptr();
 	int vertex_count = mesh.vertices.size();
 
-
-
-	Vector3 vertex[3]={
-		p_transform_b.xform( face_B->vertex[0] ),
-		p_transform_b.xform( face_B->vertex[1] ),
-		p_transform_b.xform( face_B->vertex[2] ),
+	Vector3 vertex[3] = {
+		p_transform_b.xform(face_B->vertex[0]),
+		p_transform_b.xform(face_B->vertex[1]),
+		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() ))
+	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized()))
 		return;
 
-
 	// faces of A
-	for (int i=0;i<face_count;i++) {
+	for (int i = 0; i < face_count; i++) {
 
-//		Vector3 axis = p_transform_a.xform( faces[i].plane ).normal;
-		Vector3 axis = p_transform_a.basis.xform( faces[i].plane.normal ).normalized();
+		//		Vector3 axis = p_transform_a.xform( faces[i].plane ).normal;
+		Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized();
 
-		if (!separator.test_axis( axis ))
+		if (!separator.test_axis(axis))
 			return;
 	}
 
-
 	// A<->B edges
-	for (int i=0;i<edge_count;i++) {
+	for (int i = 0; i < edge_count; i++) {
 
-		Vector3 e1=p_transform_a.xform( vertices[edges[i].a] ) - p_transform_a.xform( vertices[edges[i].b] );
+		Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]);
 
-		for (int j=0;j<3;j++) {
+		for (int j = 0; j < 3; j++) {
 
-			Vector3 e2=vertex[j]-vertex[(j+1)%3];
+			Vector3 e2 = vertex[j] - vertex[(j + 1) % 3];
 
-			Vector3 axis=e1.cross( e2 ).normalized();
+			Vector3 axis = e1.cross(e2).normalized();
 
-			if (!separator.test_axis( axis ))
+			if (!separator.test_axis(axis))
 				return;
 		}
 	}
 
-
 	if (withMargin) {
 
 		//vertex-vertex
-		for(int i=0;i<vertex_count;i++) {
+		for (int i = 0; i < vertex_count; i++) {
 
 			Vector3 va = p_transform_a.xform(vertices[i]);
 
-			for(int j=0;j<3;j++) {
+			for (int j = 0; j < 3; j++) {
 
-				if (!separator.test_axis( (va-vertex[j]).normalized() ))
+				if (!separator.test_axis((va - vertex[j]).normalized()))
 					return;
-
 			}
 		}
 		//edge-vertex( hsell)
 
-		for (int i=0;i<edge_count;i++) {
-
-			Vector3 e1=p_transform_a.basis.xform( vertices[ edges[i].a] );
-			Vector3 e2=p_transform_a.basis.xform( vertices[ edges[i].b] );
-			Vector3 n = (e2-e1);
+		for (int i = 0; i < edge_count; i++) {
 
-			for(int j=0;j<3;j++) {
+			Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]);
+			Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]);
+			Vector3 n = (e2 - e1);
 
-				Vector3 e3=vertex[j];
+			for (int j = 0; j < 3; j++) {
 
+				Vector3 e3 = vertex[j];
 
-				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized()))
 					return;
 			}
 		}
 
-		for (int i=0;i<3;i++) {
+		for (int i = 0; i < 3; i++) {
 
-			Vector3 e1=vertex[i];
-			Vector3 e2=vertex[(i+1)%3];
-			Vector3 n = (e2-e1);
+			Vector3 e1 = vertex[i];
+			Vector3 e2 = vertex[(i + 1) % 3];
+			Vector3 n = (e2 - e1);
 
-			for(int j=0;j<vertex_count;j++) {
+			for (int j = 0; j < vertex_count; j++) {
 
-				Vector3 e3=p_transform_a.xform(vertices[j]);
+				Vector3 e3 = p_transform_a.xform(vertices[j]);
 
-
-				if (!separator.test_axis( (e1-e3).cross(n).cross(n).normalized() ))
+				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized()))
 					return;
 			}
 		}
 	}
 
 	separator.generate_contacts();
-
 }
 
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, float p_margin_a, float p_margin_b) {
 
-bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis,float p_margin_a,float p_margin_b) {
-
-	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
-
-	ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_PLANE,false);
-	ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_RAY,false);
-	ERR_FAIL_COND_V(p_shape_A->is_concave(),false);
+	PhysicsServer::ShapeType type_A = p_shape_A->get_type();
 
-	PhysicsServer::ShapeType type_B=p_shape_B->get_type();
+	ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_PLANE, false);
+	ERR_FAIL_COND_V(type_A == PhysicsServer::SHAPE_RAY, false);
+	ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
 
-	ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_PLANE,false);
-	ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_RAY,false);
-	ERR_FAIL_COND_V(p_shape_B->is_concave(),false);
+	PhysicsServer::ShapeType type_B = p_shape_B->get_type();
 
+	ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_PLANE, false);
+	ERR_FAIL_COND_V(type_B == PhysicsServer::SHAPE_RAY, false);
+	ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
 
-	static const CollisionFunc collision_table[5][5]={
-		{_collision_sphere_sphere<false>,
-		 _collision_sphere_box<false>,
-		 _collision_sphere_capsule<false>,
-		 _collision_sphere_convex_polygon<false>,
-		 _collision_sphere_face<false>},
-		{0,
-		 _collision_box_box<false>,
-		 _collision_box_capsule<false>,
-		 _collision_box_convex_polygon<false>,
-		 _collision_box_face<false>},
-		{0,
-		 0,
-		 _collision_capsule_capsule<false>,
-		 _collision_capsule_convex_polygon<false>,
-		 _collision_capsule_face<false>},
-		{0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<false>,
-		 _collision_convex_polygon_face<false>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 0},
+	static const CollisionFunc collision_table[5][5] = {
+		{ _collision_sphere_sphere<false>,
+				_collision_sphere_box<false>,
+				_collision_sphere_capsule<false>,
+				_collision_sphere_convex_polygon<false>,
+				_collision_sphere_face<false> },
+		{ 0,
+				_collision_box_box<false>,
+				_collision_box_capsule<false>,
+				_collision_box_convex_polygon<false>,
+				_collision_box_face<false> },
+		{ 0,
+				0,
+				_collision_capsule_capsule<false>,
+				_collision_capsule_convex_polygon<false>,
+				_collision_capsule_face<false> },
+		{ 0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<false>,
+				_collision_convex_polygon_face<false> },
+		{ 0,
+				0,
+				0,
+				0,
+				0 },
 	};
 
-	static const CollisionFunc collision_table_margin[5][5]={
-		{_collision_sphere_sphere<true>,
-		 _collision_sphere_box<true>,
-		 _collision_sphere_capsule<true>,
-		 _collision_sphere_convex_polygon<true>,
-		 _collision_sphere_face<true>},
-		{0,
-		 _collision_box_box<true>,
-		 _collision_box_capsule<true>,
-		 _collision_box_convex_polygon<true>,
-		 _collision_box_face<true>},
-		{0,
-		 0,
-		 _collision_capsule_capsule<true>,
-		 _collision_capsule_convex_polygon<true>,
-		 _collision_capsule_face<true>},
-		{0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<true>,
-		 _collision_convex_polygon_face<true>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 0},
+	static const CollisionFunc collision_table_margin[5][5] = {
+		{ _collision_sphere_sphere<true>,
+				_collision_sphere_box<true>,
+				_collision_sphere_capsule<true>,
+				_collision_sphere_convex_polygon<true>,
+				_collision_sphere_face<true> },
+		{ 0,
+				_collision_box_box<true>,
+				_collision_box_capsule<true>,
+				_collision_box_convex_polygon<true>,
+				_collision_box_face<true> },
+		{ 0,
+				0,
+				_collision_capsule_capsule<true>,
+				_collision_capsule_convex_polygon<true>,
+				_collision_capsule_face<true> },
+		{ 0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<true>,
+				_collision_convex_polygon_face<true> },
+		{ 0,
+				0,
+				0,
+				0,
+				0 },
 	};
 
 	_CollectorCallback callback;
-	callback.callback=p_result_callback;
-	callback.swap=p_swap;
-	callback.userdata=p_userdata;
-	callback.collided=false;
-	callback.prev_axis=r_prev_axis;
+	callback.callback = p_result_callback;
+	callback.swap = p_swap;
+	callback.userdata = p_userdata;
+	callback.collided = false;
+	callback.prev_axis = r_prev_axis;
 
-	const ShapeSW *A=p_shape_A;
-	const ShapeSW *B=p_shape_B;
-	const Transform *transform_A=&p_transform_A;
-	const Transform *transform_B=&p_transform_B;
-	float margin_A=p_margin_a;
-	float margin_B=p_margin_b;
+	const ShapeSW *A = p_shape_A;
+	const ShapeSW *B = p_shape_B;
+	const Transform *transform_A = &p_transform_A;
+	const Transform *transform_B = &p_transform_B;
+	float margin_A = p_margin_a;
+	float margin_B = p_margin_b;
 
 	if (type_A > type_B) {
-		SWAP(A,B);
-		SWAP(transform_A,transform_B);
-		SWAP(type_A,type_B);
-		SWAP(margin_A,margin_B);
+		SWAP(A, B);
+		SWAP(transform_A, transform_B);
+		SWAP(type_A, type_B);
+		SWAP(margin_A, margin_B);
 		callback.swap = !callback.swap;
 	}
 
-
 	CollisionFunc collision_func;
-	if (margin_A!=0.0 || margin_B!=0.0) {
-		collision_func = collision_table_margin[type_A-2][type_B-2];
+	if (margin_A != 0.0 || margin_B != 0.0) {
+		collision_func = collision_table_margin[type_A - 2][type_B - 2];
 
 	} else {
-		collision_func = collision_table[type_A-2][type_B-2];
-
+		collision_func = collision_table[type_A - 2][type_B - 2];
 	}
-	ERR_FAIL_COND_V(!collision_func,false);
+	ERR_FAIL_COND_V(!collision_func, false);
 
-
-	collision_func(A,*transform_A,B,*transform_B,&callback,margin_A,margin_B);
+	collision_func(A, *transform_A, B, *transform_B, &callback, margin_A, margin_B);
 
 	return callback.collided;
-
 }
diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h
index 60387a978..2f49e266d 100644
--- a/servers/physics/collision_solver_sat.h
+++ b/servers/physics/collision_solver_sat.h
@@ -31,7 +31,6 @@
 
 #include "collision_solver_sw.h"
 
-
-bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL,float p_margin_a=0,float p_margin_b=0);
+bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = NULL, float p_margin_a = 0, float p_margin_b = 0);
 
 #endif // COLLISION_SOLVER_SAT_H
diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp
index 886d93c4b..3ffb6f017 100644
--- a/servers/physics/collision_solver_sw.cpp
+++ b/servers/physics/collision_solver_sw.cpp
@@ -29,18 +29,16 @@
 #include "collision_solver_sw.h"
 #include "collision_solver_sat.h"
 
-#include "gjk_epa.h"
 #include "collision_solver_sat.h"
-
+#include "gjk_epa.h"
 
 #define collision_solver sat_calculate_penetration
 //#define collision_solver gjk_epa_calculate_penetration
 
+bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
 
-bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
-
-	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A);
-	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE)
+	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
 		return false;
 	Plane p = p_transform_A.xform(plane->get_plane());
 
@@ -48,57 +46,54 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transf
 	Vector3 supports[max_supports];
 	int support_count;
 
-	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count);
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
 
-	bool found=false;
+	bool found = false;
 
-	for(int i=0;i<support_count;i++) {
+	for (int i = 0; i < support_count; i++) {
 
-		supports[i] = p_transform_B.xform( supports[i] );
-		if (p.distance_to(supports[i])>=0)
+		supports[i] = p_transform_B.xform(supports[i]);
+		if (p.distance_to(supports[i]) >= 0)
 			continue;
-		found=true;
+		found = true;
 
 		Vector3 support_A = p.project(supports[i]);
 
 		if (p_result_callback) {
 			if (p_swap_result)
-				p_result_callback(supports[i],support_A,p_userdata);
+				p_result_callback(supports[i], support_A, p_userdata);
 			else
-				p_result_callback(support_A,supports[i],p_userdata);
+				p_result_callback(support_A, supports[i], p_userdata);
 		}
-
 	}
 
-
 	return found;
 }
 
-bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
+bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
 
-
-	const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A);
+	const RayShapeSW *ray = static_cast<const RayShapeSW *>(p_shape_A);
 
 	Vector3 from = p_transform_A.origin;
-	Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length();
-	Vector3 support_A=to;
+	Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length();
+	Vector3 support_A = to;
 
 	Transform ai = p_transform_B.affine_inverse();
 
 	from = ai.xform(from);
 	to = ai.xform(to);
 
-	Vector3 p,n;
-	if (!p_shape_B->intersect_segment(from,to,p,n))
+	Vector3 p, n;
+	if (!p_shape_B->intersect_segment(from, to, p, n))
 		return false;
 
-	Vector3 support_B=p_transform_B.xform(p);
+	Vector3 support_B = p_transform_B.xform(p);
 
 	if (p_result_callback) {
 		if (p_swap_result)
-			p_result_callback(support_B,support_A,p_userdata);
+			p_result_callback(support_B, support_A, p_userdata);
 		else
-			p_result_callback(support_A,support_B,p_userdata);
+			p_result_callback(support_A, support_B, p_userdata);
 	}
 	return true;
 }
@@ -117,169 +112,153 @@ struct _ConcaveCollisionInfo {
 	bool tested;
 	float margin_A;
 	float margin_B;
-	Vector3 close_A,close_B;
-
+	Vector3 close_A, close_B;
 };
 
 void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) {
 
-
-	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
+	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
 	cinfo.aabb_tests++;
 
-	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,NULL,cinfo.margin_A,cinfo.margin_B);
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, NULL, cinfo.margin_A, cinfo.margin_B);
 	if (!collided)
 		return;
 
-	cinfo.collided=true;
+	cinfo.collided = true;
 	cinfo.collisions++;
-
 }
 
-bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A,float p_margin_B) {
+bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, float p_margin_A, float p_margin_B) {
 
-
-	const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
+	const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
 
 	_ConcaveCollisionInfo cinfo;
-	cinfo.transform_A=&p_transform_A;
-	cinfo.shape_A=p_shape_A;
-	cinfo.transform_B=&p_transform_B;
-	cinfo.result_callback=p_result_callback;
-	cinfo.userdata=p_userdata;
-	cinfo.swap_result=p_swap_result;
-	cinfo.collided=false;
-	cinfo.collisions=0;
-	cinfo.margin_A=p_margin_A;
-	cinfo.margin_B=p_margin_B;
+	cinfo.transform_A = &p_transform_A;
+	cinfo.shape_A = p_shape_A;
+	cinfo.transform_B = &p_transform_B;
+	cinfo.result_callback = p_result_callback;
+	cinfo.userdata = p_userdata;
+	cinfo.swap_result = p_swap_result;
+	cinfo.collided = false;
+	cinfo.collisions = 0;
+	cinfo.margin_A = p_margin_A;
+	cinfo.margin_B = p_margin_B;
 
-	cinfo.aabb_tests=0;
+	cinfo.aabb_tests = 0;
 
 	Transform rel_transform = p_transform_A;
-	rel_transform.origin-=p_transform_B.origin;
+	rel_transform.origin -= p_transform_B.origin;
 
 	//quickly compute a local AABB
 
 	AABB local_aabb;
-	for(int i=0;i<3;i++) {
-
-	     Vector3 axis( p_transform_B.basis.get_axis(i) );
-	     float axis_scale = 1.0/axis.length();
-	     axis*=axis_scale;
+	for (int i = 0; i < 3; i++) {
 
-	     float smin,smax;
-	     p_shape_A->project_range(axis,rel_transform,smin,smax);
-	     smin-=p_margin_A;
-	     smax+=p_margin_A;
-	     smin*=axis_scale;
-	     smax*=axis_scale;
+		Vector3 axis(p_transform_B.basis.get_axis(i));
+		float axis_scale = 1.0 / axis.length();
+		axis *= axis_scale;
 
+		float smin, smax;
+		p_shape_A->project_range(axis, rel_transform, smin, smax);
+		smin -= p_margin_A;
+		smax += p_margin_A;
+		smin *= axis_scale;
+		smax *= axis_scale;
 
-	     local_aabb.pos[i]=smin;
-	     local_aabb.size[i]=smax-smin;
+		local_aabb.pos[i] = smin;
+		local_aabb.size[i] = smax - smin;
 	}
 
-	concave_B->cull(local_aabb,concave_callback,&cinfo);
+	concave_B->cull(local_aabb, concave_callback, &cinfo);
 	//print_line("COL AABB TESTS: "+itos(cinfo.aabb_tests));
 
 	return cinfo.collided;
 }
 
+bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, float p_margin_A, float p_margin_B) {
 
-bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis,float p_margin_A,float p_margin_B) {
-
-
-	PhysicsServer::ShapeType type_A=p_shape_A->get_type();
-	PhysicsServer::ShapeType type_B=p_shape_B->get_type();
-	bool concave_A=p_shape_A->is_concave();
-	bool concave_B=p_shape_B->is_concave();
+	PhysicsServer::ShapeType type_A = p_shape_A->get_type();
+	PhysicsServer::ShapeType type_B = p_shape_B->get_type();
+	bool concave_A = p_shape_A->is_concave();
+	bool concave_B = p_shape_B->is_concave();
 
 	bool swap = false;
 
-	if (type_A>type_B) {
-		SWAP(type_A,type_B);
-		SWAP(concave_A,concave_B);
-		swap=true;
+	if (type_A > type_B) {
+		SWAP(type_A, type_B);
+		SWAP(concave_A, concave_B);
+		swap = true;
 	}
 
-	if (type_A==PhysicsServer::SHAPE_PLANE) {
+	if (type_A == PhysicsServer::SHAPE_PLANE) {
 
-		if (type_B==PhysicsServer::SHAPE_PLANE)
+		if (type_B == PhysicsServer::SHAPE_PLANE)
 			return false;
-		if (type_B==PhysicsServer::SHAPE_RAY) {
+		if (type_B == PhysicsServer::SHAPE_RAY) {
 			return false;
 		}
 
 		if (swap) {
-			return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+			return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
 		} else {
-			return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+			return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}
 
-	} else if (type_A==PhysicsServer::SHAPE_RAY) {
+	} else if (type_A == PhysicsServer::SHAPE_RAY) {
 
-		if (type_B==PhysicsServer::SHAPE_RAY)
+		if (type_B == PhysicsServer::SHAPE_RAY)
 			return false;
 
 		if (swap) {
-			return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+			return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
 		} else {
-			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+			return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}
 
 	} else if (concave_B) {
 
-
 		if (concave_A)
 			return false;
 
 		if (!swap)
-			return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,p_margin_A,p_margin_B);
+			return solve_concave(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A, p_margin_B);
 		else
-			return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,p_margin_A,p_margin_B);
-
-
+			return solve_concave(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_A, p_margin_B);
 
 	} else {
 
-		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis,p_margin_A,p_margin_B);
+		return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, r_sep_axis, p_margin_A, p_margin_B);
 	}
 
-
 	return false;
 }
 
-
 void CollisionSolverSW::concave_distance_callback(void *p_userdata, ShapeSW *p_convex) {
 
-
-	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata);
+	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
 	cinfo.aabb_tests++;
 	if (cinfo.collided)
 		return;
 
-	Vector3 close_A,close_B;
-	cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A,*cinfo.transform_A,p_convex,*cinfo.transform_B,close_A,close_B);
+	Vector3 close_A, close_B;
+	cinfo.collided = !gjk_epa_calculate_distance(cinfo.shape_A, *cinfo.transform_A, p_convex, *cinfo.transform_B, close_A, close_B);
 
 	if (cinfo.collided)
 		return;
 	if (!cinfo.tested || close_A.distance_squared_to(close_B) < cinfo.close_A.distance_squared_to(cinfo.close_B)) {
 
-		cinfo.close_A=close_A;
-		cinfo.close_B=close_B;
-		cinfo.tested=true;
+		cinfo.close_A = close_A;
+		cinfo.close_B = close_B;
+		cinfo.tested = true;
 	}
 
 	cinfo.collisions++;
-
 }
 
+bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
 
-
-bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B) {
-
-	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A);
-	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE)
+	const PlaneShapeSW *plane = static_cast<const PlaneShapeSW *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE)
 		return false;
 	Plane p = p_transform_A.xform(plane->get_plane());
 
@@ -287,43 +266,41 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A,const Tran
 	Vector3 supports[max_supports];
 	int support_count;
 
-	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count);
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
 
-	bool collided=false;
+	bool collided = false;
 	Vector3 closest;
 	float closest_d;
 
+	for (int i = 0; i < support_count; i++) {
 
-	for(int i=0;i<support_count;i++) {
-
-		supports[i] = p_transform_B.xform( supports[i] );
+		supports[i] = p_transform_B.xform(supports[i]);
 		real_t d = p.distance_to(supports[i]);
-		if (i==0 || d<closest_d) {
-			closest=supports[i];
-			closest_d=d;
-			if (d<=0)
-				collided=true;
+		if (i == 0 || d < closest_d) {
+			closest = supports[i];
+			closest_d = d;
+			if (d <= 0)
+				collided = true;
 		}
-
 	}
 
-	r_point_A=p.project(closest);
-	r_point_B=closest;
+	r_point_A = p.project(closest);
+	r_point_B = closest;
 
 	return collided;
 }
 
-bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis) {
+bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
 
 	if (p_shape_A->is_concave())
 		return false;
 
-	if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) {
+	if (p_shape_B->get_type() == PhysicsServer::SHAPE_PLANE) {
 
-		Vector3 a,b;
-		bool col = solve_distance_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,a,b);
-		r_point_A=b;
-		r_point_B=a;
+		Vector3 a, b;
+		bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
+		r_point_A = b;
+		r_point_B = a;
 		return !col;
 
 	} else if (p_shape_B->is_concave()) {
@@ -331,62 +308,59 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform&
 		if (p_shape_A->is_concave())
 			return false;
 
-
-		const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B);
+		const ConcaveShapeSW *concave_B = static_cast<const ConcaveShapeSW *>(p_shape_B);
 
 		_ConcaveCollisionInfo cinfo;
-		cinfo.transform_A=&p_transform_A;
-		cinfo.shape_A=p_shape_A;
-		cinfo.transform_B=&p_transform_B;
-		cinfo.result_callback=NULL;
-		cinfo.userdata=NULL;
-		cinfo.swap_result=false;
-		cinfo.collided=false;
-		cinfo.collisions=0;
-		cinfo.aabb_tests=0;
-		cinfo.tested=false;
+		cinfo.transform_A = &p_transform_A;
+		cinfo.shape_A = p_shape_A;
+		cinfo.transform_B = &p_transform_B;
+		cinfo.result_callback = NULL;
+		cinfo.userdata = NULL;
+		cinfo.swap_result = false;
+		cinfo.collided = false;
+		cinfo.collisions = 0;
+		cinfo.aabb_tests = 0;
+		cinfo.tested = false;
 
 		Transform rel_transform = p_transform_A;
-		rel_transform.origin-=p_transform_B.origin;
+		rel_transform.origin -= p_transform_B.origin;
 
 		//quickly compute a local AABB
 
-		bool use_cc_hint=p_concave_hint!=AABB();
+		bool use_cc_hint = p_concave_hint != AABB();
 		AABB cc_hint_aabb;
 		if (use_cc_hint) {
-			cc_hint_aabb=p_concave_hint;
-			cc_hint_aabb.pos-=p_transform_B.origin;
+			cc_hint_aabb = p_concave_hint;
+			cc_hint_aabb.pos -= p_transform_B.origin;
 		}
 
 		AABB local_aabb;
-		for(int i=0;i<3;i++) {
+		for (int i = 0; i < 3; i++) {
 
-		     Vector3 axis( p_transform_B.basis.get_axis(i) );
-		     float axis_scale = 1.0/axis.length();
-		     axis*=axis_scale;
+			Vector3 axis(p_transform_B.basis.get_axis(i));
+			float axis_scale = 1.0 / axis.length();
+			axis *= axis_scale;
 
-		     float smin,smax;
+			float smin, smax;
 
-		     if (use_cc_hint) {
-			     cc_hint_aabb.project_range_in_plane(Plane(axis,0),smin,smax);
-		     } else {
-			     p_shape_A->project_range(axis,rel_transform,smin,smax);
-		      }
+			if (use_cc_hint) {
+				cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax);
+			} else {
+				p_shape_A->project_range(axis, rel_transform, smin, smax);
+			}
 
-		     smin*=axis_scale;
-		     smax*=axis_scale;
+			smin *= axis_scale;
+			smax *= axis_scale;
 
-		     local_aabb.pos[i]=smin;
-		     local_aabb.size[i]=smax-smin;
+			local_aabb.pos[i] = smin;
+			local_aabb.size[i] = smax - smin;
 		}
 
-
-		concave_B->cull(local_aabb,concave_distance_callback,&cinfo);
+		concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
 		if (!cinfo.collided) {
-//			print_line(itos(cinfo.tested));
-			r_point_A=cinfo.close_A;
-			r_point_B=cinfo.close_B;
-
+			//			print_line(itos(cinfo.tested));
+			r_point_A = cinfo.close_A;
+			r_point_B = cinfo.close_B;
 		}
 
 		//print_line("DIST AABB TESTS: "+itos(cinfo.aabb_tests));
@@ -394,10 +368,8 @@ bool CollisionSolverSW::solve_distance(const ShapeSW *p_shape_A,const Transform&
 		return !cinfo.collided;
 	} else {
 
-		return gjk_epa_calculate_distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,r_point_A,r_point_B); //should pass sepaxis..
+		return gjk_epa_calculate_distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, r_point_A, r_point_B); //should pass sepaxis..
 	}
 
-
 	return false;
 }
-
diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h
index 82ac77f73..8ff3b69ff 100644
--- a/servers/physics/collision_solver_sw.h
+++ b/servers/physics/collision_solver_sw.h
@@ -31,25 +31,21 @@
 
 #include "shape_sw.h"
 
-class CollisionSolverSW
-{
+class CollisionSolverSW {
 public:
-	typedef void (*CallbackResult)(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
-private:
+	typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
 
+private:
 	static void concave_callback(void *p_userdata, ShapeSW *p_convex);
-	static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
-	static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
-	static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,float p_margin_A=0,float p_margin_B=0);
+	static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, float p_margin_A = 0, float p_margin_B = 0);
 	static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex);
-	static bool solve_distance_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B);
+	static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
 
 public:
-
-
-	static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
-	static bool solve_distance(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,Vector3& r_point_A,Vector3& r_point_B,const AABB& p_concave_hint,Vector3 *r_sep_axis=NULL);
-
+	static bool solve_static(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = NULL, float p_margin_A = 0, float p_margin_B = 0);
+	static bool solve_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = NULL);
 };
 
 #endif // COLLISION_SOLVER__SW_H
diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h
index e178de441..56a738440 100644
--- a/servers/physics/constraint_sw.h
+++ b/servers/physics/constraint_sw.h
@@ -40,35 +40,37 @@ class ConstraintSW {
 	ConstraintSW *island_list_next;
 	int priority;
 
-
 	RID self;
 
 protected:
-	ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; priority=1; }
-public:
+	ConstraintSW(BodySW **p_body_ptr = NULL, int p_body_count = 0) {
+		_body_ptr = p_body_ptr;
+		_body_count = p_body_count;
+		island_step = 0;
+		priority = 1;
+	}
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
-	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
+	_FORCE_INLINE_ ConstraintSW *get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(ConstraintSW *p_next) { island_next = p_next; }
 
-	_FORCE_INLINE_ ConstraintSW* get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(ConstraintSW* p_next) { island_next=p_next; }
-
-	_FORCE_INLINE_ ConstraintSW* get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(ConstraintSW* p_next) { island_list_next=p_next; }
+	_FORCE_INLINE_ ConstraintSW *get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(ConstraintSW *p_next) { island_list_next = p_next; }
 
 	_FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
-	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
-
-	virtual bool setup(float p_step)=0;
-	virtual void solve(float p_step)=0;
+	virtual bool setup(float p_step) = 0;
+	virtual void solve(float p_step) = 0;
 
 	virtual ~ConstraintSW() {}
 };
diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp
index 92ca5ea8d..980ef1763 100644
--- a/servers/physics/gjk_epa.cpp
+++ b/servers/physics/gjk_epa.cpp
@@ -30,471 +30,434 @@
 
 /*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
 
-	// Config
+// Config
 
 /* GJK	*/
-#define GJK_MAX_ITERATIONS	128
-#define GJK_ACCURARY		((real_t)0.0001)
-#define GJK_MIN_DISTANCE	((real_t)0.0001)
-#define GJK_DUPLICATED_EPS	((real_t)0.0001)
-#define GJK_SIMPLEX2_EPS	((real_t)0.0)
-#define GJK_SIMPLEX3_EPS	((real_t)0.0)
-#define GJK_SIMPLEX4_EPS	((real_t)0.0)
+#define GJK_MAX_ITERATIONS 128
+#define GJK_ACCURARY ((real_t)0.0001)
+#define GJK_MIN_DISTANCE ((real_t)0.0001)
+#define GJK_DUPLICATED_EPS ((real_t)0.0001)
+#define GJK_SIMPLEX2_EPS ((real_t)0.0)
+#define GJK_SIMPLEX3_EPS ((real_t)0.0)
+#define GJK_SIMPLEX4_EPS ((real_t)0.0)
 
 /* EPA	*/
-#define EPA_MAX_VERTICES	64
-#define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
-#define EPA_MAX_ITERATIONS	255
-#define EPA_ACCURACY		((real_t)0.0001)
-#define EPA_FALLBACK		(10*EPA_ACCURACY)
-#define EPA_PLANE_EPS		((real_t)0.00001)
-#define EPA_INSIDE_EPS		((real_t)0.01)
+#define EPA_MAX_VERTICES 64
+#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2)
+#define EPA_MAX_ITERATIONS 255
+#define EPA_ACCURACY ((real_t)0.0001)
+#define EPA_FALLBACK (10 * EPA_ACCURACY)
+#define EPA_PLANE_EPS ((real_t)0.00001)
+#define EPA_INSIDE_EPS ((real_t)0.01)
 
 namespace GjkEpa2 {
 
-
-struct sResults	{
+struct sResults {
 	enum eStatus {
-		Separated,		/* Shapes doesnt penetrate */
-		Penetrating,	/* Shapes are penetrating */
-		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/
+		Separated, /* Shapes doesnt penetrate */
+		Penetrating, /* Shapes are penetrating */
+		GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching'	*/
 		EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug	*/
 	} status;
 
-	Vector3	witnesses[2];
-	Vector3	normal;
-	real_t	distance;
+	Vector3 witnesses[2];
+	Vector3 normal;
+	real_t distance;
 };
 
 // Shorthands
-typedef unsigned int	U;
-typedef unsigned char	U1;
+typedef unsigned int U;
+typedef unsigned char U1;
 
 // MinkowskiDiff
-struct	MinkowskiDiff {
+struct MinkowskiDiff {
 
-	const ShapeSW* m_shapes[2];
+	const ShapeSW *m_shapes[2];
 
 	Transform transform_A;
 	Transform transform_B;
 
 	// i wonder how this could be sped up... if it can
-	_FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const {
-		return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) );
+	_FORCE_INLINE_ Vector3 Support0(const Vector3 &d) const {
+		return transform_A.xform(m_shapes[0]->get_support(transform_A.basis.xform_inv(d).normalized()));
 	}
 
-	_FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const {
-		return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) );
+	_FORCE_INLINE_ Vector3 Support1(const Vector3 &d) const {
+		return transform_B.xform(m_shapes[1]->get_support(transform_B.basis.xform_inv(d).normalized()));
 	}
 
-	_FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const {
-		return ( Support0 ( d )-Support1 ( -d ) );
+	_FORCE_INLINE_ Vector3 Support(const Vector3 &d) const {
+		return (Support0(d) - Support1(-d));
 	}
 
-	_FORCE_INLINE_ Vector3	Support ( const Vector3& d,U index ) const
-	{
-		if ( index )
-			return ( Support1 ( d ) );
+	_FORCE_INLINE_ Vector3 Support(const Vector3 &d, U index) const {
+		if (index)
+			return (Support1(d));
 		else
-			return ( Support0 ( d ) );
+			return (Support0(d));
 	}
 };
 
-typedef	MinkowskiDiff tShape;
-
+typedef MinkowskiDiff tShape;
 
 // GJK
-struct	GJK
-{
+struct GJK {
 	/* Types		*/
-	struct	sSV
-	{
-		Vector3	d,w;
+	struct sSV {
+		Vector3 d, w;
 	};
-	struct	sSimplex
-	{
-		sSV*		c[4];
-		real_t	p[4];
-		U			rank;
+	struct sSimplex {
+		sSV *c[4];
+		real_t p[4];
+		U rank;
 	};
-	struct	eStatus	{ enum _ {
-		Valid,
-		Inside,
-		Failed		};};
-		/* Fields		*/
-		tShape			m_shape;
-		Vector3		m_ray;
-		real_t		m_distance;
-		sSimplex		m_simplices[2];
-		sSV				m_store[4];
-		sSV*			m_free[4];
-		U				m_nfree;
-		U				m_current;
-		sSimplex*		m_simplex;
-		eStatus::_		m_status;
-		/* Methods		*/
-		GJK()
-		{
-			Initialize();
-		}
-		void				Initialize()
-		{
-			m_ray		=	Vector3(0,0,0);
-			m_nfree		=	0;
-			m_status	=	eStatus::Failed;
-			m_current	=	0;
-			m_distance	=	0;
-		}
-		eStatus::_			Evaluate(const tShape& shapearg,const Vector3& guess)
-		{
-			U			iterations=0;
-			real_t	sqdist=0;
-			real_t	alpha=0;
-			Vector3	lastw[4];
-			U			clastw=0;
-			/* Initialize solver		*/
-			m_free[0]			=	&m_store[0];
-			m_free[1]			=	&m_store[1];
-			m_free[2]			=	&m_store[2];
-			m_free[3]			=	&m_store[3];
-			m_nfree				=	4;
-			m_current			=	0;
-			m_status			=	eStatus::Valid;
-			m_shape				=	shapearg;
-			m_distance			=	0;
-			/* Initialize simplex		*/
-			m_simplices[0].rank	=	0;
-			m_ray				=	guess;
-			const real_t	sqrl=	m_ray.length_squared();
-			appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0));
-			m_simplices[0].p[0]	=	1;
-			m_ray				=	m_simplices[0].c[0]->w;
-			sqdist				=	sqrl;
-			lastw[0]			=
-				lastw[1]			=
-				lastw[2]			=
-				lastw[3]			=	m_ray;
-			/* Loop						*/
-			do	{
-				const U		next=1-m_current;
-				sSimplex&	cs=m_simplices[m_current];
-				sSimplex&	ns=m_simplices[next];
-				/* Check zero							*/
-				const real_t	rl=m_ray.length();
-				if(rl<GJK_MIN_DISTANCE)
-				{/* Touching or inside				*/
-					m_status=eStatus::Inside;
-					break;
-				}
-				/* Append new vertice in -'v' direction	*/
-				appendvertice(cs,-m_ray);
-				const Vector3&	w=cs.c[cs.rank-1]->w;
-				bool				found=false;
-				for(U i=0;i<4;++i)
-				{
-					if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS)
-					{ found=true;break; }
-				}
-				if(found)
-				{/* Return old simplex				*/
-					removevertice(m_simplices[m_current]);
-					break;
-				}
-				else
-				{/* Update lastw					*/
-					lastw[clastw=(clastw+1)&3]=w;
-				}
-				/* Check for termination				*/
-				const real_t	omega=vec3_dot(m_ray,w)/rl;
-				alpha=MAX(omega,alpha);
-				if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
-				{/* Return old simplex				*/
-					removevertice(m_simplices[m_current]);
+	struct eStatus {
+		enum _ {
+			Valid,
+			Inside,
+			Failed
+		};
+	};
+	/* Fields		*/
+	tShape m_shape;
+	Vector3 m_ray;
+	real_t m_distance;
+	sSimplex m_simplices[2];
+	sSV m_store[4];
+	sSV *m_free[4];
+	U m_nfree;
+	U m_current;
+	sSimplex *m_simplex;
+	eStatus::_ m_status;
+	/* Methods		*/
+	GJK() {
+		Initialize();
+	}
+	void Initialize() {
+		m_ray = Vector3(0, 0, 0);
+		m_nfree = 0;
+		m_status = eStatus::Failed;
+		m_current = 0;
+		m_distance = 0;
+	}
+	eStatus::_ Evaluate(const tShape &shapearg, const Vector3 &guess) {
+		U iterations = 0;
+		real_t sqdist = 0;
+		real_t alpha = 0;
+		Vector3 lastw[4];
+		U clastw = 0;
+		/* Initialize solver		*/
+		m_free[0] = &m_store[0];
+		m_free[1] = &m_store[1];
+		m_free[2] = &m_store[2];
+		m_free[3] = &m_store[3];
+		m_nfree = 4;
+		m_current = 0;
+		m_status = eStatus::Valid;
+		m_shape = shapearg;
+		m_distance = 0;
+		/* Initialize simplex		*/
+		m_simplices[0].rank = 0;
+		m_ray = guess;
+		const real_t sqrl = m_ray.length_squared();
+		appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : Vector3(1, 0, 0));
+		m_simplices[0].p[0] = 1;
+		m_ray = m_simplices[0].c[0]->w;
+		sqdist = sqrl;
+		lastw[0] =
+				lastw[1] =
+						lastw[2] =
+								lastw[3] = m_ray;
+		/* Loop						*/
+		do {
+			const U next = 1 - m_current;
+			sSimplex &cs = m_simplices[m_current];
+			sSimplex &ns = m_simplices[next];
+			/* Check zero							*/
+			const real_t rl = m_ray.length();
+			if (rl < GJK_MIN_DISTANCE) { /* Touching or inside				*/
+				m_status = eStatus::Inside;
+				break;
+			}
+			/* Append new vertice in -'v' direction	*/
+			appendvertice(cs, -m_ray);
+			const Vector3 &w = cs.c[cs.rank - 1]->w;
+			bool found = false;
+			for (U i = 0; i < 4; ++i) {
+				if ((w - lastw[i]).length_squared() < GJK_DUPLICATED_EPS) {
+					found = true;
 					break;
 				}
-				/* Reduce simplex						*/
-				real_t	weights[4];
-				U			mask=0;
-				switch(cs.rank)
-				{
-				case	2:	sqdist=projectorigin(	cs.c[0]->w,
+			}
+			if (found) { /* Return old simplex				*/
+				removevertice(m_simplices[m_current]);
+				break;
+			} else { /* Update lastw					*/
+				lastw[clastw = (clastw + 1) & 3] = w;
+			}
+			/* Check for termination				*/
+			const real_t omega = vec3_dot(m_ray, w) / rl;
+			alpha = MAX(omega, alpha);
+			if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) { /* Return old simplex				*/
+				removevertice(m_simplices[m_current]);
+				break;
+			}
+			/* Reduce simplex						*/
+			real_t weights[4];
+			U mask = 0;
+			switch (cs.rank) {
+				case 2: sqdist = projectorigin(cs.c[0]->w,
 								cs.c[1]->w,
-								weights,mask);break;
-				case	3:	sqdist=projectorigin(	cs.c[0]->w,
+								weights, mask);
+						break;
+				case 3: sqdist = projectorigin(cs.c[0]->w,
 								cs.c[1]->w,
 								cs.c[2]->w,
-								weights,mask);break;
-				case	4:	sqdist=projectorigin(	cs.c[0]->w,
+								weights, mask);
+						break;
+				case 4: sqdist = projectorigin(cs.c[0]->w,
 								cs.c[1]->w,
 								cs.c[2]->w,
 								cs.c[3]->w,
-								weights,mask);break;
-				}
-				if(sqdist>=0)
-				{/* Valid	*/
-					ns.rank		=	0;
-					m_ray		=	Vector3(0,0,0);
-					m_current	=	next;
-					for(U i=0,ni=cs.rank;i<ni;++i)
-					{
-						if(mask&(1<<i))
-						{
-							ns.c[ns.rank]		=	cs.c[i];
-							ns.p[ns.rank++]		=	weights[i];
-							m_ray				+=	cs.c[i]->w*weights[i];
-						}
-						else
-						{
-							m_free[m_nfree++]	=	cs.c[i];
-						}
-					}
-					if(mask==15) m_status=eStatus::Inside;
-				}
-				else
-				{/* Return old simplex				*/
-					removevertice(m_simplices[m_current]);
-					break;
-				}
-				m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
-			} while(m_status==eStatus::Valid);
-			m_simplex=&m_simplices[m_current];
-			switch(m_status)
-			{
-			case	eStatus::Valid:		m_distance=m_ray.length();break;
-			case	eStatus::Inside:	m_distance=0;break;
-			default: {}
+								weights, mask);
+						break;
 			}
-			return(m_status);
-		}
-		bool					EncloseOrigin()
-		{
-			switch(m_simplex->rank)
-			{
-			case	1:
-				{
-					for(U i=0;i<3;++i)
-					{
-						Vector3		axis=Vector3(0,0,0);
-						axis[i]=1;
-						appendvertice(*m_simplex, axis);
-						if(EncloseOrigin())	return(true);
-						removevertice(*m_simplex);
-						appendvertice(*m_simplex,-axis);
-						if(EncloseOrigin())	return(true);
-						removevertice(*m_simplex);
+			if (sqdist >= 0) { /* Valid	*/
+				ns.rank = 0;
+				m_ray = Vector3(0, 0, 0);
+				m_current = next;
+				for (U i = 0, ni = cs.rank; i < ni; ++i) {
+					if (mask & (1 << i)) {
+						ns.c[ns.rank] = cs.c[i];
+						ns.p[ns.rank++] = weights[i];
+						m_ray += cs.c[i]->w * weights[i];
+					} else {
+						m_free[m_nfree++] = cs.c[i];
 					}
 				}
+				if (mask == 15) m_status = eStatus::Inside;
+			} else { /* Return old simplex				*/
+				removevertice(m_simplices[m_current]);
 				break;
-			case	2:
-				{
-					const Vector3	d=m_simplex->c[1]->w-m_simplex->c[0]->w;
-					for(U i=0;i<3;++i)
-					{
-						Vector3		axis=Vector3(0,0,0);
-						axis[i]=1;
-						const Vector3	p=vec3_cross(d,axis);
-						if(p.length_squared()>0)
-						{
-							appendvertice(*m_simplex, p);
-							if(EncloseOrigin())	return(true);
-							removevertice(*m_simplex);
-							appendvertice(*m_simplex,-p);
-							if(EncloseOrigin())	return(true);
-							removevertice(*m_simplex);
-						}
-					}
+			}
+			m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed;
+		} while (m_status == eStatus::Valid);
+		m_simplex = &m_simplices[m_current];
+		switch (m_status) {
+			case eStatus::Valid: m_distance = m_ray.length(); break;
+			case eStatus::Inside: m_distance = 0; break;
+			default: {}
+		}
+		return (m_status);
+	}
+	bool EncloseOrigin() {
+		switch (m_simplex->rank) {
+			case 1: {
+				for (U i = 0; i < 3; ++i) {
+					Vector3 axis = Vector3(0, 0, 0);
+					axis[i] = 1;
+					appendvertice(*m_simplex, axis);
+					if (EncloseOrigin()) return (true);
+					removevertice(*m_simplex);
+					appendvertice(*m_simplex, -axis);
+					if (EncloseOrigin()) return (true);
+					removevertice(*m_simplex);
 				}
-				break;
-			case	3:
-				{
-					const Vector3	n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
-						m_simplex->c[2]->w-m_simplex->c[0]->w);
-					if(n.length_squared()>0)
-					{
-						appendvertice(*m_simplex,n);
-						if(EncloseOrigin())	return(true);
+			} break;
+			case 2: {
+				const Vector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w;
+				for (U i = 0; i < 3; ++i) {
+					Vector3 axis = Vector3(0, 0, 0);
+					axis[i] = 1;
+					const Vector3 p = vec3_cross(d, axis);
+					if (p.length_squared() > 0) {
+						appendvertice(*m_simplex, p);
+						if (EncloseOrigin()) return (true);
 						removevertice(*m_simplex);
-						appendvertice(*m_simplex,-n);
-						if(EncloseOrigin())	return(true);
+						appendvertice(*m_simplex, -p);
+						if (EncloseOrigin()) return (true);
 						removevertice(*m_simplex);
 					}
 				}
-				break;
-			case	4:
-				{
-					if(Math::abs(det(	m_simplex->c[0]->w-m_simplex->c[3]->w,
-						m_simplex->c[1]->w-m_simplex->c[3]->w,
-						m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
-						return(true);
+			} break;
+			case 3: {
+				const Vector3 n = vec3_cross(m_simplex->c[1]->w - m_simplex->c[0]->w,
+						m_simplex->c[2]->w - m_simplex->c[0]->w);
+				if (n.length_squared() > 0) {
+					appendvertice(*m_simplex, n);
+					if (EncloseOrigin()) return (true);
+					removevertice(*m_simplex);
+					appendvertice(*m_simplex, -n);
+					if (EncloseOrigin()) return (true);
+					removevertice(*m_simplex);
 				}
-				break;
-			}
-			return(false);
-		}
-		/* Internals	*/
-		void				getsupport(const Vector3& d,sSV& sv) const
-		{
-			sv.d	=	d/d.length();
-			sv.w	=	m_shape.Support(sv.d);
-		}
-		void				removevertice(sSimplex& simplex)
-		{
-			m_free[m_nfree++]=simplex.c[--simplex.rank];
-		}
-		void				appendvertice(sSimplex& simplex,const Vector3& v)
-		{
-			simplex.p[simplex.rank]=0;
-			simplex.c[simplex.rank]=m_free[--m_nfree];
-			getsupport(v,*simplex.c[simplex.rank++]);
+			} break;
+			case 4: {
+				if (Math::abs(det(m_simplex->c[0]->w - m_simplex->c[3]->w,
+							m_simplex->c[1]->w - m_simplex->c[3]->w,
+							m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0)
+					return (true);
+			} break;
 		}
-		static real_t		det(const Vector3& a,const Vector3& b,const Vector3& c)
-		{
-			return(	a.y*b.z*c.x+a.z*b.x*c.y-
-				a.x*b.z*c.y-a.y*b.x*c.z+
-				a.x*b.y*c.z-a.z*b.y*c.x);
-		}
-		static real_t		projectorigin(	const Vector3& a,
-			const Vector3& b,
-			real_t* w,U& m)
-		{
-			const Vector3	d=b-a;
-			const real_t	l=d.length_squared();
-			if(l>GJK_SIMPLEX2_EPS)
-			{
-				const real_t	t(l>0?-vec3_dot(a,d)/l:0);
-				if(t>=1)		{ w[0]=0;w[1]=1;m=2;return(b.length_squared()); }
-				else if(t<=0)	{ w[0]=1;w[1]=0;m=1;return(a.length_squared()); }
-				else			{ w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); }
+		return (false);
+	}
+	/* Internals	*/
+	void getsupport(const Vector3 &d, sSV &sv) const {
+		sv.d = d / d.length();
+		sv.w = m_shape.Support(sv.d);
+	}
+	void removevertice(sSimplex &simplex) {
+		m_free[m_nfree++] = simplex.c[--simplex.rank];
+	}
+	void appendvertice(sSimplex &simplex, const Vector3 &v) {
+		simplex.p[simplex.rank] = 0;
+		simplex.c[simplex.rank] = m_free[--m_nfree];
+		getsupport(v, *simplex.c[simplex.rank++]);
+	}
+	static real_t det(const Vector3 &a, const Vector3 &b, const Vector3 &c) {
+		return (a.y * b.z * c.x + a.z * b.x * c.y -
+				a.x * b.z * c.y - a.y * b.x * c.z +
+				a.x * b.y * c.z - a.z * b.y * c.x);
+	}
+	static real_t projectorigin(const Vector3 &a,
+			const Vector3 &b,
+			real_t *w, U &m) {
+		const Vector3 d = b - a;
+		const real_t l = d.length_squared();
+		if (l > GJK_SIMPLEX2_EPS) {
+			const real_t t(l > 0 ? -vec3_dot(a, d) / l : 0);
+			if (t >= 1) {
+				w[0] = 0;
+				w[1] = 1;
+				m = 2;
+				return (b.length_squared());
+			} else if (t <= 0) {
+				w[0] = 1;
+				w[1] = 0;
+				m = 1;
+				return (a.length_squared());
+			} else {
+				w[0] = 1 - (w[1] = t);
+				m = 3;
+				return ((a + d * t).length_squared());
 			}
-			return(-1);
 		}
-		static real_t		projectorigin(	const Vector3& a,
-			const Vector3& b,
-			const Vector3& c,
-			real_t* w,U& m)
-		{
-			static const U		imd3[]={1,2,0};
-			const Vector3*	vt[]={&a,&b,&c};
-			const Vector3		dl[]={a-b,b-c,c-a};
-			const Vector3		n=vec3_cross(dl[0],dl[1]);
-			const real_t		l=n.length_squared();
-			if(l>GJK_SIMPLEX3_EPS)
-			{
-				real_t	mindist=-1;
-				real_t	subw[2];
-				U			subm;
-				for(U i=0;i<3;++i)
-				{
-					if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0)
-					{
-						const U			j=imd3[i];
-						const real_t	subd(projectorigin(*vt[i],*vt[j],subw,subm));
-						if((mindist<0)||(subd<mindist))
-						{
-							mindist		=	subd;
-							m			=	static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
-							w[i]		=	subw[0];
-							w[j]		=	subw[1];
-							w[imd3[j]]	=	0;
-						}
+		return (-1);
+	}
+	static real_t projectorigin(const Vector3 &a,
+			const Vector3 &b,
+			const Vector3 &c,
+			real_t *w, U &m) {
+		static const U imd3[] = { 1, 2, 0 };
+		const Vector3 *vt[] = { &a, &b, &c };
+		const Vector3 dl[] = { a - b, b - c, c - a };
+		const Vector3 n = vec3_cross(dl[0], dl[1]);
+		const real_t l = n.length_squared();
+		if (l > GJK_SIMPLEX3_EPS) {
+			real_t mindist = -1;
+			real_t subw[2];
+			U subm;
+			for (U i = 0; i < 3; ++i) {
+				if (vec3_dot(*vt[i], vec3_cross(dl[i], n)) > 0) {
+					const U j = imd3[i];
+					const real_t subd(projectorigin(*vt[i], *vt[j], subw, subm));
+					if ((mindist < 0) || (subd < mindist)) {
+						mindist = subd;
+						m = static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0));
+						w[i] = subw[0];
+						w[j] = subw[1];
+						w[imd3[j]] = 0;
 					}
 				}
-				if(mindist<0)
-				{
-					const real_t	d=vec3_dot(a,n);
-					const real_t	s=Math::sqrt(l);
-					const Vector3	p=n*(d/l);
-					mindist	=	p.length_squared();
-					m		=	7;
-					w[0]	=	(vec3_cross(dl[1],b-p)).length()/s;
-					w[1]	=	(vec3_cross(dl[2],c-p)).length()/s;
-					w[2]	=	1-(w[0]+w[1]);
-				}
-				return(mindist);
 			}
-			return(-1);
+			if (mindist < 0) {
+				const real_t d = vec3_dot(a, n);
+				const real_t s = Math::sqrt(l);
+				const Vector3 p = n * (d / l);
+				mindist = p.length_squared();
+				m = 7;
+				w[0] = (vec3_cross(dl[1], b - p)).length() / s;
+				w[1] = (vec3_cross(dl[2], c - p)).length() / s;
+				w[2] = 1 - (w[0] + w[1]);
+			}
+			return (mindist);
 		}
-		static real_t		projectorigin(	const Vector3& a,
-			const Vector3& b,
-			const Vector3& c,
-			const Vector3& d,
-			real_t* w,U& m)
-		{
-			static const U		imd3[]={1,2,0};
-			const Vector3*	vt[]={&a,&b,&c,&d};
-			const Vector3		dl[]={a-d,b-d,c-d};
-			const real_t		vl=det(dl[0],dl[1],dl[2]);
-			const bool			ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0;
-			if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
-			{
-				real_t	mindist=-1;
-				real_t	subw[3];
-				U			subm;
-				for(U i=0;i<3;++i)
-				{
-					const U			j=imd3[i];
-					const real_t	s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j]));
-					if(s>0)
-					{
-						const real_t	subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
-						if((mindist<0)||(subd<mindist))
-						{
-							mindist		=	subd;
-							m			=	static_cast<U>((subm&1?1<<i:0)+
-								(subm&2?1<<j:0)+
-								(subm&4?8:0));
-							w[i]		=	subw[0];
-							w[j]		=	subw[1];
-							w[imd3[j]]	=	0;
-							w[3]		=	subw[2];
-						}
+		return (-1);
+	}
+	static real_t projectorigin(const Vector3 &a,
+			const Vector3 &b,
+			const Vector3 &c,
+			const Vector3 &d,
+			real_t *w, U &m) {
+		static const U imd3[] = { 1, 2, 0 };
+		const Vector3 *vt[] = { &a, &b, &c, &d };
+		const Vector3 dl[] = { a - d, b - d, c - d };
+		const real_t vl = det(dl[0], dl[1], dl[2]);
+		const bool ng = (vl * vec3_dot(a, vec3_cross(b - c, a - b))) <= 0;
+		if (ng && (Math::abs(vl) > GJK_SIMPLEX4_EPS)) {
+			real_t mindist = -1;
+			real_t subw[3];
+			U subm;
+			for (U i = 0; i < 3; ++i) {
+				const U j = imd3[i];
+				const real_t s = vl * vec3_dot(d, vec3_cross(dl[i], dl[j]));
+				if (s > 0) {
+					const real_t subd = projectorigin(*vt[i], *vt[j], d, subw, subm);
+					if ((mindist < 0) || (subd < mindist)) {
+						mindist = subd;
+						m = static_cast<U>((subm & 1 ? 1 << i : 0) +
+										   (subm & 2 ? 1 << j : 0) +
+										   (subm & 4 ? 8 : 0));
+						w[i] = subw[0];
+						w[j] = subw[1];
+						w[imd3[j]] = 0;
+						w[3] = subw[2];
 					}
 				}
-				if(mindist<0)
-				{
-					mindist	=	0;
-					m		=	15;
-					w[0]	=	det(c,b,d)/vl;
-					w[1]	=	det(a,c,d)/vl;
-					w[2]	=	det(b,a,d)/vl;
-					w[3]	=	1-(w[0]+w[1]+w[2]);
-				}
-				return(mindist);
 			}
-			return(-1);
+			if (mindist < 0) {
+				mindist = 0;
+				m = 15;
+				w[0] = det(c, b, d) / vl;
+				w[1] = det(a, c, d) / vl;
+				w[2] = det(b, a, d) / vl;
+				w[3] = 1 - (w[0] + w[1] + w[2]);
+			}
+			return (mindist);
 		}
+		return (-1);
+	}
 };
 
-	// EPA
-	struct	EPA
-	{
-		/* Types		*/
-		typedef	GJK::sSV	sSV;
-		struct	sFace
-		{
-			Vector3	n;
-			real_t	d;
-			real_t	p;
-			sSV*		c[3];
-			sFace*		f[3];
-			sFace*		l[2];
-			U1			e[3];
-			U1			pass;
-		};
-		struct	sList
-		{
-			sFace*		root;
-			U			count;
-			sList() : root(0),count(0)	{}
-		};
-		struct	sHorizon
-		{
-			sFace*		cf;
-			sFace*		ff;
-			U			nf;
-			sHorizon() : cf(0),ff(0),nf(0)	{}
-		};
-		struct	eStatus { enum _ {
+// EPA
+struct EPA {
+	/* Types		*/
+	typedef GJK::sSV sSV;
+	struct sFace {
+		Vector3 n;
+		real_t d;
+		real_t p;
+		sSV *c[3];
+		sFace *f[3];
+		sFace *l[2];
+		U1 e[3];
+		U1 pass;
+	};
+	struct sList {
+		sFace *root;
+		U count;
+		sList()
+			: root(0), count(0) {}
+	};
+	struct sHorizon {
+		sFace *cf;
+		sFace *ff;
+		U nf;
+		sHorizon()
+			: cf(0), ff(0), nf(0) {}
+	};
+	struct eStatus {
+		enum _ {
 			Valid,
 			Touching,
 			Degenerated,
@@ -504,271 +467,255 @@ struct	GJK
 			OutOfVertices,
 			AccuraryReached,
 			FallBack,
-			Failed		};};
-			/* Fields		*/
-			eStatus::_		m_status;
-			GJK::sSimplex	m_result;
-			Vector3		m_normal;
-			real_t		m_depth;
-			sSV				m_sv_store[EPA_MAX_VERTICES];
-			sFace			m_fc_store[EPA_MAX_FACES];
-			U				m_nextsv;
-			sList			m_hull;
-			sList			m_stock;
-			/* Methods		*/
-			EPA()
-			{
-				Initialize();
-			}
-
+			Failed
+		};
+	};
+	/* Fields		*/
+	eStatus::_ m_status;
+	GJK::sSimplex m_result;
+	Vector3 m_normal;
+	real_t m_depth;
+	sSV m_sv_store[EPA_MAX_VERTICES];
+	sFace m_fc_store[EPA_MAX_FACES];
+	U m_nextsv;
+	sList m_hull;
+	sList m_stock;
+	/* Methods		*/
+	EPA() {
+		Initialize();
+	}
 
-			static inline void		bind(sFace* fa,U ea,sFace* fb,U eb)
-			{
-				fa->e[ea]=(U1)eb;fa->f[ea]=fb;
-				fb->e[eb]=(U1)ea;fb->f[eb]=fa;
-			}
-			static inline void		append(sList& list,sFace* face)
-			{
-				face->l[0]	=	0;
-				face->l[1]	=	list.root;
-				if(list.root) list.root->l[0]=face;
-				list.root	=	face;
-				++list.count;
-			}
-			static inline void		remove(sList& list,sFace* face)
-			{
-				if(face->l[1]) face->l[1]->l[0]=face->l[0];
-				if(face->l[0]) face->l[0]->l[1]=face->l[1];
-				if(face==list.root) list.root=face->l[1];
-				--list.count;
-			}
+	static inline void bind(sFace *fa, U ea, sFace *fb, U eb) {
+		fa->e[ea] = (U1)eb;
+		fa->f[ea] = fb;
+		fb->e[eb] = (U1)ea;
+		fb->f[eb] = fa;
+	}
+	static inline void append(sList &list, sFace *face) {
+		face->l[0] = 0;
+		face->l[1] = list.root;
+		if (list.root) list.root->l[0] = face;
+		list.root = face;
+		++list.count;
+	}
+	static inline void remove(sList &list, sFace *face) {
+		if (face->l[1]) face->l[1]->l[0] = face->l[0];
+		if (face->l[0]) face->l[0]->l[1] = face->l[1];
+		if (face == list.root) list.root = face->l[1];
+		--list.count;
+	}
 
+	void Initialize() {
+		m_status = eStatus::Failed;
+		m_normal = Vector3(0, 0, 0);
+		m_depth = 0;
+		m_nextsv = 0;
+		for (U i = 0; i < EPA_MAX_FACES; ++i) {
+			append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]);
+		}
+	}
+	eStatus::_ Evaluate(GJK &gjk, const Vector3 &guess) {
+		GJK::sSimplex &simplex = *gjk.m_simplex;
+		if ((simplex.rank > 1) && gjk.EncloseOrigin()) {
 
-			void				Initialize()
-			{
-				m_status	=	eStatus::Failed;
-				m_normal	=	Vector3(0,0,0);
-				m_depth		=	0;
-				m_nextsv	=	0;
-				for(U i=0;i<EPA_MAX_FACES;++i)
-				{
-					append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
-				}
+			/* Clean up				*/
+			while (m_hull.root) {
+				sFace *f = m_hull.root;
+				remove(m_hull, f);
+				append(m_stock, f);
 			}
-			eStatus::_			Evaluate(GJK& gjk,const Vector3& guess)
-			{
-				GJK::sSimplex&	simplex=*gjk.m_simplex;
-				if((simplex.rank>1)&&gjk.EncloseOrigin())
-				{
-
-					/* Clean up				*/
-					while(m_hull.root)
-					{
-						sFace*	f = m_hull.root;
-						remove(m_hull,f);
-						append(m_stock,f);
-					}
-					m_status	=	eStatus::Valid;
-					m_nextsv	=	0;
-					/* Orient simplex		*/
-					if(gjk.det(	simplex.c[0]->w-simplex.c[3]->w,
-						simplex.c[1]->w-simplex.c[3]->w,
-						simplex.c[2]->w-simplex.c[3]->w)<0)
-					{
-						SWAP(simplex.c[0],simplex.c[1]);
-						SWAP(simplex.p[0],simplex.p[1]);
-					}
-					/* Build initial hull	*/
-					sFace*	tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
-						newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
-						newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
-						newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
-					if(m_hull.count==4)
-					{
-						sFace*		best=findbest();
-						sFace		outer=*best;
-						U			pass=0;
-						U			iterations=0;
-						bind(tetra[0],0,tetra[1],0);
-						bind(tetra[0],1,tetra[2],0);
-						bind(tetra[0],2,tetra[3],0);
-						bind(tetra[1],1,tetra[3],2);
-						bind(tetra[1],2,tetra[2],1);
-						bind(tetra[2],2,tetra[3],1);
-						m_status=eStatus::Valid;
-						for(;iterations<EPA_MAX_ITERATIONS;++iterations)
-						{
-							if(m_nextsv<EPA_MAX_VERTICES)
-							{
-								sHorizon		horizon;
-								sSV*			w=&m_sv_store[m_nextsv++];
-								bool			valid=true;
-								best->pass	=	(U1)(++pass);
-								gjk.getsupport(best->n,*w);
-								const real_t	wdist=vec3_dot(best->n,w->w)-best->d;
-								if(wdist>EPA_ACCURACY)
-								{
-									for(U j=0;(j<3)&&valid;++j)
-									{
-										valid&=expand(	pass,w,
-											best->f[j],best->e[j],
-											horizon);
-									}
-									if(valid&&(horizon.nf>=3))
-									{
-										bind(horizon.cf,1,horizon.ff,2);
-										remove(m_hull,best);
-										append(m_stock,best);
-										best=findbest();
-										if(best->p>=outer.p) outer=*best;
-									} else { m_status=eStatus::InvalidHull;break; }
-								} else { m_status=eStatus::AccuraryReached;break; }
-							} else { m_status=eStatus::OutOfVertices;break; }
+			m_status = eStatus::Valid;
+			m_nextsv = 0;
+			/* Orient simplex		*/
+			if (gjk.det(simplex.c[0]->w - simplex.c[3]->w,
+						simplex.c[1]->w - simplex.c[3]->w,
+						simplex.c[2]->w - simplex.c[3]->w) < 0) {
+				SWAP(simplex.c[0], simplex.c[1]);
+				SWAP(simplex.p[0], simplex.p[1]);
+			}
+			/* Build initial hull	*/
+			sFace *tetra[] = { newface(simplex.c[0], simplex.c[1], simplex.c[2], true),
+				newface(simplex.c[1], simplex.c[0], simplex.c[3], true),
+				newface(simplex.c[2], simplex.c[1], simplex.c[3], true),
+				newface(simplex.c[0], simplex.c[2], simplex.c[3], true) };
+			if (m_hull.count == 4) {
+				sFace *best = findbest();
+				sFace outer = *best;
+				U pass = 0;
+				U iterations = 0;
+				bind(tetra[0], 0, tetra[1], 0);
+				bind(tetra[0], 1, tetra[2], 0);
+				bind(tetra[0], 2, tetra[3], 0);
+				bind(tetra[1], 1, tetra[3], 2);
+				bind(tetra[1], 2, tetra[2], 1);
+				bind(tetra[2], 2, tetra[3], 1);
+				m_status = eStatus::Valid;
+				for (; iterations < EPA_MAX_ITERATIONS; ++iterations) {
+					if (m_nextsv < EPA_MAX_VERTICES) {
+						sHorizon horizon;
+						sSV *w = &m_sv_store[m_nextsv++];
+						bool valid = true;
+						best->pass = (U1)(++pass);
+						gjk.getsupport(best->n, *w);
+						const real_t wdist = vec3_dot(best->n, w->w) - best->d;
+						if (wdist > EPA_ACCURACY) {
+							for (U j = 0; (j < 3) && valid; ++j) {
+								valid &= expand(pass, w,
+										best->f[j], best->e[j],
+										horizon);
+							}
+							if (valid && (horizon.nf >= 3)) {
+								bind(horizon.cf, 1, horizon.ff, 2);
+								remove(m_hull, best);
+								append(m_stock, best);
+								best = findbest();
+								if (best->p >= outer.p) outer = *best;
+							} else {
+								m_status = eStatus::InvalidHull;
+								break;
+							}
+						} else {
+							m_status = eStatus::AccuraryReached;
+							break;
 						}
-						const Vector3	projection=outer.n*outer.d;
-						m_normal	=	outer.n;
-						m_depth		=	outer.d;
-						m_result.rank	=	3;
-						m_result.c[0]	=	outer.c[0];
-						m_result.c[1]	=	outer.c[1];
-						m_result.c[2]	=	outer.c[2];
-						m_result.p[0]	=	vec3_cross(	outer.c[1]->w-projection,
-							outer.c[2]->w-projection).length();
-						m_result.p[1]	=	vec3_cross(	outer.c[2]->w-projection,
-							outer.c[0]->w-projection).length();
-						m_result.p[2]	=	vec3_cross(	outer.c[0]->w-projection,
-							outer.c[1]->w-projection).length();
-						const real_t	sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
-						m_result.p[0]	/=	sum;
-						m_result.p[1]	/=	sum;
-						m_result.p[2]	/=	sum;
-						return(m_status);
+					} else {
+						m_status = eStatus::OutOfVertices;
+						break;
 					}
 				}
-				/* Fallback		*/
-				m_status	=	eStatus::FallBack;
-				m_normal	=	-guess;
-				const real_t	nl=m_normal.length();
-				if(nl>0)
-					m_normal	=	m_normal/nl;
-				else
-					m_normal	=	Vector3(1,0,0);
-				m_depth	=	0;
-				m_result.rank=1;
-				m_result.c[0]=simplex.c[0];
-				m_result.p[0]=1;
-				return(m_status);
-			}
-			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
-			{
-				if(m_stock.root)
-				{
-					sFace*	face=m_stock.root;
-					remove(m_stock,face);
-					append(m_hull,face);
-					face->pass	=	0;
-					face->c[0]	=	a;
-					face->c[1]	=	b;
-					face->c[2]	=	c;
-					face->n		=	vec3_cross(b->w-a->w,c->w-a->w);
-					const real_t	l=face->n.length();
-					const bool		v=l>EPA_ACCURACY;
-					face->p		=	MIN(MIN(
-						vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)),
-						vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))),
-						vec3_dot(c->w,vec3_cross(face->n,c->w-a->w)))	/
-						(v?l:1);
-					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
-					if(v)
-					{
-						face->d		=	vec3_dot(a->w,face->n)/l;
-						face->n		/=	l;
-						if(forced||(face->d>=-EPA_PLANE_EPS))
-						{
-							return(face);
-						} else m_status=eStatus::NonConvex;
-					} else m_status=eStatus::Degenerated;
-					remove(m_hull,face);
-					append(m_stock,face);
-					return(0);
-				}
-				m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
-				return(0);
+				const Vector3 projection = outer.n * outer.d;
+				m_normal = outer.n;
+				m_depth = outer.d;
+				m_result.rank = 3;
+				m_result.c[0] = outer.c[0];
+				m_result.c[1] = outer.c[1];
+				m_result.c[2] = outer.c[2];
+				m_result.p[0] = vec3_cross(outer.c[1]->w - projection,
+						outer.c[2]->w - projection)
+										.length();
+				m_result.p[1] = vec3_cross(outer.c[2]->w - projection,
+						outer.c[0]->w - projection)
+										.length();
+				m_result.p[2] = vec3_cross(outer.c[0]->w - projection,
+						outer.c[1]->w - projection)
+										.length();
+				const real_t sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
+				m_result.p[0] /= sum;
+				m_result.p[1] /= sum;
+				m_result.p[2] /= sum;
+				return (m_status);
 			}
-			sFace*				findbest()
-			{
-				sFace*		minf=m_hull.root;
-				real_t	mind=minf->d*minf->d;
-				real_t	maxp=minf->p;
-				for(sFace* f=minf->l[1];f;f=f->l[1])
-				{
-					const real_t	sqd=f->d*f->d;
-					if((f->p>=maxp)&&(sqd<mind))
-					{
-						minf=f;
-						mind=sqd;
-						maxp=f->p;
-					}
-				}
-				return(minf);
+		}
+		/* Fallback		*/
+		m_status = eStatus::FallBack;
+		m_normal = -guess;
+		const real_t nl = m_normal.length();
+		if (nl > 0)
+			m_normal = m_normal / nl;
+		else
+			m_normal = Vector3(1, 0, 0);
+		m_depth = 0;
+		m_result.rank = 1;
+		m_result.c[0] = simplex.c[0];
+		m_result.p[0] = 1;
+		return (m_status);
+	}
+	sFace *newface(sSV *a, sSV *b, sSV *c, bool forced) {
+		if (m_stock.root) {
+			sFace *face = m_stock.root;
+			remove(m_stock, face);
+			append(m_hull, face);
+			face->pass = 0;
+			face->c[0] = a;
+			face->c[1] = b;
+			face->c[2] = c;
+			face->n = vec3_cross(b->w - a->w, c->w - a->w);
+			const real_t l = face->n.length();
+			const bool v = l > EPA_ACCURACY;
+			face->p = MIN(MIN(
+								  vec3_dot(a->w, vec3_cross(face->n, a->w - b->w)),
+								  vec3_dot(b->w, vec3_cross(face->n, b->w - c->w))),
+							  vec3_dot(c->w, vec3_cross(face->n, c->w - a->w))) /
+					  (v ? l : 1);
+			face->p = face->p >= -EPA_INSIDE_EPS ? 0 : face->p;
+			if (v) {
+				face->d = vec3_dot(a->w, face->n) / l;
+				face->n /= l;
+				if (forced || (face->d >= -EPA_PLANE_EPS)) {
+					return (face);
+				} else
+					m_status = eStatus::NonConvex;
+			} else
+				m_status = eStatus::Degenerated;
+			remove(m_hull, face);
+			append(m_stock, face);
+			return (0);
+		}
+		m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
+		return (0);
+	}
+	sFace *findbest() {
+		sFace *minf = m_hull.root;
+		real_t mind = minf->d * minf->d;
+		real_t maxp = minf->p;
+		for (sFace *f = minf->l[1]; f; f = f->l[1]) {
+			const real_t sqd = f->d * f->d;
+			if ((f->p >= maxp) && (sqd < mind)) {
+				minf = f;
+				mind = sqd;
+				maxp = f->p;
 			}
-			bool				expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
-			{
-				static const U	i1m3[]={1,2,0};
-				static const U	i2m3[]={2,0,1};
-				if(f->pass!=pass)
-				{
-					const U	e1=i1m3[e];
-					if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
-					{
-						sFace*	nf=newface(f->c[e1],f->c[e],w,false);
-						if(nf)
-						{
-							bind(nf,0,f,e);
-							if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
-							horizon.cf=nf;
-							++horizon.nf;
-							return(true);
-						}
-					}
+		}
+		return (minf);
+	}
+	bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon) {
+		static const U i1m3[] = { 1, 2, 0 };
+		static const U i2m3[] = { 2, 0, 1 };
+		if (f->pass != pass) {
+			const U e1 = i1m3[e];
+			if ((vec3_dot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) {
+				sFace *nf = newface(f->c[e1], f->c[e], w, false);
+				if (nf) {
+					bind(nf, 0, f, e);
+					if (horizon.cf)
+						bind(horizon.cf, 1, nf, 2);
 					else
-					{
-						const U	e2=i2m3[e];
-						f->pass		=	(U1)pass;
-						if(	expand(pass,w,f->f[e1],f->e[e1],horizon)&&
-							expand(pass,w,f->f[e2],f->e[e2],horizon))
-						{
-							remove(m_hull,f);
-							append(m_stock,f);
-							return(true);
-						}
-					}
+						horizon.ff = nf;
+					horizon.cf = nf;
+					++horizon.nf;
+					return (true);
+				}
+			} else {
+				const U e2 = i2m3[e];
+				f->pass = (U1)pass;
+				if (expand(pass, w, f->f[e1], f->e[e1], horizon) &&
+						expand(pass, w, f->f[e2], f->e[e2], horizon)) {
+					remove(m_hull, f);
+					append(m_stock, f);
+					return (true);
 				}
-				return(false);
 			}
-
-	};
-
-	//
-	static void	Initialize(	const ShapeSW* shape0,const Transform& wtrs0,
-		const ShapeSW* shape1,const Transform& wtrs1,
-		sResults& results,
-		tShape& shape,
-		bool withmargins)
-	{
-		/* Results		*/
-		results.witnesses[0]	=
-			results.witnesses[1]	=	Vector3(0,0,0);
-		results.status			=	sResults::Separated;
-		/* Shape		*/
-		shape.m_shapes[0]		=	shape0;
-		shape.m_shapes[1]		=	shape1;
-		shape.transform_A		=	wtrs0;
-		shape.transform_B		=	wtrs1;
-
+		}
+		return (false);
 	}
+};
 
-
+//
+static void Initialize(const ShapeSW *shape0, const Transform &wtrs0,
+		const ShapeSW *shape1, const Transform &wtrs1,
+		sResults &results,
+		tShape &shape,
+		bool withmargins) {
+	/* Results		*/
+	results.witnesses[0] =
+			results.witnesses[1] = Vector3(0, 0, 0);
+	results.status = sResults::Separated;
+	/* Shape		*/
+	shape.m_shapes[0] = shape0;
+	shape.m_shapes[1] = shape1;
+	shape.transform_A = wtrs0;
+	shape.transform_B = wtrs1;
+}
 
 //
 // Api
@@ -777,87 +724,75 @@ struct	GJK
 //
 
 //
-bool Distance(	const ShapeSW*	shape0,
-									  const Transform&		wtrs0,
-									  const ShapeSW*	shape1,
-									  const Transform&		wtrs1,
-									  const Vector3&		guess,
-									  sResults&				results)
-{
-	tShape			shape;
-	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
-	GJK				gjk;
-	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,guess);
-	if(gjk_status==GJK::eStatus::Valid)
-	{
-		Vector3	w0=Vector3(0,0,0);
-		Vector3	w1=Vector3(0,0,0);
-		for(U i=0;i<gjk.m_simplex->rank;++i)
-		{
-			const real_t	p=gjk.m_simplex->p[i];
-			w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
-			w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
+bool Distance(const ShapeSW *shape0,
+		const Transform &wtrs0,
+		const ShapeSW *shape1,
+		const Transform &wtrs1,
+		const Vector3 &guess,
+		sResults &results) {
+	tShape shape;
+	Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false);
+	GJK gjk;
+	GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess);
+	if (gjk_status == GJK::eStatus::Valid) {
+		Vector3 w0 = Vector3(0, 0, 0);
+		Vector3 w1 = Vector3(0, 0, 0);
+		for (U i = 0; i < gjk.m_simplex->rank; ++i) {
+			const real_t p = gjk.m_simplex->p[i];
+			w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p;
+			w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p;
 		}
-		results.witnesses[0]	=	w0;
-		results.witnesses[1]	=	w1;
-		results.normal			=	w0-w1;
-		results.distance		=	results.normal.length();
-		results.normal			/=	results.distance>GJK_MIN_DISTANCE?results.distance:1;
-		return(true);
-	}
-	else
-	{
-		results.status	=	gjk_status==GJK::eStatus::Inside?
-			sResults::Penetrating	:
-		sResults::GJK_Failed	;
-		return(false);
+		results.witnesses[0] = w0;
+		results.witnesses[1] = w1;
+		results.normal = w0 - w1;
+		results.distance = results.normal.length();
+		results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
+		return (true);
+	} else {
+		results.status = gjk_status == GJK::eStatus::Inside ?
+								 sResults::Penetrating :
+								 sResults::GJK_Failed;
+		return (false);
 	}
 }
 
 //
-bool Penetration(	const ShapeSW*	shape0,
-									 const Transform&		wtrs0,
-									 const ShapeSW*	shape1,
-									 const Transform&		wtrs1,
-									 const Vector3&		guess,
-									 sResults&				results
-									)
-{
-	tShape			shape;
-	Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
-	GJK				gjk;
-	GJK::eStatus::_	gjk_status=gjk.Evaluate(shape,-guess);
-	switch(gjk_status)
-	{
-	case	GJK::eStatus::Inside:
-		{
-			EPA				epa;
-			EPA::eStatus::_	epa_status=epa.Evaluate(gjk,-guess);
-			if(epa_status!=EPA::eStatus::Failed)
-			{
-				Vector3	w0=Vector3(0,0,0);
-				for(U i=0;i<epa.m_result.rank;++i)
-				{
-					w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
+bool Penetration(const ShapeSW *shape0,
+		const Transform &wtrs0,
+		const ShapeSW *shape1,
+		const Transform &wtrs1,
+		const Vector3 &guess,
+		sResults &results) {
+	tShape shape;
+	Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false);
+	GJK gjk;
+	GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess);
+	switch (gjk_status) {
+		case GJK::eStatus::Inside: {
+			EPA epa;
+			EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess);
+			if (epa_status != EPA::eStatus::Failed) {
+				Vector3 w0 = Vector3(0, 0, 0);
+				for (U i = 0; i < epa.m_result.rank; ++i) {
+					w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i];
 				}
-				results.status			=	sResults::Penetrating;
-				results.witnesses[0]	=	w0;
-				results.witnesses[1]	=	w0-epa.m_normal*epa.m_depth;
-				results.normal			=	-epa.m_normal;
-				results.distance		=	-epa.m_depth;
-				return(true);
-			} else results.status=sResults::EPA_Failed;
-		}
-		break;
-	case	GJK::eStatus::Failed:
-		results.status=sResults::GJK_Failed;
-		break;
-	default: {}
+				results.status = sResults::Penetrating;
+				results.witnesses[0] = w0;
+				results.witnesses[1] = w0 - epa.m_normal * epa.m_depth;
+				results.normal = -epa.m_normal;
+				results.distance = -epa.m_depth;
+				return (true);
+			} else
+				results.status = sResults::EPA_Failed;
+		} break;
+		case GJK::eStatus::Failed:
+			results.status = sResults::GJK_Failed;
+			break;
+		default: {}
 	}
-	return(false);
+	return (false);
 }
 
-
 /* Symbols cleanup		*/
 
 #undef GJK_MAX_ITERATIONS
@@ -876,43 +811,35 @@ bool Penetration(	const ShapeSW*	shape0,
 #undef EPA_PLANE_EPS
 #undef EPA_INSIDE_EPS
 
-
 } // end of namespace
 
-
-
-
-
-bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B) {
-
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
 
 	GjkEpa2::sResults res;
 
-	if (GjkEpa2::Distance(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) {
+	if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
 
-		r_result_A=res.witnesses[0];
-		r_result_B=res.witnesses[1];
+		r_result_A = res.witnesses[0];
+		r_result_B = res.witnesses[1];
 		return true;
 	}
 
 	return false;
-
 }
 
-bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) {
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
 
 	GjkEpa2::sResults res;
 
-	if (GjkEpa2::Penetration(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) {
+	if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
 		if (p_result_callback) {
 			if (p_swap)
-				p_result_callback(res.witnesses[1],res.witnesses[0],p_userdata);
+				p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
 			else
-				p_result_callback(res.witnesses[0],res.witnesses[1],p_userdata);
+				p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
 		}
 		return true;
 	}
 
 	return false;
 }
-
diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h
index 58cf8f50c..ae5db733b 100644
--- a/servers/physics/gjk_epa.h
+++ b/servers/physics/gjk_epa.h
@@ -35,7 +35,7 @@
 */
 #include "collision_solver_sw.h"
 
-bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false);
-bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, Vector3& r_result_A, Vector3& r_result_B);
+bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false);
+bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
 
 #endif
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
index c94cc8bd8..08f1d1e1a 100644
--- a/servers/physics/joints/cone_twist_joint_sw.cpp
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -34,29 +34,26 @@ See corresponding header file for licensing info.
 
 #include "cone_twist_joint_sw.h"
 
-static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
+static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
 
-  if (Math::abs(n.z) > 0.707106781186547524400844362) {
-    // choose p in y-z plane
-    real_t a = n[1]*n[1] + n[2]*n[2];
-    real_t k = 1.0/Math::sqrt(a);
-    p=Vector3(0,-n[2]*k,n[1]*k);
-    // set q = n x p
-    q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
-  }
-  else {
-    // choose p in x-y plane
-    real_t a = n.x*n.x + n.y*n.y;
-    real_t k = 1.0/Math::sqrt(a);
-    p=Vector3(-n.y*k,n.x*k,0);
-    // set q = n x p
-    q=Vector3(-n.z*p.y,n.z*p.x,a*k);
-  }
+	if (Math::abs(n.z) > 0.707106781186547524400844362) {
+		// choose p in y-z plane
+		real_t a = n[1] * n[1] + n[2] * n[2];
+		real_t k = 1.0 / Math::sqrt(a);
+		p = Vector3(0, -n[2] * k, n[1] * k);
+		// set q = n x p
+		q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
+	} else {
+		// choose p in x-y plane
+		real_t a = n.x * n.x + n.y * n.y;
+		real_t k = 1.0 / Math::sqrt(a);
+		p = Vector3(-n.y * k, n.x * k, 0);
+		// set q = n x p
+		q = Vector3(-n.z * p.y, n.z * p.x, a * k);
+	}
 }
 
-
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	real_t coeff_1 = Math_PI / 4.0f;
 	real_t coeff_2 = 3.0f * coeff_1;
 	real_t abs_y = Math::abs(y);
@@ -71,32 +68,31 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
 	return (y < 0.0f) ? -angle : angle;
 }
 
-ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) :  JointSW(_arr,2) {
+ConeTwistJointSW::ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame)
+	: JointSW(_arr, 2) {
 
-	A=rbA;
-	B=rbB;
+	A = rbA;
+	B = rbB;
 
+	m_rbAFrame = rbAFrame;
+	m_rbBFrame = rbBFrame;
 
-	m_rbAFrame=rbAFrame;
-	m_rbBFrame=rbBFrame;
-
-	m_swingSpan1 = Math_PI/4.0;
-	m_swingSpan2 = Math_PI/4.0;
-	m_twistSpan  = Math_PI*2;
+	m_swingSpan1 = Math_PI / 4.0;
+	m_swingSpan2 = Math_PI / 4.0;
+	m_twistSpan = Math_PI * 2;
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 
-	m_appliedImpulse=0;
+	m_appliedImpulse = 0;
 }
 
-
-bool	ConeTwistJointSW::setup(float p_step) {
+bool ConeTwistJointSW::setup(float p_step) {
 	m_appliedImpulse = real_t(0.);
 
 	//set bias, sign, clear accumulator
@@ -107,109 +103,97 @@ bool	ConeTwistJointSW::setup(float p_step) {
 	m_accTwistLimitImpulse = real_t(0.);
 	m_accSwingLimitImpulse = real_t(0.);
 
-	if (!m_angularOnly)
-	{
+	if (!m_angularOnly) {
 		Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
 		Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
 		Vector3 relPos = pivotBInW - pivotAInW;
 
 		Vector3 normal[3];
-		if (relPos.length_squared() > CMP_EPSILON)
-		{
+		if (relPos.length_squared() > CMP_EPSILON) {
 			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0]=Vector3(real_t(1.0),0,0);
+		} else {
+			normal[0] = Vector3(real_t(1.0), 0, 0);
 		}
 
 		plane_space(normal[0], normal[1], normal[2]);
 
-		for (int i=0;i<3;i++)
-		{
+		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,
-				normal[i],
-				A->get_inv_inertia(),
-				A->get_inv_mass(),
-				B->get_inv_inertia(),
-				B->get_inv_mass()));
+												A->get_transform().basis.transposed(),
+												B->get_transform().basis.transposed(),
+												pivotAInW - A->get_transform().origin,
+												pivotBInW - B->get_transform().origin,
+												normal[i],
+												A->get_inv_inertia(),
+												A->get_inv_mass(),
+												B->get_inv_inertia(),
+												B->get_inv_mass()));
 		}
 	}
 
-	Vector3 b1Axis1,b1Axis2,b1Axis3;
-	Vector3 b2Axis1,b2Axis2;
+	Vector3 b1Axis1, b1Axis2, b1Axis3;
+	Vector3 b2Axis1, b2Axis2;
 
-	b1Axis1 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(0) );
-	b2Axis1 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(0) );
+	b1Axis1 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(0));
+	b2Axis1 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(0));
 
-	real_t swing1=real_t(0.),swing2 = real_t(0.);
+	real_t swing1 = real_t(0.), swing2 = real_t(0.);
 
-	real_t swx=real_t(0.),swy = real_t(0.);
+	real_t swx = real_t(0.), swy = real_t(0.);
 	real_t thresh = real_t(10.);
 	real_t fact;
 
 	// Get Frame into world space
-	if (m_swingSpan1 >= real_t(0.05f))
-	{
-		b1Axis2 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(1) );
-//		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+	if (m_swingSpan1 >= real_t(0.05f)) {
+		b1Axis2 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(1));
+		//		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis2);
-		swing1  = atan2fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing1 = atan2fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + real_t(1.0));
 		swing1 *= fact;
-
 	}
 
-	if (m_swingSpan2 >= real_t(0.05f))
-	{
-		b1Axis3 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(2) );
-//		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+	if (m_swingSpan2 >= real_t(0.05f)) {
+		b1Axis3 = A->get_transform().basis.xform(this->m_rbAFrame.basis.get_axis(2));
+		//		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis3);
-		swing2  = atan2fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing2 = atan2fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + real_t(1.0));
 		swing2 *= fact;
 	}
 
-	real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
-	real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
-	real_t EllipseAngle = Math::abs(swing1*swing1)* RMaxAngle1Sq + Math::abs(swing2*swing2) * RMaxAngle2Sq;
+	real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
+	real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
+	real_t EllipseAngle = Math::abs(swing1 * swing1) * RMaxAngle1Sq + Math::abs(swing2 * swing2) * RMaxAngle2Sq;
 
-	if (EllipseAngle > 1.0f)
-	{
-		m_swingCorrection = EllipseAngle-1.0f;
+	if (EllipseAngle > 1.0f) {
+		m_swingCorrection = EllipseAngle - 1.0f;
 		m_solveSwingLimit = true;
 
 		// Calculate necessary axis & factors
-		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
 		m_swingAxis.normalize();
 
 		real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
 
-		m_kSwing =  real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
-			B->compute_angular_impulse_denominator(m_swingAxis));
-
+		m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
+										B->compute_angular_impulse_denominator(m_swingAxis));
 	}
 
 	// Twist limits
-	if (m_twistSpan >= real_t(0.))
-	{
-		Vector3 b2Axis2 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(1) );
-		Quat rotationArc = Quat(b2Axis1,b1Axis1);
+	if (m_twistSpan >= real_t(0.)) {
+		Vector3 b2Axis2 = B->get_transform().basis.xform(this->m_rbBFrame.basis.get_axis(1));
+		Quat rotationArc = Quat(b2Axis1, b1Axis1);
 		Vector3 TwistRef = rotationArc.xform(b2Axis2);
-		real_t twist = atan2fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+		real_t twist = atan2fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
 
 		real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
-		if (twist <= -m_twistSpan*lockedFreeFactor)
-		{
+		if (twist <= -m_twistSpan * lockedFreeFactor) {
 			m_twistCorrection = -(twist + m_twistSpan);
 			m_solveTwistLimit = true;
 
@@ -218,28 +202,24 @@ bool	ConeTwistJointSW::setup(float p_step) {
 			m_twistAxis *= -1.0f;
 
 			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
-				B->compute_angular_impulse_denominator(m_twistAxis));
-
-		}	else
-			if (twist >  m_twistSpan*lockedFreeFactor)
-			{
-				m_twistCorrection = (twist - m_twistSpan);
-				m_solveTwistLimit = true;
+											B->compute_angular_impulse_denominator(m_twistAxis));
 
-				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
-				m_twistAxis.normalize();
+		} else if (twist > m_twistSpan * lockedFreeFactor) {
+			m_twistCorrection = (twist - m_twistSpan);
+			m_solveTwistLimit = true;
 
-				m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
-					B->compute_angular_impulse_denominator(m_twistAxis));
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
 
-			}
+			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
+											B->compute_angular_impulse_denominator(m_twistAxis));
+		}
 	}
 
 	return true;
 }
 
-void	ConeTwistJointSW::solve(real_t	timeStep)
-{
+void ConeTwistJointSW::solve(real_t timeStep) {
 
 	Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
 	Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@@ -247,8 +227,7 @@ void	ConeTwistJointSW::solve(real_t	timeStep)
 	real_t tau = real_t(0.3);
 
 	//linear part
-	if (!m_angularOnly)
-	{
+	if (!m_angularOnly) {
 		Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
 		Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
 
@@ -256,16 +235,15 @@ void	ConeTwistJointSW::solve(real_t	timeStep)
 		Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
 		Vector3 vel = vel1 - vel2;
 
-		for (int i=0;i<3;i++)
-		{
-			const Vector3& normal = m_jac[i].m_linearJointAxis;
+		for (int i = 0; i < 3; i++) {
+			const Vector3 &normal = m_jac[i].m_linearJointAxis;
 			real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
 
 			real_t rel_vel;
 			rel_vel = normal.dot(vel);
 			//positional error (zeroth order error)
 			real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-			real_t impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			real_t impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
 			m_appliedImpulse += impulse;
 			Vector3 impulse_vector = normal * impulse;
 			A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
@@ -275,79 +253,73 @@ void	ConeTwistJointSW::solve(real_t	timeStep)
 
 	{
 		///solve angular part
-		const Vector3& angVelA = A->get_angular_velocity();
-		const Vector3& angVelB = B->get_angular_velocity();
+		const Vector3 &angVelA = A->get_angular_velocity();
+		const Vector3 &angVelB = B->get_angular_velocity();
 
 		// solve swing limit
-		if (m_solveSwingLimit)
-		{
-			real_t amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(real_t(1.)/timeStep)*m_biasFactor);
+		if (m_solveSwingLimit) {
+			real_t amplitude = ((angVelB - angVelA).dot(m_swingAxis) * m_relaxationFactor * m_relaxationFactor + m_swingCorrection * (real_t(1.) / timeStep) * m_biasFactor);
 			real_t impulseMag = amplitude * m_kSwing;
 
 			// Clamp the accumulated impulse
 			real_t temp = m_accSwingLimitImpulse;
-			m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0) );
+			m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0));
 			impulseMag = m_accSwingLimitImpulse - temp;
 
 			Vector3 impulse = m_swingAxis * impulseMag;
 
 			A->apply_torque_impulse(impulse);
 			B->apply_torque_impulse(-impulse);
-
 		}
 
 		// solve twist limit
-		if (m_solveTwistLimit)
-		{
-			real_t amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(real_t(1.)/timeStep)*m_biasFactor );
+		if (m_solveTwistLimit) {
+			real_t amplitude = ((angVelB - angVelA).dot(m_twistAxis) * m_relaxationFactor * m_relaxationFactor + m_twistCorrection * (real_t(1.) / timeStep) * m_biasFactor);
 			real_t impulseMag = amplitude * m_kTwist;
 
 			// Clamp the accumulated impulse
 			real_t temp = m_accTwistLimitImpulse;
-			m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0) );
+			m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0));
 			impulseMag = m_accTwistLimitImpulse - temp;
 
 			Vector3 impulse = m_twistAxis * impulseMag;
 
 			A->apply_torque_impulse(impulse);
 			B->apply_torque_impulse(-impulse);
-
 		}
-
 	}
-
 }
 
 void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
 
-			m_swingSpan1=p_value;
-			m_swingSpan2=p_value;
+			m_swingSpan1 = p_value;
+			m_swingSpan2 = p_value;
 		} break;
 		case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
 
-			m_twistSpan=p_value;
+			m_twistSpan = p_value;
 		} break;
 		case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
 
-			m_biasFactor=p_value;
+			m_biasFactor = p_value;
 		} break;
 		case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
 
-			m_limitSoftness=p_value;
+			m_limitSoftness = p_value;
 		} break;
 		case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
 
-			m_relaxationFactor=p_value;
+			m_relaxationFactor = p_value;
 		} break;
 	}
 }
 
-float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
+float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
 
 			return m_swingSpan1;
diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h
index 0d64d6744..f38806cbd 100644
--- a/servers/physics/joints/cone_twist_joint_sw.h
+++ b/servers/physics/joints/cone_twist_joint_sw.h
@@ -51,14 +51,11 @@ Written by: Marcus Hennix
 #ifndef CONE_TWIST_JOINT_SW_H
 #define CONE_TWIST_JOINT_SW_H
 
-#include "servers/physics/joints_sw.h"
 #include "servers/physics/joints/jacobian_entry_sw.h"
-
-
+#include "servers/physics/joints_sw.h"
 
 ///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
-class ConeTwistJointSW : public JointSW
-{
+class ConeTwistJointSW : public JointSW {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
@@ -72,86 +69,73 @@ public:
 		BodySW *_arr[2];
 	};
 
-	JacobianEntrySW	m_jac[3]; //3 orthogonal linear constraints
-
+	JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
 
 	real_t m_appliedImpulse;
 	Transform m_rbAFrame;
 	Transform m_rbBFrame;
 
-	real_t	m_limitSoftness;
-	real_t	m_biasFactor;
-	real_t	m_relaxationFactor;
-
-	real_t	m_swingSpan1;
-	real_t	m_swingSpan2;
-	real_t	m_twistSpan;
+	real_t m_limitSoftness;
+	real_t m_biasFactor;
+	real_t m_relaxationFactor;
 
-	Vector3   m_swingAxis;
-	Vector3	m_twistAxis;
+	real_t m_swingSpan1;
+	real_t m_swingSpan2;
+	real_t m_twistSpan;
 
-	real_t	m_kSwing;
-	real_t	m_kTwist;
+	Vector3 m_swingAxis;
+	Vector3 m_twistAxis;
 
-	real_t	m_twistLimitSign;
-	real_t	m_swingCorrection;
-	real_t	m_twistCorrection;
+	real_t m_kSwing;
+	real_t m_kTwist;
 
-	real_t	m_accSwingLimitImpulse;
-	real_t	m_accTwistLimitImpulse;
+	real_t m_twistLimitSign;
+	real_t m_swingCorrection;
+	real_t m_twistCorrection;
 
-	bool		m_angularOnly;
-	bool		m_solveTwistLimit;
-	bool		m_solveSwingLimit;
+	real_t m_accSwingLimitImpulse;
+	real_t m_accTwistLimitImpulse;
 
+	bool m_angularOnly;
+	bool m_solveTwistLimit;
+	bool m_solveSwingLimit;
 
 public:
-
 	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
 
 	virtual bool setup(float p_step);
 	virtual void solve(float p_step);
 
-	ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame);
-
+	ConeTwistJointSW(BodySW *rbA, BodySW *rbB, const Transform &rbAFrame, const Transform &rbBFrame);
 
-	void	setAngularOnly(bool angularOnly)
-	{
+	void setAngularOnly(bool angularOnly) {
 		m_angularOnly = angularOnly;
 	}
 
-	void	setLimit(real_t _swingSpan1,real_t _swingSpan2,real_t _twistSpan,  real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f)
-	{
+	void setLimit(real_t _swingSpan1, real_t _swingSpan2, real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f) {
 		m_swingSpan1 = _swingSpan1;
 		m_swingSpan2 = _swingSpan2;
-		m_twistSpan  = _twistSpan;
+		m_twistSpan = _twistSpan;
 
-		m_limitSoftness =  _softness;
+		m_limitSoftness = _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
 	}
 
-	inline int getSolveTwistLimit()
-	{
+	inline int getSolveTwistLimit() {
 		return m_solveTwistLimit;
 	}
 
-	inline int getSolveSwingLimit()
-	{
+	inline int getSolveSwingLimit() {
 		return m_solveTwistLimit;
 	}
 
-	inline real_t getTwistLimitSign()
-	{
+	inline real_t getTwistLimitSign() {
 		return m_twistLimitSign;
 	}
 
 	void set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value);
 	float get_param(PhysicsServer::ConeTwistJointParam p_param) const;
-
-
 };
 
-
-
 #endif // CONE_TWIST_JOINT_SW_H
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index cd643a853..929bf0f0b 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -34,167 +34,128 @@ See corresponding header file for licensing info.
 
 #include "generic_6dof_joint_sw.h"
 
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-real_t btGetMatrixElem(const Matrix3& mat, int index);
-real_t btGetMatrixElem(const Matrix3& mat, int index)
-{
-	int i = index%3;
-	int j = index/3;
+real_t btGetMatrixElem(const Matrix3 &mat, int index);
+real_t btGetMatrixElem(const Matrix3 &mat, int index) {
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz);
-bool	matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz)
-{
-//	// rot =  cy*cz          -cy*sz           sy
-//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
-//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
-//
-
-		if (btGetMatrixElem(mat,2) < real_t(1.0))
-		{
-			if (btGetMatrixElem(mat,2) > real_t(-1.0))
-			{
-				xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-				xyz[1] = Math::asin(btGetMatrixElem(mat,2));
-				xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
-				return true;
-			}
-			else
-			{
-				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-				xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-				xyz[1] = -Math_PI*0.5;
-				xyz[2] = real_t(0.0);
-				return false;
-			}
-		}
-		else
-		{
-			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-			xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-			xyz[1] = Math_PI*0.5;
-			xyz[2] = 0.0;
+bool matrixToEulerXYZ(const Matrix3 &mat, Vector3 &xyz);
+bool matrixToEulerXYZ(const Matrix3 &mat, Vector3 &xyz) {
+	//	// rot =  cy*cz          -cy*sz           sy
+	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+	//
 
+	if (btGetMatrixElem(mat, 2) < real_t(1.0)) {
+		if (btGetMatrixElem(mat, 2) > real_t(-1.0)) {
+			xyz[0] = Math::atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = Math::asin(btGetMatrixElem(mat, 2));
+			xyz[2] = Math::atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
+			return true;
+		} else {
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -Math::atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
+			xyz[1] = -Math_PI * 0.5;
+			xyz[2] = real_t(0.0);
+			return false;
 		}
-
+	} else {
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = Math::atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
+		xyz[1] = Math_PI * 0.5;
+		xyz[2] = 0.0;
+	}
 
 	return false;
 }
 
-
-
 //////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
 
-
-int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
-{
-	if(m_loLimit>m_hiLimit)
-	{
-		m_currentLimit = 0;//Free from violation
+int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value) {
+	if (m_loLimit > m_hiLimit) {
+		m_currentLimit = 0; //Free from violation
 		return 0;
 	}
 
-	if (test_value < m_loLimit)
-	{
-		m_currentLimit = 1;//low limit violation
-		m_currentLimitError =  test_value - m_loLimit;
+	if (test_value < m_loLimit) {
+		m_currentLimit = 1; //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
 		return 1;
-	}
-	else if (test_value> m_hiLimit)
-	{
-		m_currentLimit = 2;//High limit violation
+	} else if (test_value > m_hiLimit) {
+		m_currentLimit = 2; //High limit violation
 		m_currentLimitError = test_value - m_hiLimit;
 		return 2;
 	};
 
-	m_currentLimit = 0;//Free from violation
+	m_currentLimit = 0; //Free from violation
 	return 0;
-
 }
 
-
 real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
-		real_t timeStep,Vector3& axis,real_t jacDiagABInv,
-		BodySW * body0, BodySW * body1)
-{
-    if (needApplyTorques()==false) return 0.0f;
+		real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
+		BodySW *body0, BodySW *body1) {
+	if (needApplyTorques() == false) return 0.0f;
 
-    real_t target_velocity = m_targetVelocity;
-    real_t maxMotorForce = m_maxMotorForce;
+	real_t target_velocity = m_targetVelocity;
+	real_t maxMotorForce = m_maxMotorForce;
 
 	//current error correction
-    if (m_currentLimit!=0)
-    {
-	target_velocity = -m_ERP*m_currentLimitError/(timeStep);
-	maxMotorForce = m_maxLimitForce;
-    }
-
-    maxMotorForce *= timeStep;
-
-    // current velocity difference
-    Vector3 vel_diff = body0->get_angular_velocity();
-    if (body1)
-    {
-	vel_diff -= body1->get_angular_velocity();
-    }
+	if (m_currentLimit != 0) {
+		target_velocity = -m_ERP * m_currentLimitError / (timeStep);
+		maxMotorForce = m_maxLimitForce;
+	}
 
+	maxMotorForce *= timeStep;
 
+	// current velocity difference
+	Vector3 vel_diff = body0->get_angular_velocity();
+	if (body1) {
+		vel_diff -= body1->get_angular_velocity();
+	}
 
-    real_t rel_vel = axis.dot(vel_diff);
+	real_t rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-    real_t motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
-
-
-    if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON  )
-    {
-	return 0.0f;//no need for applying force
-    }
+	real_t motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
 
+	if (motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON) {
+		return 0.0f; //no need for applying force
+	}
 
 	// correction impulse
-    real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+	real_t unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
 
 	// clip correction impulse
-    real_t clippedMotorImpulse;
-
-    ///@todo: should clip against accumulated impulse
-    if (unclippedMotorImpulse>0.0f)
-    {
-	clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
-    }
-    else
-    {
-	clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
-    }
+	real_t clippedMotorImpulse;
 
+	///@todo: should clip against accumulated impulse
+	if (unclippedMotorImpulse > 0.0f) {
+		clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+	} else {
+		clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+	}
 
 	// sort with accumulated impulses
-    real_t	lo = real_t(-1e30);
-    real_t	hi = real_t(1e30);
-
-    real_t oldaccumImpulse = m_accumulatedImpulse;
-    real_t sum = oldaccumImpulse + clippedMotorImpulse;
-    m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
-
-    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
-
+	real_t lo = real_t(-1e30);
+	real_t hi = real_t(1e30);
 
+	real_t oldaccumImpulse = m_accumulatedImpulse;
+	real_t sum = oldaccumImpulse + clippedMotorImpulse;
+	m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
 
-    Vector3 motorImp = clippedMotorImpulse * axis;
+	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
 
+	Vector3 motorImp = clippedMotorImpulse * axis;
 
-    body0->apply_torque_impulse(motorImp);
-    if (body1) body1->apply_torque_impulse(-motorImp);
-
-    return clippedMotorImpulse;
-
+	body0->apply_torque_impulse(motorImp);
+	if (body1) body1->apply_torque_impulse(-motorImp);
 
+	return clippedMotorImpulse;
 }
 
 //////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -202,120 +163,96 @@ real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
 //////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
 real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
 		real_t timeStep,
-	real_t jacDiagABInv,
-	BodySW* body1,const Vector3 &pointInA,
-	BodySW* body2,const Vector3 &pointInB,
-	int limit_index,
-	const Vector3 & axis_normal_on_a,
-		const Vector3 & anchorPos)
-{
-
-///find relative velocity
-//    Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
-//    Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
-    Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
-    Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
-
-    Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
-    Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
-    Vector3 vel = vel1 - vel2;
+		real_t jacDiagABInv,
+		BodySW *body1, const Vector3 &pointInA,
+		BodySW *body2, const Vector3 &pointInB,
+		int limit_index,
+		const Vector3 &axis_normal_on_a,
+		const Vector3 &anchorPos) {
 
-    real_t rel_vel = axis_normal_on_a.dot(vel);
+	///find relative velocity
+	//    Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
+	//    Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
+	Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
+	Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
 
+	Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
+	Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
+	Vector3 vel = vel1 - vel2;
 
+	real_t rel_vel = axis_normal_on_a.dot(vel);
 
-/// apply displacement correction
+	/// apply displacement correction
 
-//positional error (zeroth order error)
-    real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-    real_t	lo = real_t(-1e30);
-    real_t	hi = real_t(1e30);
+	//positional error (zeroth order error)
+	real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+	real_t lo = real_t(-1e30);
+	real_t hi = real_t(1e30);
 
-    real_t minLimit = m_lowerLimit[limit_index];
-    real_t maxLimit = m_upperLimit[limit_index];
+	real_t minLimit = m_lowerLimit[limit_index];
+	real_t maxLimit = m_upperLimit[limit_index];
 
-    //handle the limits
-    if (minLimit < maxLimit)
-    {
-	{
-	    if (depth > maxLimit)
-	    {
-		depth -= maxLimit;
-		lo = real_t(0.);
-
-	    }
-	    else
-	    {
-		if (depth < minLimit)
-		{
-		    depth -= minLimit;
-		    hi = real_t(0.);
-		}
-		else
+	//handle the limits
+	if (minLimit < maxLimit) {
 		{
-		    return 0.0f;
+			if (depth > maxLimit) {
+				depth -= maxLimit;
+				lo = real_t(0.);
+
+			} else {
+				if (depth < minLimit) {
+					depth -= minLimit;
+					hi = real_t(0.);
+				} else {
+					return 0.0f;
+				}
+			}
 		}
-	    }
 	}
-    }
 
-    real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
+	real_t normalImpulse = m_limitSoftness[limit_index] * (m_restitution[limit_index] * depth / timeStep - m_damping[limit_index] * rel_vel) * jacDiagABInv;
 
+	real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
+	real_t sum = oldNormalImpulse + normalImpulse;
+	m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
+	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 
-
-
-    real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
-    real_t sum = oldNormalImpulse + normalImpulse;
-    m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
-    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-    Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
-    body1->apply_impulse( rel_pos1, impulse_vector);
-    body2->apply_impulse( rel_pos2, -impulse_vector);
-    return normalImpulse;
+	Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
+	body1->apply_impulse(rel_pos1, impulse_vector);
+	body2->apply_impulse(rel_pos2, -impulse_vector);
+	return normalImpulse;
 }
 
 //////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
 
-
-Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
-	 :  JointSW(_arr,2)
-	, m_frameInA(frameInA)
-	, m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
-	A=rbA;
-	B=rbB;
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+Generic6DOFJointSW::Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA)
+	: JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
+	A = rbA;
+	B = rbB;
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
+void Generic6DOFJointSW::calculateAngleInfo() {
+	Matrix3 relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis;
 
-
-
-
-void Generic6DOFJointSW::calculateAngleInfo()
-{
-	Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
-
-	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 
 	// in euler angle mode we do not actually constrain the angular velocity
-  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
-  //
-  //    to get			constrain w2-w1 along		...not
-  //    ------			---------------------		------
-  //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
-  //    d(angle[1])/dt = 0	ax[1]
-  //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
-  //
-  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
-  // to prove the result for angle[0], write the expression for angle[0] from
-  // GetInfo1 then take the derivative. to prove this for angle[2] it is
-  // easier to take the euler rate expression for d(angle[2])/dt with respect
-  // to the components of w and set that to 0.
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
 
 	Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
 	Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
@@ -324,291 +261,260 @@ void Generic6DOFJointSW::calculateAngleInfo()
 	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
-//    if(m_debugDrawer)
-//    {
-//
-//    	char buff[300];
-//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
-//		m_calculatedAxisAngleDiff[0],
-//		m_calculatedAxisAngleDiff[1],
-//		m_calculatedAxisAngleDiff[2]);
-//    	m_debugDrawer->reportErrorWarning(buff);
-//    }
-
+	//    if(m_debugDrawer)
+	//    {
+	//
+	//    	char buff[300];
+	//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+	//		m_calculatedAxisAngleDiff[0],
+	//		m_calculatedAxisAngleDiff[1],
+	//		m_calculatedAxisAngleDiff[2]);
+	//    	m_debugDrawer->reportErrorWarning(buff);
+	//    }
 }
 
-void Generic6DOFJointSW::calculateTransforms()
-{
-    m_calculatedTransformA = A->get_transform() * m_frameInA;
-    m_calculatedTransformB = B->get_transform() * m_frameInB;
+void Generic6DOFJointSW::calculateTransforms() {
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
 
-    calculateAngleInfo();
+	calculateAngleInfo();
 }
 
-
 void Generic6DOFJointSW::buildLinearJacobian(
-    JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
-    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,
-	normalWorld,
-	A->get_inv_inertia(),
-	A->get_inv_mass(),
-	B->get_inv_inertia(),
-	B->get_inv_mass()));
-
+		JacobianEntrySW &jacLinear, const Vector3 &normalWorld,
+		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,
+										 normalWorld,
+										 A->get_inv_inertia(),
+										 A->get_inv_mass(),
+										 B->get_inv_inertia(),
+										 B->get_inv_mass()));
 }
 
 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_inv_inertia(),
-				      B->get_inv_inertia()));
-
+		JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) {
+	memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
+										  A->get_transform().basis.transposed(),
+										  B->get_transform().basis.transposed(),
+										  A->get_inv_inertia(),
+										  B->get_inv_inertia()));
 }
 
-bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
-{
-    real_t angle = m_calculatedAxisAngleDiff[axis_index];
+bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index) {
+	real_t angle = m_calculatedAxisAngleDiff[axis_index];
 
-    //test limits
-    m_angularLimits[axis_index].testLimitValue(angle);
-    return m_angularLimits[axis_index].needApplyTorques();
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
 }
 
 bool Generic6DOFJointSW::setup(float p_step) {
 
 	// Clear accumulated impulses for the next simulation step
-    m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
-    int i;
-    for(i = 0; i < 3; i++)
-    {
-	m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
-    }
-    //calculates transform
-    calculateTransforms();
+	m_linearLimits.m_accumulatedImpulse = Vector3(real_t(0.), real_t(0.), real_t(0.));
+	int i;
+	for (i = 0; i < 3; i++) {
+		m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
+	}
+	//calculates transform
+	calculateTransforms();
 
-//  const Vector3& pivotAInW = m_calculatedTransformA.origin;
-//  const Vector3& pivotBInW = m_calculatedTransformB.origin;
+	//  const Vector3& pivotAInW = m_calculatedTransformA.origin;
+	//  const Vector3& pivotBInW = m_calculatedTransformB.origin;
 	calcAnchorPos();
 	Vector3 pivotAInW = m_AnchorPos;
 	Vector3 pivotBInW = m_AnchorPos;
 
-// not used here
-//    Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
-//    Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
+	// not used here
+	//    Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
+	//    Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
 
-    Vector3 normalWorld;
-    //linear part
-    for (i=0;i<3;i++)
-    {
-	if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
-	{
+	Vector3 normalWorld;
+	//linear part
+	for (i = 0; i < 3; i++) {
+		if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
 			if (m_useLinearReferenceFrameA)
-		    normalWorld = m_calculatedTransformA.basis.get_axis(i);
+				normalWorld = m_calculatedTransformA.basis.get_axis(i);
 			else
-		    normalWorld = m_calculatedTransformB.basis.get_axis(i);
-
-	    buildLinearJacobian(
-		m_jacLinear[i],normalWorld ,
-		pivotAInW,pivotBInW);
+				normalWorld = m_calculatedTransformB.basis.get_axis(i);
 
+			buildLinearJacobian(
+					m_jacLinear[i], normalWorld,
+					pivotAInW, pivotBInW);
+		}
 	}
-    }
 
-    // angular part
-    for (i=0;i<3;i++)
-    {
-	//calculates error angle
-	if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
-	{
-	    normalWorld = this->getAxis(i);
-	    // Create angular atom
-	    buildAngularJacobian(m_jacAng[i],normalWorld);
+	// angular part
+	for (i = 0; i < 3; i++) {
+		//calculates error angle
+		if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) {
+			normalWorld = this->getAxis(i);
+			// Create angular atom
+			buildAngularJacobian(m_jacAng[i], normalWorld);
+		}
 	}
-    }
 
 	return true;
 }
 
+void Generic6DOFJointSW::solve(real_t timeStep) {
+	m_timeStep = timeStep;
 
-void Generic6DOFJointSW::solve(real_t	timeStep)
-{
-    m_timeStep = timeStep;
-
-    //calculateTransforms();
+	//calculateTransforms();
 
-    int i;
+	int i;
 
-    // linear
+	// linear
 
-    Vector3 pointInA = m_calculatedTransformA.origin;
+	Vector3 pointInA = m_calculatedTransformA.origin;
 	Vector3 pointInB = m_calculatedTransformB.origin;
 
 	real_t jacDiagABInv;
 	Vector3 linear_axis;
-    for (i=0;i<3;i++)
-    {
-	if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
-	{
-	    jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
+	for (i = 0; i < 3; i++) {
+		if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) {
+			jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
 
 			if (m_useLinearReferenceFrameA)
-		    linear_axis = m_calculatedTransformA.basis.get_axis(i);
+				linear_axis = m_calculatedTransformA.basis.get_axis(i);
 			else
-		    linear_axis = m_calculatedTransformB.basis.get_axis(i);
-
-	    m_linearLimits.solveLinearAxis(
-		m_timeStep,
-		jacDiagABInv,
-		A,pointInA,
-		B,pointInB,
-		i,linear_axis, m_AnchorPos);
+				linear_axis = m_calculatedTransformB.basis.get_axis(i);
 
+			m_linearLimits.solveLinearAxis(
+					m_timeStep,
+					jacDiagABInv,
+					A, pointInA,
+					B, pointInB,
+					i, linear_axis, m_AnchorPos);
+		}
 	}
-    }
 
-    // angular
-    Vector3 angular_axis;
-    real_t angularJacDiagABInv;
-    for (i=0;i<3;i++)
-    {
-	if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
-	{
+	// angular
+	Vector3 angular_axis;
+	real_t angularJacDiagABInv;
+	for (i = 0; i < 3; i++) {
+		if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) {
 
 			// get axis
 			angular_axis = getAxis(i);
 
 			angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
 
-			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
+			m_angularLimits[i].solveAngularLimits(m_timeStep, angular_axis, angularJacDiagABInv, A, B);
+		}
 	}
-    }
 }
 
-void	Generic6DOFJointSW::updateRHS(real_t	timeStep)
-{
-    (void)timeStep;
-
+void Generic6DOFJointSW::updateRHS(real_t timeStep) {
+	(void)timeStep;
 }
 
-Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
-{
-    return m_calculatedAxis[axis_index];
+Vector3 Generic6DOFJointSW::getAxis(int axis_index) const {
+	return m_calculatedAxis[axis_index];
 }
 
-real_t Generic6DOFJointSW::getAngle(int axis_index) const
-{
-    return m_calculatedAxisAngleDiff[axis_index];
+real_t Generic6DOFJointSW::getAngle(int axis_index) const {
+	return m_calculatedAxisAngleDiff[axis_index];
 }
 
-void Generic6DOFJointSW::calcAnchorPos(void)
-{
+void Generic6DOFJointSW::calcAnchorPos(void) {
 	real_t imA = A->get_inv_mass();
 	real_t imB = B->get_inv_mass();
 	real_t weight;
-	if(imB == real_t(0.0))
-	{
+	if (imB == real_t(0.0)) {
 		weight = real_t(1.0);
-	}
-	else
-	{
+	} else {
 		weight = imA / (imA + imB);
 	}
-	const Vector3& pA = m_calculatedTransformA.origin;
-	const Vector3& pB = m_calculatedTransformB.origin;
+	const Vector3 &pA = m_calculatedTransformA.origin;
+	const Vector3 &pB = m_calculatedTransformB.origin;
 	m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
 	return;
 } // Generic6DOFJointSW::calcAnchorPos()
 
+void Generic6DOFJointSW::set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
 
-void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
-
-	ERR_FAIL_INDEX(p_axis,3);
-	switch(p_param) {
+	ERR_FAIL_INDEX(p_axis, 3);
+	switch (p_param) {
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
 
-			m_linearLimits.m_lowerLimit[p_axis]=p_value;
+			m_linearLimits.m_lowerLimit[p_axis] = p_value;
 		} break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
 
-			m_linearLimits.m_upperLimit[p_axis]=p_value;
+			m_linearLimits.m_upperLimit[p_axis] = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
 
-			m_linearLimits.m_limitSoftness[p_axis]=p_value;
+			m_linearLimits.m_limitSoftness[p_axis] = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
 
-			m_linearLimits.m_restitution[p_axis]=p_value;
+			m_linearLimits.m_restitution[p_axis] = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
 
-			m_linearLimits.m_damping[p_axis]=p_value;
+			m_linearLimits.m_damping[p_axis] = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
 
-			m_angularLimits[p_axis].m_loLimit=p_value;
+			m_angularLimits[p_axis].m_loLimit = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
 
-			m_angularLimits[p_axis].m_hiLimit=p_value;
+			m_angularLimits[p_axis].m_hiLimit = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
 
-			m_angularLimits[p_axis].m_limitSoftness=p_value;
+			m_angularLimits[p_axis].m_limitSoftness = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
 
-			m_angularLimits[p_axis].m_damping=p_value;
+			m_angularLimits[p_axis].m_damping = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
 
-			m_angularLimits[p_axis].m_bounce=p_value;
+			m_angularLimits[p_axis].m_bounce = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
 
-			m_angularLimits[p_axis].m_maxLimitForce=p_value;
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
 
-			m_angularLimits[p_axis].m_ERP=p_value;
+			m_angularLimits[p_axis].m_ERP = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
 
-			m_angularLimits[p_axis].m_targetVelocity=p_value;
+			m_angularLimits[p_axis].m_targetVelocity = p_value;
 
 		} break;
 		case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
 
-			m_angularLimits[p_axis].m_maxLimitForce=p_value;
+			m_angularLimits[p_axis].m_maxLimitForce = p_value;
 
 		} break;
 	}
 }
 
-float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
-	ERR_FAIL_INDEX_V(p_axis,3,0);
-	switch(p_param) {
+float Generic6DOFJointSW::get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const {
+	ERR_FAIL_INDEX_V(p_axis, 3, 0);
+	switch (p_param) {
 		case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
 
 			return m_linearLimits.m_lowerLimit[p_axis];
@@ -682,31 +588,29 @@ float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJoi
 	return 0;
 }
 
-void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
+void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value) {
 
-	ERR_FAIL_INDEX(p_axis,3);
+	ERR_FAIL_INDEX(p_axis, 3);
 
-	switch(p_flag) {
+	switch (p_flag) {
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
 
-			m_linearLimits.enable_limit[p_axis]=p_value;
+			m_linearLimits.enable_limit[p_axis] = p_value;
 		} break;
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
 
-			m_angularLimits[p_axis].m_enableLimit=p_value;
+			m_angularLimits[p_axis].m_enableLimit = p_value;
 		} break;
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
 
-			m_angularLimits[p_axis].m_enableMotor=p_value;
+			m_angularLimits[p_axis].m_enableMotor = p_value;
 		} break;
 	}
-
-
 }
-bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
+bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const {
 
-	ERR_FAIL_INDEX_V(p_axis,3,0);
-	switch(p_flag) {
+	ERR_FAIL_INDEX_V(p_axis, 3, 0);
+	switch (p_flag) {
 		case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
 
 			return m_linearLimits.enable_limit[p_axis];
diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h
index 4ac727c12..c226c5990 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.h
+++ b/servers/physics/joints/generic_6dof_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
 #ifndef GENERIC_6DOF_JOINT_SW_H
 #define GENERIC_6DOF_JOINT_SW_H
 
-#include "servers/physics/joints_sw.h"
 #include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
@@ -53,7 +52,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 /*
 2007-09-09
 Generic6DOFJointSW Refactored by Francisco Le?n
@@ -61,80 +59,73 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 //! Rotation Limit structure for generic joints
 class G6DOFRotationalLimitMotorSW {
 public:
-    //! limit_parameters
-    //!@{
-    real_t m_loLimit;//!< joint limit
-    real_t m_hiLimit;//!< joint limit
-    real_t m_targetVelocity;//!< target motor velocity
-    real_t m_maxMotorForce;//!< max force on motor
-    real_t m_maxLimitForce;//!< max force on limit
-    real_t m_damping;//!< Damping.
-    real_t m_limitSoftness;//! Relaxation factor
-    real_t m_ERP;//!< Error tolerance factor when joint is at limit
-    real_t m_bounce;//!< restitution factor
-    bool m_enableMotor;
-    bool m_enableLimit;
+	//! limit_parameters
+	//!@{
+	real_t m_loLimit; //!< joint limit
+	real_t m_hiLimit; //!< joint limit
+	real_t m_targetVelocity; //!< target motor velocity
+	real_t m_maxMotorForce; //!< max force on motor
+	real_t m_maxLimitForce; //!< max force on limit
+	real_t m_damping; //!< Damping.
+	real_t m_limitSoftness; //! Relaxation factor
+	real_t m_ERP; //!< Error tolerance factor when joint is at limit
+	real_t m_bounce; //!< restitution factor
+	bool m_enableMotor;
+	bool m_enableLimit;
 
-    //!@}
-
-    //! temp_variables
-    //!@{
-    real_t m_currentLimitError;//!  How much is violated this limit
-    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
-    real_t m_accumulatedImpulse;
-    //!@}
-
-    G6DOFRotationalLimitMotorSW()
-    {
-	m_accumulatedImpulse = 0.f;
-	m_targetVelocity = 0;
-	m_maxMotorForce = 0.1f;
-	m_maxLimitForce = 300.0f;
-	m_loLimit = -1e30;
-	m_hiLimit = 1e30;
-	m_ERP = 0.5f;
-	m_bounce = 0.0f;
-	m_damping = 1.0f;
-	m_limitSoftness = 0.5f;
-	m_currentLimit = 0;
-	m_currentLimitError = 0;
-	m_enableMotor = false;
-	m_enableLimit=false;
-    }
+	//!@}
 
-    G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW & limot)
-    {
-	m_targetVelocity = limot.m_targetVelocity;
-	m_maxMotorForce = limot.m_maxMotorForce;
-	m_limitSoftness = limot.m_limitSoftness;
-	m_loLimit = limot.m_loLimit;
-	m_hiLimit = limot.m_hiLimit;
-	m_ERP = limot.m_ERP;
-	m_bounce = limot.m_bounce;
-	m_currentLimit = limot.m_currentLimit;
-	m_currentLimitError = limot.m_currentLimitError;
-	m_enableMotor = limot.m_enableMotor;
-    }
+	//! temp_variables
+	//!@{
+	real_t m_currentLimitError; //!  How much is violated this limit
+	int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
+	real_t m_accumulatedImpulse;
+	//!@}
 
+	G6DOFRotationalLimitMotorSW() {
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 0.1f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = -1e30;
+		m_hiLimit = 1e30;
+		m_ERP = 0.5f;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+		m_enableLimit = false;
+	}
 
+	G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW &limot) {
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
+		m_ERP = limot.m_ERP;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
 
 	//! Is limited
-    bool isLimited()
-    {
-	if(m_loLimit>=m_hiLimit) return false;
-	return true;
-    }
+	bool isLimited() {
+		if (m_loLimit >= m_hiLimit) return false;
+		return true;
+	}
 
 	//! Need apply correction
-    bool needApplyTorques()
-    {
-	if(m_currentLimit == 0 && m_enableMotor == false) return false;
-	return true;
-    }
+	bool needApplyTorques() {
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
 
 	//! calculates  error
 	/*!
@@ -143,84 +134,69 @@ public:
 	int testLimitValue(real_t test_value);
 
 	//! apply the correction impulses for two bodies
-    real_t solveAngularLimits(real_t timeStep,Vector3& axis, real_t jacDiagABInv,BodySW * body0, BodySW * body1);
-
-
+	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, BodySW *body0, BodySW *body1);
 };
 
-
-
-class G6DOFTranslationalLimitMotorSW
-{
+class G6DOFTranslationalLimitMotorSW {
 public:
-	Vector3 m_lowerLimit;//!< the constraint lower limits
-    Vector3 m_upperLimit;//!< the constraint upper limits
-    Vector3 m_accumulatedImpulse;
-    //! Linear_Limit_parameters
-    //!@{
-    Vector3	m_limitSoftness;//!< Softness for linear limit
-    Vector3	m_damping;//!< Damping for linear limit
-    Vector3	m_restitution;//! Bounce parameter for linear limit
-    //!@}
-    bool enable_limit[3];
+	Vector3 m_lowerLimit; //!< the constraint lower limits
+	Vector3 m_upperLimit; //!< the constraint upper limits
+	Vector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	Vector3 m_limitSoftness; //!< Softness for linear limit
+	Vector3 m_damping; //!< Damping for linear limit
+	Vector3 m_restitution; //! Bounce parameter for linear limit
+	//!@}
+	bool enable_limit[3];
 
-    G6DOFTranslationalLimitMotorSW()
-    {
-	m_lowerLimit=Vector3(0.f,0.f,0.f);
-	m_upperLimit=Vector3(0.f,0.f,0.f);
-	m_accumulatedImpulse=Vector3(0.f,0.f,0.f);
+	G6DOFTranslationalLimitMotorSW() {
+		m_lowerLimit = Vector3(0.f, 0.f, 0.f);
+		m_upperLimit = Vector3(0.f, 0.f, 0.f);
+		m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f);
 
-	m_limitSoftness = Vector3(1,1,1)*0.7f;
-	m_damping = Vector3(1,1,1)*real_t(1.0f);
-	m_restitution = Vector3(1,1,1)*real_t(0.5f);
+		m_limitSoftness = Vector3(1, 1, 1) * 0.7f;
+		m_damping = Vector3(1, 1, 1) * real_t(1.0f);
+		m_restitution = Vector3(1, 1, 1) * real_t(0.5f);
 
-	enable_limit[0]=true;
-	enable_limit[1]=true;
-	enable_limit[2]=true;
-    }
+		enable_limit[0] = true;
+		enable_limit[1] = true;
+		enable_limit[2] = true;
+	}
 
-    G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW & other )
-    {
-	m_lowerLimit = other.m_lowerLimit;
-	m_upperLimit = other.m_upperLimit;
-	m_accumulatedImpulse = other.m_accumulatedImpulse;
+	G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW &other) {
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
 
-	m_limitSoftness = other.m_limitSoftness ;
-	m_damping = other.m_damping;
-	m_restitution = other.m_restitution;
-    }
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
+	}
 
-    //! Test limit
+	//! Test limit
 	/*!
     - free means upper < lower,
     - locked means upper == lower
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex)
-    {
-       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-    }
-
-
-    real_t solveLinearAxis(
-	real_t timeStep,
-	real_t jacDiagABInv,
-	BodySW* body1,const Vector3 &pointInA,
-	BodySW* body2,const Vector3 &pointInB,
-	int limit_index,
-	const Vector3 & axis_normal_on_a,
-		const Vector3 & anchorPos);
-
+	inline bool isLimited(int limitIndex) {
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
 
+	real_t solveLinearAxis(
+			real_t timeStep,
+			real_t jacDiagABInv,
+			BodySW *body1, const Vector3 &pointInA,
+			BodySW *body2, const Vector3 &pointInB,
+			int limit_index,
+			const Vector3 &axis_normal_on_a,
+			const Vector3 &anchorPos);
 };
 
-
-class Generic6DOFJointSW : public JointSW
-{
+class Generic6DOFJointSW : public JointSW {
 protected:
-
-
 	union {
 		struct {
 			BodySW *A;
@@ -231,195 +207,167 @@ protected:
 	};
 
 	//! relative_frames
-    //!@{
-	Transform	m_frameInA;//!< the constraint space w.r.t body A
-    Transform	m_frameInB;//!< the constraint space w.r.t body B
-    //!@}
+	//!@{
+	Transform m_frameInA; //!< the constraint space w.r.t body A
+	Transform m_frameInB; //!< the constraint space w.r.t body B
+	//!@}
 
-    //! Jacobians
-    //!@{
-    JacobianEntrySW	m_jacLinear[3];//!< 3 orthogonal linear constraints
-    JacobianEntrySW	m_jacAng[3];//!< 3 orthogonal angular constraints
-    //!@}
+	//! Jacobians
+	//!@{
+	JacobianEntrySW m_jacLinear[3]; //!< 3 orthogonal linear constraints
+	JacobianEntrySW m_jacAng[3]; //!< 3 orthogonal angular constraints
+	//!@}
 
 	//! Linear_Limit_parameters
-    //!@{
-    G6DOFTranslationalLimitMotorSW m_linearLimits;
-    //!@}
-
-
-    //! hinge_parameters
-    //!@{
-    G6DOFRotationalLimitMotorSW m_angularLimits[3];
+	//!@{
+	G6DOFTranslationalLimitMotorSW m_linearLimits;
 	//!@}
 
+	//! hinge_parameters
+	//!@{
+	G6DOFRotationalLimitMotorSW m_angularLimits[3];
+	//!@}
 
 protected:
-    //! temporal variables
-    //!@{
-    real_t m_timeStep;
-    Transform m_calculatedTransformA;
-    Transform m_calculatedTransformB;
-    Vector3 m_calculatedAxisAngleDiff;
-    Vector3 m_calculatedAxis[3];
+	//! temporal variables
+	//!@{
+	real_t m_timeStep;
+	Transform m_calculatedTransformA;
+	Transform m_calculatedTransformB;
+	Vector3 m_calculatedAxisAngleDiff;
+	Vector3 m_calculatedAxis[3];
 
 	Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
 
-    bool	m_useLinearReferenceFrameA;
-
-    //!@}
-
-    Generic6DOFJointSW&	operator=(Generic6DOFJointSW&	other)
-    {
-	ERR_PRINT("pito");
-	(void) other;
-	return *this;
-    }
-
+	bool m_useLinearReferenceFrameA;
 
+	//!@}
 
-    void buildLinearJacobian(
-	JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
-	const Vector3 & pivotAInW,const Vector3 & pivotBInW);
+	Generic6DOFJointSW &operator=(Generic6DOFJointSW &other) {
+		ERR_PRINT("pito");
+		(void)other;
+		return *this;
+	}
 
-    void buildAngularJacobian(JacobianEntrySW & jacAngular,const Vector3 & jointAxisW);
+	void buildLinearJacobian(
+			JacobianEntrySW &jacLinear, const Vector3 &normalWorld,
+			const Vector3 &pivotAInW, const Vector3 &pivotBInW);
 
+	void buildAngularJacobian(JacobianEntrySW &jacAngular, const Vector3 &jointAxisW);
 
 	//! calcs the euler angles between the two bodies.
-    void calculateAngleInfo();
-
-
+	void calculateAngleInfo();
 
 public:
-    Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA);
-
-    virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
+	Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
 
-    virtual bool setup(float p_step);
-    virtual void solve(float p_step);
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
 
+	virtual bool setup(float p_step);
+	virtual void solve(float p_step);
 
 	//! Calcs global transform of the offsets
 	/*!
 	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 	\sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo
 	*/
-    void calculateTransforms();
+	void calculateTransforms();
 
 	//! Gets the global transform of the offset for body A
-    /*!
+	/*!
     \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
     */
-    const Transform & getCalculatedTransformA() const
-    {
-	return m_calculatedTransformA;
-    }
+	const Transform &getCalculatedTransformA() const {
+		return m_calculatedTransformA;
+	}
 
-    //! Gets the global transform of the offset for body B
-    /*!
+	//! Gets the global transform of the offset for body B
+	/*!
     \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
     */
-    const Transform & getCalculatedTransformB() const
-    {
-	return m_calculatedTransformB;
-    }
-
-    const Transform & getFrameOffsetA() const
-    {
-	return m_frameInA;
-    }
-
-    const Transform & getFrameOffsetB() const
-    {
-	return m_frameInB;
-    }
+	const Transform &getCalculatedTransformB() const {
+		return m_calculatedTransformB;
+	}
 
+	const Transform &getFrameOffsetA() const {
+		return m_frameInA;
+	}
 
-    Transform & getFrameOffsetA()
-    {
-	return m_frameInA;
-    }
+	const Transform &getFrameOffsetB() const {
+		return m_frameInB;
+	}
 
-    Transform & getFrameOffsetB()
-    {
-	return m_frameInB;
-    }
+	Transform &getFrameOffsetA() {
+		return m_frameInA;
+	}
 
+	Transform &getFrameOffsetB() {
+		return m_frameInB;
+	}
 
 	//! performs Jacobian calculation, and also calculates angle differences and axis
 
-
-    void	updateRHS(real_t	timeStep);
+	void updateRHS(real_t timeStep);
 
 	//! Get the rotation axis in global coordinates
 	/*!
 	\pre Generic6DOFJointSW.buildJacobian must be called previously.
 	*/
-    Vector3 getAxis(int axis_index) const;
+	Vector3 getAxis(int axis_index) const;
 
-    //! Get the relative Euler angle
-    /*!
+	//! Get the relative Euler angle
+	/*!
 	\pre Generic6DOFJointSW.buildJacobian must be called previously.
 	*/
-    real_t getAngle(int axis_index) const;
+	real_t getAngle(int axis_index) const;
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
 	\pre Generic6DOFJointSW.buildJacobian must be called previously.
 	*/
-    bool testAngularLimitMotor(int axis_index);
+	bool testAngularLimitMotor(int axis_index);
 
-    void	setLinearLowerLimit(const Vector3& linearLower)
-    {
-	m_linearLimits.m_lowerLimit = linearLower;
-    }
+	void setLinearLowerLimit(const Vector3 &linearLower) {
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
 
-    void	setLinearUpperLimit(const Vector3& linearUpper)
-    {
-	m_linearLimits.m_upperLimit = linearUpper;
-    }
+	void setLinearUpperLimit(const Vector3 &linearUpper) {
+		m_linearLimits.m_upperLimit = linearUpper;
+	}
 
-    void	setAngularLowerLimit(const Vector3& angularLower)
-    {
-	m_angularLimits[0].m_loLimit = angularLower.x;
-	m_angularLimits[1].m_loLimit = angularLower.y;
-	m_angularLimits[2].m_loLimit = angularLower.z;
-    }
+	void setAngularLowerLimit(const Vector3 &angularLower) {
+		m_angularLimits[0].m_loLimit = angularLower.x;
+		m_angularLimits[1].m_loLimit = angularLower.y;
+		m_angularLimits[2].m_loLimit = angularLower.z;
+	}
 
-    void	setAngularUpperLimit(const Vector3& angularUpper)
-    {
-	m_angularLimits[0].m_hiLimit = angularUpper.x;
-	m_angularLimits[1].m_hiLimit = angularUpper.y;
-	m_angularLimits[2].m_hiLimit = angularUpper.z;
-    }
+	void setAngularUpperLimit(const Vector3 &angularUpper) {
+		m_angularLimits[0].m_hiLimit = angularUpper.x;
+		m_angularLimits[1].m_hiLimit = angularUpper.y;
+		m_angularLimits[2].m_hiLimit = angularUpper.z;
+	}
 
 	//! Retrieves the angular limit informacion
-    G6DOFRotationalLimitMotorSW * getRotationalLimitMotor(int index)
-    {
-	return &m_angularLimits[index];
-    }
-
-    //! Retrieves the  limit informacion
-    G6DOFTranslationalLimitMotorSW * getTranslationalLimitMotor()
-    {
-	return &m_linearLimits;
-    }
+	G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) {
+		return &m_angularLimits[index];
+	}
 
-    //first 3 are linear, next 3 are angular
-    void setLimit(int axis, real_t lo, real_t hi)
-    {
-	if(axis<3)
-	{
-		m_linearLimits.m_lowerLimit[axis] = lo;
-		m_linearLimits.m_upperLimit[axis] = hi;
+	//! Retrieves the  limit informacion
+	G6DOFTranslationalLimitMotorSW *getTranslationalLimitMotor() {
+		return &m_linearLimits;
 	}
-	else
-	{
-		m_angularLimits[axis-3].m_loLimit = lo;
-		m_angularLimits[axis-3].m_hiLimit = hi;
+
+	//first 3 are linear, next 3 are angular
+	void setLimit(int axis, real_t lo, real_t hi) {
+		if (axis < 3) {
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		} else {
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
 	}
-    }
 
 	//! Test limit
 	/*!
@@ -428,34 +376,27 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex)
-    {
-	if(limitIndex<3)
-	{
+	bool isLimited(int limitIndex) {
+		if (limitIndex < 3) {
 			return m_linearLimits.isLimited(limitIndex);
-
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
 	}
-	return m_angularLimits[limitIndex-3].isLimited();
-    }
 
-    const BodySW* getRigidBodyA() const
-    {
-	return A;
-    }
-    const BodySW* getRigidBodyB() const
-    {
-	return B;
-    }
+	const BodySW *getRigidBodyA() const {
+		return A;
+	}
+	const BodySW *getRigidBodyB() const {
+		return B;
+	}
 
 	virtual void calcAnchorPos(void); // overridable
 
-    void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value);
-    float get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const;
-
-    void set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
-    bool get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const;
+	void set_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param, float p_value);
+	float get_param(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisParam p_param) const;
 
+	void set_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
+	bool get_flag(Vector3::Axis p_axis, PhysicsServer::G6DOFJointAxisFlag p_flag) const;
 };
 
-
 #endif // GENERIC_6DOF_JOINT_SW_H
diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp
index 6f552db3a..190f8fe33 100644
--- a/servers/physics/joints/hinge_joint_sw.cpp
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -34,65 +34,63 @@ See corresponding header file for licensing info.
 
 #include "hinge_joint_sw.h"
 
-static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
+static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
 
-  if (Math::abs(n.z) > 0.707106781186547524400844362) {
-    // choose p in y-z plane
-    real_t a = n[1]*n[1] + n[2]*n[2];
-    real_t k = 1.0/Math::sqrt(a);
-    p=Vector3(0,-n[2]*k,n[1]*k);
-    // set q = n x p
-    q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
-  }
-  else {
-    // choose p in x-y plane
-    real_t a = n.x*n.x + n.y*n.y;
-    real_t k = 1.0/Math::sqrt(a);
-    p=Vector3(-n.y*k,n.x*k,0);
-    // set q = n x p
-    q=Vector3(-n.z*p.y,n.z*p.x,a*k);
-  }
+	if (Math::abs(n.z) > 0.707106781186547524400844362) {
+		// choose p in y-z plane
+		real_t a = n[1] * n[1] + n[2] * n[2];
+		real_t k = 1.0 / Math::sqrt(a);
+		p = Vector3(0, -n[2] * k, n[1] * k);
+		// set q = n x p
+		q = Vector3(a * k, -n[0] * p[2], n[0] * p[1]);
+	} else {
+		// choose p in x-y plane
+		real_t a = n.x * n.x + n.y * n.y;
+		real_t k = 1.0 / Math::sqrt(a);
+		p = Vector3(-n.y * k, n.x * k, 0);
+		// set q = n x p
+		q = Vector3(-n.z * p.y, n.z * p.x, a * k);
+	}
 }
 
-HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) :  JointSW(_arr,2) {
+HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB)
+	: JointSW(_arr, 2) {
 
-	A=rbA;
-	B=rbB;
+	A = rbA;
+	B = rbB;
 
-	m_rbAFrame=frameA;
-	m_rbBFrame=frameB;
+	m_rbAFrame = frameA;
+	m_rbBFrame = frameB;
 	// flip axis
 	m_rbBFrame.basis[0][2] *= real_t(-1.);
 	m_rbBFrame.basis[1][2] *= real_t(-1.);
 	m_rbBFrame.basis[2][2] *= real_t(-1.);
 
-
 	//start with free
 	m_lowerLimit = Math_PI;
 	m_upperLimit = -Math_PI;
 
-
 	m_useLimit = false;
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
 
-	tau=0.3;
-
-	m_angularOnly=false;
-	m_enableAngularMotor=false;
+	tau = 0.3;
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	m_angularOnly = false;
+	m_enableAngularMotor = false;
 
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
-HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB,
-									const  Vector3& axisInA,const Vector3& axisInB) :  JointSW(_arr,2) {
+HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+		const Vector3 &axisInA, const Vector3 &axisInB)
+	: JointSW(_arr, 2) {
 
-	A=rbA;
-	B=rbB;
+	A = rbA;
+	B = rbB;
 
 	m_rbAFrame.origin = pivotInA;
 
@@ -112,76 +110,67 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons
 		rbAxisA1 = rbAxisA2.cross(axisInA);
 	}
 
-	m_rbAFrame.basis=Matrix3( rbAxisA1.x,rbAxisA2.x,axisInA.x,
-									rbAxisA1.y,rbAxisA2.y,axisInA.y,
-									rbAxisA1.z,rbAxisA2.z,axisInA.z );
+	m_rbAFrame.basis = Matrix3(rbAxisA1.x, rbAxisA2.x, axisInA.x,
+			rbAxisA1.y, rbAxisA2.y, axisInA.y,
+			rbAxisA1.z, rbAxisA2.z, axisInA.z);
 
-	Quat rotationArc = Quat(axisInA,axisInB);
-	Vector3 rbAxisB1 =  rotationArc.xform(rbAxisA1);
-	Vector3 rbAxisB2 =  axisInB.cross(rbAxisB1);
+	Quat rotationArc = Quat(axisInA, axisInB);
+	Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
+	Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
 	m_rbBFrame.origin = pivotInB;
-	m_rbBFrame.basis=Matrix3( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
-									rbAxisB1.y,rbAxisB2.y,-axisInB.y,
-									rbAxisB1.z,rbAxisB2.z,-axisInB.z );
+	m_rbBFrame.basis = Matrix3(rbAxisB1.x, rbAxisB2.x, -axisInB.x,
+			rbAxisB1.y, rbAxisB2.y, -axisInB.y,
+			rbAxisB1.z, rbAxisB2.z, -axisInB.z);
 
 	//start with free
 	m_lowerLimit = Math_PI;
 	m_upperLimit = -Math_PI;
 
-
 	m_useLimit = false;
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
 
-	tau=0.3;
-
-	m_angularOnly=false;
-	m_enableAngularMotor=false;
+	tau = 0.3;
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	m_angularOnly = false;
+	m_enableAngularMotor = false;
 
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
-
-
 bool HingeJointSW::setup(float p_step) {
 
 	m_appliedImpulse = real_t(0.);
 
-	if (!m_angularOnly)
-	{
+	if (!m_angularOnly) {
 		Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
 		Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
 		Vector3 relPos = pivotBInW - pivotAInW;
 
 		Vector3 normal[3];
-		if (relPos.length_squared() > CMP_EPSILON)
-		{
+		if (relPos.length_squared() > CMP_EPSILON) {
 			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0]=Vector3(real_t(1.0),0,0);
+		} else {
+			normal[0] = Vector3(real_t(1.0), 0, 0);
 		}
 
 		plane_space(normal[0], normal[1], normal[2]);
 
-		for (int i=0;i<3;i++)
-		{
+		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,
-				normal[i],
-				A->get_inv_inertia(),
-				A->get_inv_mass(),
-				B->get_inv_inertia(),
-				B->get_inv_mass()) );
+												A->get_transform().basis.transposed(),
+												B->get_transform().basis.transposed(),
+												pivotAInW - A->get_transform().origin,
+												pivotBInW - B->get_transform().origin,
+												normal[i],
+												A->get_inv_inertia(),
+												A->get_inv_mass(),
+												B->get_inv_inertia(),
+												B->get_inv_mass()));
 		}
 	}
 
@@ -192,62 +181,56 @@ bool HingeJointSW::setup(float p_step) {
 	Vector3 jointAxis0local;
 	Vector3 jointAxis1local;
 
-	plane_space(m_rbAFrame.basis.get_axis(2),jointAxis0local,jointAxis1local);
-
-	A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
-	Vector3 jointAxis0 = A->get_transform().basis.xform( jointAxis0local );
-	Vector3 jointAxis1 = A->get_transform().basis.xform( jointAxis1local );
-	Vector3 hingeAxisWorld = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
+	plane_space(m_rbAFrame.basis.get_axis(2), jointAxis0local, jointAxis1local);
 
-	memnew_placement(&m_jacAng[0],	JacobianEntrySW(jointAxis0,
-		A->get_transform().basis.transposed(),
-		B->get_transform().basis.transposed(),
-		A->get_inv_inertia(),
-		B->get_inv_inertia()));
+	A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+	Vector3 jointAxis0 = A->get_transform().basis.xform(jointAxis0local);
+	Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
+	Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
 
-	memnew_placement(&m_jacAng[1],	JacobianEntrySW(jointAxis1,
-		A->get_transform().basis.transposed(),
-		B->get_transform().basis.transposed(),
-		A->get_inv_inertia(),
-		B->get_inv_inertia()));
+	memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
+										   A->get_transform().basis.transposed(),
+										   B->get_transform().basis.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_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_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_inv_inertia(),
+										   B->get_inv_inertia()));
 
 	// Compute limit information
 	real_t hingeAngle = get_hinge_angle();
 
-//	print_line("angle: "+rtos(hingeAngle));
+	//	print_line("angle: "+rtos(hingeAngle));
 	//set bias, sign, clear accumulator
 	m_correction = real_t(0.);
 	m_limitSign = real_t(0.);
 	m_solveLimit = false;
 	m_accLimitImpulse = real_t(0.);
 
-
-
 	/*if (m_useLimit) {
 		print_line("low: "+rtos(m_lowerLimit));
 		print_line("hi: "+rtos(m_upperLimit));
 	}*/
 
-//	if (m_lowerLimit < m_upperLimit)
-	if (m_useLimit && m_lowerLimit <= m_upperLimit)
-	{
-//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		if (hingeAngle <= m_lowerLimit)
-		{
+	//	if (m_lowerLimit < m_upperLimit)
+	if (m_useLimit && m_lowerLimit <= m_upperLimit) {
+		//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+		if (hingeAngle <= m_lowerLimit) {
 			m_correction = (m_lowerLimit - hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
 		}
-//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		else if (hingeAngle >= m_upperLimit)
-		{
+		//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+		else if (hingeAngle >= m_upperLimit) {
 			m_correction = m_upperLimit - hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;
@@ -255,9 +238,9 @@ bool HingeJointSW::setup(float p_step) {
 	}
 
 	//Compute K = J*W*J' for hinge axis
-	Vector3 axisA =  A->get_transform().basis.xform(  m_rbAFrame.basis.get_axis(2) );
-	m_kHinge =   1.0f / (A->compute_angular_impulse_denominator(axisA) +
-				     B->compute_angular_impulse_denominator(axisA));
+	Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+	m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
+							  B->compute_angular_impulse_denominator(axisA));
 
 	return true;
 }
@@ -270,8 +253,7 @@ void HingeJointSW::solve(float p_step) {
 	//real_t tau = real_t(0.3);
 
 	//linear part
-	if (!m_angularOnly)
-	{
+	if (!m_angularOnly) {
 		Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
 		Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
 
@@ -279,80 +261,74 @@ void HingeJointSW::solve(float p_step) {
 		Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
 		Vector3 vel = vel1 - vel2;
 
-		for (int i=0;i<3;i++)
-		{
-			const Vector3& normal = m_jac[i].m_linearJointAxis;
+		for (int i = 0; i < 3; i++) {
+			const Vector3 &normal = m_jac[i].m_linearJointAxis;
 			real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
 
 			real_t rel_vel;
 			rel_vel = normal.dot(vel);
 			//positional error (zeroth order error)
 			real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-			real_t impulse = depth*tau/p_step  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			real_t impulse = depth * tau / p_step * jacDiagABInv - rel_vel * jacDiagABInv;
 			m_appliedImpulse += impulse;
 			Vector3 impulse_vector = normal * impulse;
-			A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
-			B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
+			A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
+			B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
 		}
 	}
 
-
 	{
 		///solve angular part
 
 		// get axes in world space
-		Vector3 axisA =  A->get_transform().basis.xform(  m_rbAFrame.basis.get_axis(2) );
-		Vector3 axisB =  B->get_transform().basis.xform(  m_rbBFrame.basis.get_axis(2) );
+		Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
+		Vector3 axisB = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(2));
 
-		const Vector3& angVelA = A->get_angular_velocity();
-		const Vector3& angVelB = B->get_angular_velocity();
+		const Vector3 &angVelA = A->get_angular_velocity();
+		const Vector3 &angVelB = B->get_angular_velocity();
 
 		Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
 		Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
 
 		Vector3 angAorthog = angVelA - angVelAroundHingeAxisA;
 		Vector3 angBorthog = angVelB - angVelAroundHingeAxisB;
-		Vector3 velrelOrthog = angAorthog-angBorthog;
+		Vector3 velrelOrthog = angAorthog - angBorthog;
 		{
 			//solve orthogonal angular velocity correction
 			real_t relaxation = real_t(1.);
 			real_t len = velrelOrthog.length();
-			if (len > real_t(0.00001))
-			{
+			if (len > real_t(0.00001)) {
 				Vector3 normal = velrelOrthog.normalized();
 				real_t denom = A->compute_angular_impulse_denominator(normal) +
-					B->compute_angular_impulse_denominator(normal);
+							   B->compute_angular_impulse_denominator(normal);
 				// scale for mass and relaxation
-				velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor;
+				velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor;
 			}
 
 			//solve angular positional correction
-			Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step);
+			Vector3 angularError = -axisA.cross(axisB) * (real_t(1.) / p_step);
 			real_t len2 = angularError.length();
-			if (len2>real_t(0.00001))
-			{
+			if (len2 > real_t(0.00001)) {
 				Vector3 normal2 = angularError.normalized();
 				real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
-						B->compute_angular_impulse_denominator(normal2);
-				angularError *= (real_t(1.)/denom2) * relaxation;
+								B->compute_angular_impulse_denominator(normal2);
+				angularError *= (real_t(1.) / denom2) * relaxation;
 			}
 
-			A->apply_torque_impulse(-velrelOrthog+angularError);
-			B->apply_torque_impulse(velrelOrthog-angularError);
+			A->apply_torque_impulse(-velrelOrthog + angularError);
+			B->apply_torque_impulse(velrelOrthog - angularError);
 
 			// solve limit
-			if (m_solveLimit)
-			{
-				real_t amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (real_t(1.)/p_step)*m_biasFactor  ) * m_limitSign;
+			if (m_solveLimit) {
+				real_t amplitude = ((angVelB - angVelA).dot(axisA) * m_relaxationFactor + m_correction * (real_t(1.) / p_step) * m_biasFactor) * m_limitSign;
 
 				real_t impulseMag = amplitude * m_kHinge;
 
 				// Clamp the accumulated impulse
 				real_t temp = m_accLimitImpulse;
-				m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0) );
+				m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0));
 				impulseMag = m_accLimitImpulse - temp;
 
-
 				Vector3 impulse = axisA * impulseMag * m_limitSign;
 				A->apply_torque_impulse(impulse);
 				B->apply_torque_impulse(-impulse);
@@ -360,10 +336,9 @@ void HingeJointSW::solve(float p_step) {
 		}
 
 		//apply motor
-		if (m_enableAngularMotor)
-		{
+		if (m_enableAngularMotor) {
 			//todo: add limits too
-			Vector3 angularLimit(0,0,0);
+			Vector3 angularLimit(0, 0, 0);
 
 			Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
 			real_t projRelVel = velrel.dot(axisA);
@@ -377,12 +352,10 @@ void HingeJointSW::solve(float p_step) {
 			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
 			Vector3 motorImp = clippedMotorImpulse * axisA;
 
-			A->apply_torque_impulse(motorImp+angularLimit);
-			B->apply_torque_impulse(-motorImp-angularLimit);
-
+			A->apply_torque_impulse(motorImp + angularLimit);
+			B->apply_torque_impulse(-motorImp - angularLimit);
 		}
 	}
-
 }
 /*
 void	HingeJointSW::updateRHS(real_t	timeStep)
@@ -392,8 +365,7 @@ void	HingeJointSW::updateRHS(real_t	timeStep)
 }
 */
 
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	real_t coeff_1 = Math_PI / 4.0f;
 	real_t coeff_2 = 3.0f * coeff_1;
 	real_t abs_y = Math::abs(y);
@@ -408,33 +380,30 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
 	return (y < 0.0f) ? -angle : angle;
 }
 
-
 real_t HingeJointSW::get_hinge_angle() {
-	const Vector3 refAxis0  = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(0) );
-	const Vector3 refAxis1  = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(1) );
-	const Vector3 swingAxis = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(1) );
+	const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0));
+	const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1));
+	const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1));
 
-	return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
+	return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 }
 
-
 void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float p_value) {
 
 	switch (p_param) {
 
-		case PhysicsServer::HINGE_JOINT_BIAS: tau=p_value; break;
-		case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit=p_value; break;
-		case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit=p_value; break;
-		case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor=p_value; break;
-		case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness=p_value; break;
-		case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor=p_value; break;
-		case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity=p_value; break;
-		case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse=p_value; break;
-
+		case PhysicsServer::HINGE_JOINT_BIAS: tau = p_value; break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit = p_value; break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit = p_value; break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor = p_value; break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness = p_value; break;
+		case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor = p_value; break;
+		case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity = p_value; break;
+		case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse = p_value; break;
 	}
 }
 
-float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
+float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const {
 
 	switch (p_param) {
 
@@ -446,25 +415,23 @@ float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
 		case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor;
 		case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity;
 		case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse;
-
 	}
 
 	return 0;
 }
 
-void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value){
+void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value) {
 
 	switch (p_flag) {
-		case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit=p_value; break;
-		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor=p_value; break;
+		case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit = p_value; break;
+		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor = p_value; break;
 	}
-
 }
-bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const{
+bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const {
 
 	switch (p_flag) {
 		case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit;
-		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:return m_enableAngularMotor;
+		case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: return m_enableAngularMotor;
 	}
 
 	return false;
diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h
index 60203e3cc..90852baac 100644
--- a/servers/physics/joints/hinge_joint_sw.h
+++ b/servers/physics/joints/hinge_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
 #ifndef HINGE_JOINT_SW_H
 #define HINGE_JOINT_SW_H
 
-#include "servers/physics/joints_sw.h"
 #include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
@@ -53,8 +52,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 class HingeJointSW : public JointSW {
 
 	union {
@@ -66,41 +63,39 @@ class HingeJointSW : public JointSW {
 		BodySW *_arr[2];
 	};
 
-	JacobianEntrySW	m_jac[3]; //3 orthogonal linear constraints
-	JacobianEntrySW	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+	JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
+	JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
 	Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 	Transform m_rbBFrame;
 
-	real_t	m_motorTargetVelocity;
-	real_t	m_maxMotorImpulse;
+	real_t m_motorTargetVelocity;
+	real_t m_maxMotorImpulse;
 
-	real_t	m_limitSoftness;
-	real_t	m_biasFactor;
-	real_t    m_relaxationFactor;
+	real_t m_limitSoftness;
+	real_t m_biasFactor;
+	real_t m_relaxationFactor;
 
-	real_t    m_lowerLimit;
-	real_t    m_upperLimit;
+	real_t m_lowerLimit;
+	real_t m_upperLimit;
 
-	real_t	m_kHinge;
+	real_t m_kHinge;
 
-	real_t	m_limitSign;
-	real_t	m_correction;
+	real_t m_limitSign;
+	real_t m_correction;
 
-	real_t	m_accLimitImpulse;
+	real_t m_accLimitImpulse;
 
 	real_t tau;
 
-	bool            m_useLimit;
-	bool		m_angularOnly;
-	bool		m_enableAngularMotor;
-	bool		m_solveLimit;
+	bool m_useLimit;
+	bool m_angularOnly;
+	bool m_enableAngularMotor;
+	bool m_solveLimit;
 
 	real_t m_appliedImpulse;
 
-
 public:
-
 	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
 
 	virtual bool setup(float p_step);
@@ -114,9 +109,8 @@ public:
 	void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
 	bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
 
-	HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB);
-	HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, const Vector3& axisInA,const Vector3& axisInB);
-
+	HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB);
+	HingeJointSW(BodySW *rbA, BodySW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
 };
 
 #endif // HINGE_JOINT_SW_H
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h
index 1a5485995..26d04ba88 100644
--- a/servers/physics/joints/jacobian_entry_sw.h
+++ b/servers/physics/joints/jacobian_entry_sw.h
@@ -53,22 +53,21 @@ subject to the following restrictions:
 
 class JacobianEntrySW {
 public:
-	JacobianEntrySW() {};
+	JacobianEntrySW(){};
 	//constraint between two different rigidbodies
 	JacobianEntrySW(
-		const Matrix3& world2A,
-		const Matrix3& world2B,
-		const Vector3& rel_pos1,const Vector3& rel_pos2,
-		const Vector3& jointAxis,
-		const Vector3& inertiaInvA,
-		const real_t massInvA,
-		const Vector3& inertiaInvB,
-		const real_t massInvB)
-		:m_linearJointAxis(jointAxis)
-	{
+			const Matrix3 &world2A,
+			const Matrix3 &world2B,
+			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+			const Vector3 &jointAxis,
+			const Vector3 &inertiaInvA,
+			const real_t massInvA,
+			const Vector3 &inertiaInvB,
+			const real_t massInvB)
+		: m_linearJointAxis(jointAxis) {
 		m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
 		m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
 
@@ -76,104 +75,92 @@ public:
 	}
 
 	//angular constraint between two different rigidbodies
-	JacobianEntrySW(const Vector3& jointAxis,
-		const Matrix3& world2A,
-		const Matrix3& world2B,
-		const Vector3& inertiaInvA,
-		const Vector3& inertiaInvB)
-		:m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
-	{
-		m_aJ= world2A.xform(jointAxis);
+	JacobianEntrySW(const Vector3 &jointAxis,
+			const Matrix3 &world2A,
+			const Matrix3 &world2B,
+			const Vector3 &inertiaInvA,
+			const Vector3 &inertiaInvB)
+		: m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))) {
+		m_aJ = world2A.xform(jointAxis);
 		m_bJ = world2B.xform(-jointAxis);
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
 	}
 
 	//angular constraint between two different rigidbodies
-	JacobianEntrySW(const Vector3& axisInA,
-		const Vector3& axisInB,
-		const Vector3& inertiaInvA,
-		const Vector3& inertiaInvB)
-		: m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
-		, m_aJ(axisInA)
-		, m_bJ(-axisInB)
-	{
-		m_0MinvJt	= inertiaInvA * m_aJ;
+	JacobianEntrySW(const Vector3 &axisInA,
+			const Vector3 &axisInB,
+			const Vector3 &inertiaInvA,
+			const Vector3 &inertiaInvB)
+		: m_linearJointAxis(Vector3(real_t(0.), real_t(0.), real_t(0.))), m_aJ(axisInA), m_bJ(-axisInB) {
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
 	}
 
 	//constraint on one rigidbody
 	JacobianEntrySW(
-		const Matrix3& world2A,
-		const Vector3& rel_pos1,const Vector3& rel_pos2,
-		const Vector3& jointAxis,
-		const Vector3& inertiaInvA,
-		const real_t massInvA)
-		:m_linearJointAxis(jointAxis)
-	{
-		m_aJ= world2A.xform(rel_pos1.cross(jointAxis));
+			const Matrix3 &world2A,
+			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
+			const Vector3 &jointAxis,
+			const Vector3 &inertiaInvA,
+			const real_t massInvA)
+		: m_linearJointAxis(jointAxis) {
+		m_aJ = world2A.xform(rel_pos1.cross(jointAxis));
 		m_bJ = world2A.xform(rel_pos2.cross(-jointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
-		m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = Vector3(real_t(0.), real_t(0.), real_t(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
 
 		ERR_FAIL_COND(m_Adiag <= real_t(0.0));
 	}
 
-	real_t	getDiagonal() const { return m_Adiag; }
+	real_t getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	real_t	getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const
-	{
-		const JacobianEntrySW& jacA = *this;
+	real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA) const {
+		const JacobianEntrySW &jacA = *this;
 		real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
 		real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
 		return lin + ang;
 	}
 
-
-
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	real_t	getNonDiagonal(const JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const
-	{
-		const JacobianEntrySW& jacA = *this;
+	real_t getNonDiagonal(const JacobianEntrySW &jacB, const real_t massInvA, const real_t massInvB) const {
+		const JacobianEntrySW &jacA = *this;
 		Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
-		Vector3 lin0 = massInvA * lin ;
+		Vector3 lin0 = massInvA * lin;
 		Vector3 lin1 = massInvB * lin;
-		Vector3 sum = ang0+ang1+lin0+lin1;
-		return sum[0]+sum[1]+sum[2];
+		Vector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
 	}
 
-	real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB)
-	{
+	real_t getRelativeVelocity(const Vector3 &linvelA, const Vector3 &angvelA, const Vector3 &linvelB, const Vector3 &angvelB) {
 		Vector3 linrel = linvelA - linvelB;
-		Vector3 angvela  = angvelA * m_aJ;
-		Vector3 angvelb  = angvelB * m_bJ;
+		Vector3 angvela = angvelA * m_aJ;
+		Vector3 angvelb = angvelB * m_bJ;
 		linrel *= m_linearJointAxis;
 		angvela += angvelb;
 		angvela += linrel;
-		real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		real_t rel_vel2 = angvela[0] + angvela[1] + angvela[2];
 		return rel_vel2 + CMP_EPSILON;
 	}
-//private:
+	//private:
 
-	Vector3	m_linearJointAxis;
-	Vector3	m_aJ;
-	Vector3	m_bJ;
-	Vector3	m_0MinvJt;
-	Vector3	m_1MinvJt;
+	Vector3 m_linearJointAxis;
+	Vector3 m_aJ;
+	Vector3 m_bJ;
+	Vector3 m_0MinvJt;
+	Vector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	real_t	m_Adiag;
-
+	real_t m_Adiag;
 };
 
-
 #endif // JACOBIAN_ENTRY_SW_H
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
index 74d62e35f..0f66c48db 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -38,41 +38,37 @@ bool PinJointSW::setup(float p_step) {
 
 	m_appliedImpulse = real_t(0.);
 
-	Vector3	normal(0,0,0);
+	Vector3 normal(0, 0, 0);
 
-	for (int i=0;i<3;i++)
-	{
+	for (int i = 0; i < 3; i++) {
 		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,
-			normal,
-			A->get_inv_inertia(),
-			A->get_inv_mass(),
-			B->get_inv_inertia(),
-			B->get_inv_mass()));
+		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,
+											normal,
+											A->get_inv_inertia(),
+											A->get_inv_mass(),
+											B->get_inv_inertia(),
+											B->get_inv_mass()));
 		normal[i] = 0;
 	}
 
 	return true;
 }
 
-void PinJointSW::solve(float p_step){
+void PinJointSW::solve(float p_step) {
 
 	Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
 	Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
 
+	Vector3 normal(0, 0, 0);
 
-	Vector3 normal(0,0,0);
+	//	Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
+	//	Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
 
-
-//	Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
-//	Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
-
-	for (int i=0;i<3;i++)
-	{
+	for (int i = 0; i < 3; i++) {
 		normal[i] = 1;
 		real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
 
@@ -87,7 +83,7 @@ void PinJointSW::solve(float p_step){
 		real_t rel_vel;
 		rel_vel = normal.dot(vel);
 
-	/*
+		/*
 		//velocity error (first order error)
 		real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA,
 														B->getLinearVelocity(),angvelB);
@@ -96,38 +92,37 @@ void PinJointSW::solve(float p_step){
 		//positional error (zeroth order error)
 		real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
 
-		real_t impulse = depth*m_tau/p_step  * jacDiagABInv -  m_damping * rel_vel * jacDiagABInv;
+		real_t impulse = depth * m_tau / p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
 
 		real_t impulseClamp = m_impulseClamp;
-		if (impulseClamp > 0)
-		{
+		if (impulseClamp > 0) {
 			if (impulse < -impulseClamp)
 				impulse = -impulseClamp;
 			if (impulse > impulseClamp)
 				impulse = impulseClamp;
 		}
 
-		m_appliedImpulse+=impulse;
+		m_appliedImpulse += impulse;
 		Vector3 impulse_vector = normal * impulse;
-		A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
-		B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
+		A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
+		B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
 
 		normal[i] = 0;
 	}
 }
 
-void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float p_value) {
+void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, float p_value) {
 
-	switch(p_param)	 {
-		case PhysicsServer::PIN_JOINT_BIAS: m_tau=p_value; break;
-		case PhysicsServer::PIN_JOINT_DAMPING: m_damping=p_value; break;
-		case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp=p_value; break;
+	switch (p_param) {
+		case PhysicsServer::PIN_JOINT_BIAS: m_tau = p_value; break;
+		case PhysicsServer::PIN_JOINT_DAMPING: m_damping = p_value; break;
+		case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp = p_value; break;
 	}
 }
 
-float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
+float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const {
 
-	switch(p_param)	 {
+	switch (p_param) {
 		case PhysicsServer::PIN_JOINT_BIAS: return m_tau;
 		case PhysicsServer::PIN_JOINT_DAMPING: return m_damping;
 		case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp;
@@ -136,26 +131,22 @@ float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
 	return 0;
 }
 
-PinJointSW::PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b) : JointSW(_arr,2) {
-
-	A=p_body_a;
-	B=p_body_b;
-	m_pivotInA=p_pos_a;
-	m_pivotInB=p_pos_b;
-
-	m_tau=0.3;
-	m_damping=1;
-	m_impulseClamp=0;
-	m_appliedImpulse=0;
+PinJointSW::PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b)
+	: JointSW(_arr, 2) {
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	A = p_body_a;
+	B = p_body_b;
+	m_pivotInA = p_pos_a;
+	m_pivotInB = p_pos_b;
 
+	m_tau = 0.3;
+	m_damping = 1;
+	m_impulseClamp = 0;
+	m_appliedImpulse = 0;
 
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
 PinJointSW::~PinJointSW() {
-
-
-
 }
diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h
index 679797249..5bc16dfc4 100644
--- a/servers/physics/joints/pin_joint_sw.h
+++ b/servers/physics/joints/pin_joint_sw.h
@@ -34,8 +34,8 @@ Adapted to Godot from the Bullet library.
 #ifndef PIN_JOINT_SW_H
 #define PIN_JOINT_SW_H
 
-#include "servers/physics/joints_sw.h"
 #include "servers/physics/joints/jacobian_entry_sw.h"
+#include "servers/physics/joints_sw.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
@@ -52,7 +52,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 class PinJointSW : public JointSW {
 
 	union {
@@ -64,37 +63,33 @@ class PinJointSW : public JointSW {
 		BodySW *_arr[2];
 	};
 
+	real_t m_tau; //bias
+	real_t m_damping;
+	real_t m_impulseClamp;
+	real_t m_appliedImpulse;
 
-	real_t	m_tau; //bias
-	real_t	m_damping;
-	real_t	m_impulseClamp;
-	real_t  m_appliedImpulse;
-
-	JacobianEntrySW	m_jac[3]; //3 orthogonal linear constraints
+	JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
 
-	Vector3	m_pivotInA;
-	Vector3	m_pivotInB;
+	Vector3 m_pivotInA;
+	Vector3 m_pivotInB;
 
 public:
-
 	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
 
 	virtual bool setup(float p_step);
 	virtual void solve(float p_step);
 
-	void set_param(PhysicsServer::PinJointParam p_param,float p_value);
+	void set_param(PhysicsServer::PinJointParam p_param, float p_value);
 	float get_param(PhysicsServer::PinJointParam p_param) const;
 
-	void set_pos_A(const Vector3& p_pos) { m_pivotInA=p_pos; }
-	void set_pos_B(const Vector3& p_pos) { m_pivotInB=p_pos; }
+	void set_pos_A(const Vector3 &p_pos) { m_pivotInA = p_pos; }
+	void set_pos_B(const Vector3 &p_pos) { m_pivotInB = p_pos; }
 
 	Vector3 get_pos_A() { return m_pivotInB; }
 	Vector3 get_pos_B() { return m_pivotInA; }
 
-	PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b);
+	PinJointSW(BodySW *p_body_a, const Vector3 &p_pos_a, BodySW *p_body_b, const Vector3 &p_pos_b);
 	~PinJointSW();
 };
 
-
-
 #endif // PIN_JOINT_SW_H
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
index 7d1933e0d..d1a5e50ec 100644
--- a/servers/physics/joints/slider_joint_sw.cpp
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -36,8 +36,7 @@ See corresponding header file for licensing info.
 
 //-----------------------------------------------------------------------------
 
-static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
-{
+static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	real_t coeff_1 = Math_PI / 4.0f;
 	real_t coeff_2 = 3.0f * coeff_1;
 	real_t abs_y = Math::abs(y);
@@ -52,13 +51,11 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
 	return (y < 0.0f) ? -angle : angle;
 }
 
-
-void SliderJointSW::initParams()
-{
-    m_lowerLinLimit = real_t(1.0);
-    m_upperLinLimit = real_t(-1.0);
-    m_lowerAngLimit = real_t(0.);
-    m_upperAngLimit = real_t(0.);
+void SliderJointSW::initParams() {
+	m_lowerLinLimit = real_t(1.0);
+	m_upperLinLimit = real_t(-1.0);
+	m_lowerAngLimit = real_t(0.);
+	m_upperAngLimit = real_t(0.);
 	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingDirLin = real_t(0.);
@@ -84,40 +81,35 @@ void SliderJointSW::initParams()
 	m_accumulatedLinMotorImpulse = real_t(0.0);
 
 	m_poweredAngMotor = false;
-    m_targetAngMotorVelocity = real_t(0.);
-    m_maxAngMotorForce = real_t(0.);
+	m_targetAngMotorVelocity = real_t(0.);
+	m_maxAngMotorForce = real_t(0.);
 	m_accumulatedAngMotorImpulse = real_t(0.0);
 
 } // SliderJointSW::initParams()
 
 //-----------------------------------------------------------------------------
 
-
 //-----------------------------------------------------------------------------
 
-SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB)
-	:  JointSW(_arr,2)
-	, m_frameInA(frameInA)
-	, m_frameInB(frameInB)
-{
+SliderJointSW::SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB)
+	: JointSW(_arr, 2), m_frameInA(frameInA), m_frameInB(frameInB) {
 
-	A=rbA;
-	B=rbB;
+	A = rbA;
+	B = rbB;
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 
 	initParams();
 } // SliderJointSW::SliderJointSW()
 
 //-----------------------------------------------------------------------------
 
-bool SliderJointSW::setup(float p_step)
-{
+bool SliderJointSW::setup(float p_step) {
 
 	//calculate transforms
-    m_calculatedTransformA = A->get_transform() * m_frameInA;
-    m_calculatedTransformB = B->get_transform() * m_frameInB;
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
+	m_calculatedTransformB = B->get_transform() * m_frameInB;
 	m_realPivotAInW = m_calculatedTransformA.origin;
 	m_realPivotBInW = m_calculatedTransformB.origin;
 	m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
@@ -125,42 +117,38 @@ bool SliderJointSW::setup(float p_step)
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
 	m_relPosA = m_projPivotInW - A->get_transform().origin;
 	m_relPosB = m_realPivotBInW - B->get_transform().origin;
-    Vector3 normalWorld;
-    int i;
-    //linear part
-    for(i = 0; i < 3; i++)
-    {
+	Vector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++) {
 		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,
-			normalWorld,
-			A->get_inv_inertia(),
-			A->get_inv_mass(),
-			B->get_inv_inertia(),
-			B->get_inv_mass()
-			));
+											   A->get_transform().basis.transposed(),
+											   B->get_transform().basis.transposed(),
+											   m_relPosA,
+											   m_relPosB,
+											   normalWorld,
+											   A->get_inv_inertia(),
+											   A->get_inv_mass(),
+											   B->get_inv_inertia(),
+											   B->get_inv_mass()));
 		m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
 		m_depth[i] = m_delta.dot(normalWorld);
-    }
+	}
 	testLinLimits();
-    // angular part
-    for(i = 0; i < 3; i++)
-    {
+	// angular part
+	for (i = 0; i < 3; i++) {
 		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_inv_inertia(),
-	    B->get_inv_inertia()
-			));
+		memnew_placement(&m_jacAng[i], JacobianEntrySW(
+											   normalWorld,
+											   A->get_transform().basis.transposed(),
+											   B->get_transform().basis.transposed(),
+											   A->get_inv_inertia(),
+											   B->get_inv_inertia()));
 	}
 	testAngLimits();
 	Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
-	m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
+	m_kAngle = real_t(1.0) / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
 	// clear accumulator for motors
 	m_accumulatedLinMotorImpulse = real_t(0.0);
 	m_accumulatedAngMotorImpulse = real_t(0.0);
@@ -172,14 +160,13 @@ bool SliderJointSW::setup(float p_step)
 
 void SliderJointSW::solve(real_t p_step) {
 
-    int i;
-    // linear
-    Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
-    Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
-    Vector3 vel = velA - velB;
-	for(i = 0; i < 3; i++)
-    {
-		const Vector3& normal = m_jacLin[i].m_linearJointAxis;
+	int i;
+	// linear
+	Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
+	Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
+	Vector3 vel = velA - velB;
+	for (i = 0; i < 3; i++) {
+		const Vector3 &normal = m_jacLin[i].m_linearJointAxis;
 		real_t rel_vel = normal.dot(vel);
 		// calculate positional error
 		real_t depth = m_depth[i];
@@ -190,81 +177,70 @@ void SliderJointSW::solve(real_t p_step) {
 		// calcutate and apply impulse
 		real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
 		Vector3 impulse_vector = normal * normalImpulse;
-		A->apply_impulse( m_relPosA, impulse_vector);
-		B->apply_impulse(m_relPosB,-impulse_vector);
-		if(m_poweredLinMotor && (!i))
-		{ // apply linear motor
-			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
-			{
+		A->apply_impulse(m_relPosA, impulse_vector);
+		B->apply_impulse(m_relPosB, -impulse_vector);
+		if (m_poweredLinMotor && (!i)) { // apply linear motor
+			if (m_accumulatedLinMotorImpulse < m_maxLinMotorForce) {
 				real_t desiredMotorVel = m_targetLinMotorVelocity;
 				real_t motor_relvel = desiredMotorVel + rel_vel;
 				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
 				// clamp accumulated impulse
 				real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse);
-				if(new_acc  > m_maxLinMotorForce)
-				{
+				if (new_acc > m_maxLinMotorForce) {
 					new_acc = m_maxLinMotorForce;
 				}
-				real_t del = new_acc  - m_accumulatedLinMotorImpulse;
-				if(normalImpulse < real_t(0.0))
-				{
+				real_t del = new_acc - m_accumulatedLinMotorImpulse;
+				if (normalImpulse < real_t(0.0)) {
 					normalImpulse = -del;
-				}
-				else
-				{
+				} else {
 					normalImpulse = del;
 				}
 				m_accumulatedLinMotorImpulse = new_acc;
 				// apply clamped impulse
 				impulse_vector = normal * normalImpulse;
-				A->apply_impulse( m_relPosA, impulse_vector);
-				B->apply_impulse( m_relPosB,-impulse_vector);
+				A->apply_impulse(m_relPosA, impulse_vector);
+				B->apply_impulse(m_relPosB, -impulse_vector);
 			}
 		}
-    }
+	}
 	// angular
 	// get axes in world space
-	Vector3 axisA =  m_calculatedTransformA.basis.get_axis(0);
-	Vector3 axisB =  m_calculatedTransformB.basis.get_axis(0);
+	Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
+	Vector3 axisB = m_calculatedTransformB.basis.get_axis(0);
 
-	const Vector3& angVelA = A->get_angular_velocity();
-	const Vector3& angVelB = B->get_angular_velocity();
+	const Vector3 &angVelA = A->get_angular_velocity();
+	const Vector3 &angVelB = B->get_angular_velocity();
 
 	Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
 	Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
 
 	Vector3 angAorthog = angVelA - angVelAroundAxisA;
 	Vector3 angBorthog = angVelB - angVelAroundAxisB;
-	Vector3 velrelOrthog = angAorthog-angBorthog;
+	Vector3 velrelOrthog = angAorthog - angBorthog;
 	//solve orthogonal angular velocity correction
 	real_t len = velrelOrthog.length();
-	if (len > real_t(0.00001))
-	{
+	if (len > real_t(0.00001)) {
 		Vector3 normal = velrelOrthog.normalized();
 		real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal);
-		velrelOrthog *= (real_t(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+		velrelOrthog *= (real_t(1.) / denom) * m_dampingOrthoAng * m_softnessOrthoAng;
 	}
 	//solve angular positional correction
-	Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step);
+	Vector3 angularError = axisA.cross(axisB) * (real_t(1.) / p_step);
 	real_t len2 = angularError.length();
-	if (len2>real_t(0.00001))
-	{
+	if (len2 > real_t(0.00001)) {
 		Vector3 normal2 = angularError.normalized();
 		real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2);
-		angularError *= (real_t(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+		angularError *= (real_t(1.) / denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
 	}
 	// apply impulse
-	A->apply_torque_impulse(-velrelOrthog+angularError);
-	B->apply_torque_impulse(velrelOrthog-angularError);
+	A->apply_torque_impulse(-velrelOrthog + angularError);
+	B->apply_torque_impulse(velrelOrthog - angularError);
 	real_t impulseMag;
 	//solve angular limits
-	if(m_solveAngLim)
-	{
+	if (m_solveAngLim) {
 		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
 		impulseMag *= m_kAngle * m_softnessLimAng;
-	}
-	else
-	{
+	} else {
 		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
 		impulseMag *= m_kAngle * m_softnessDirAng;
 	}
@@ -272,10 +248,8 @@ void SliderJointSW::solve(real_t p_step) {
 	A->apply_torque_impulse(impulse);
 	B->apply_torque_impulse(-impulse);
 	//apply angular motor
-	if(m_poweredAngMotor)
-	{
-		if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
-		{
+	if (m_poweredAngMotor) {
+		if (m_accumulatedAngMotorImpulse < m_maxAngMotorForce) {
 			Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
 			real_t projRelVel = velrel.dot(axisA);
 
@@ -285,17 +259,13 @@ void SliderJointSW::solve(real_t p_step) {
 			real_t angImpulse = m_kAngle * motor_relvel;
 			// clamp accumulated impulse
 			real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
-			if(new_acc  > m_maxAngMotorForce)
-			{
+			if (new_acc > m_maxAngMotorForce) {
 				new_acc = m_maxAngMotorForce;
 			}
-			real_t del = new_acc  - m_accumulatedAngMotorImpulse;
-			if(angImpulse < real_t(0.0))
-			{
+			real_t del = new_acc - m_accumulatedAngMotorImpulse;
+			if (angImpulse < real_t(0.0)) {
 				angImpulse = -del;
-			}
-			else
-			{
+			} else {
 				angImpulse = del;
 			}
 			m_accumulatedAngMotorImpulse = new_acc;
@@ -311,96 +281,75 @@ void SliderJointSW::solve(real_t p_step) {
 
 //-----------------------------------------------------------------------------
 
-void SliderJointSW::calculateTransforms(void){
-	m_calculatedTransformA = A->get_transform() * m_frameInA ;
+void SliderJointSW::calculateTransforms(void) {
+	m_calculatedTransformA = A->get_transform() * m_frameInA;
 	m_calculatedTransformB = B->get_transform() * m_frameInB;
 	m_realPivotAInW = m_calculatedTransformA.origin;
 	m_realPivotBInW = m_calculatedTransformB.origin;
 	m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
 	m_delta = m_realPivotBInW - m_realPivotAInW;
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-    Vector3 normalWorld;
-    int i;
-    //linear part
-    for(i = 0; i < 3; i++)
-    {
+	Vector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++) {
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
 		m_depth[i] = m_delta.dot(normalWorld);
-    }
+	}
 } // SliderJointSW::calculateTransforms()
 
 //-----------------------------------------------------------------------------
 
-void SliderJointSW::testLinLimits(void)
-{
+void SliderJointSW::testLinLimits(void) {
 	m_solveLinLim = false;
 	m_linPos = m_depth[0];
-	if(m_lowerLinLimit <= m_upperLinLimit)
-	{
-		if(m_depth[0] > m_upperLinLimit)
-		{
+	if (m_lowerLinLimit <= m_upperLinLimit) {
+		if (m_depth[0] > m_upperLinLimit) {
 			m_depth[0] -= m_upperLinLimit;
 			m_solveLinLim = true;
-		}
-		else if(m_depth[0] < m_lowerLinLimit)
-		{
+		} else if (m_depth[0] < m_lowerLinLimit) {
 			m_depth[0] -= m_lowerLinLimit;
 			m_solveLinLim = true;
-		}
-		else
-		{
+		} else {
 			m_depth[0] = real_t(0.);
 		}
-	}
-	else
-	{
+	} else {
 		m_depth[0] = real_t(0.);
 	}
 } // SliderJointSW::testLinLimits()
 
 //-----------------------------------------------------------------------------
 
-
-void SliderJointSW::testAngLimits(void)
-{
+void SliderJointSW::testAngLimits(void) {
 	m_angDepth = real_t(0.);
 	m_solveAngLim = false;
-	if(m_lowerAngLimit <= m_upperAngLimit)
-	{
+	if (m_lowerAngLimit <= m_upperAngLimit) {
 		const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1);
 		const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2);
 		const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1);
 		real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
-		if(rot < m_lowerAngLimit)
-		{
+		if (rot < m_lowerAngLimit) {
 			m_angDepth = rot - m_lowerAngLimit;
 			m_solveAngLim = true;
-		}
-		else if(rot > m_upperAngLimit)
-		{
+		} else if (rot > m_upperAngLimit) {
 			m_angDepth = rot - m_upperAngLimit;
 			m_solveAngLim = true;
 		}
 	}
 } // SliderJointSW::testAngLimits()
 
-
 //-----------------------------------------------------------------------------
 
-
-
-Vector3 SliderJointSW::getAncorInA(void)
-{
+Vector3 SliderJointSW::getAncorInA(void) {
 	Vector3 ancorInA;
 	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
-	ancorInA = A->get_transform().inverse().xform( ancorInA );
+	ancorInA = A->get_transform().inverse().xform(ancorInA);
 	return ancorInA;
 } // SliderJointSW::getAncorInA()
 
 //-----------------------------------------------------------------------------
 
-Vector3 SliderJointSW::getAncorInB(void)
-{
+Vector3 SliderJointSW::getAncorInB(void) {
 	Vector3 ancorInB;
 	ancorInB = m_frameInB.origin;
 	return ancorInB;
@@ -408,38 +357,36 @@ Vector3 SliderJointSW::getAncorInB(void)
 
 void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float p_value) {
 
-	switch(p_param) {
-		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin=p_value; break;
-
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng=p_value; break;
-		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng=p_value; break;
+	switch (p_param) {
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin = p_value; break;
 
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng = p_value; break;
+		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng = p_value; break;
 	}
-
 }
 
 float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit;
 		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit;
 		case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin;
@@ -463,11 +410,7 @@ float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
 		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng;
 		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng;
 		case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng;
-
 	}
 
 	return 0;
-
 }
-
-
diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h
index 4e697e6f6..8ca650b22 100644
--- a/servers/physics/joints/slider_joint_sw.h
+++ b/servers/physics/joints/slider_joint_sw.h
@@ -34,9 +34,8 @@ Adapted to Godot from the Bullet library.
 #ifndef SLIDER_JOINT_SW_H
 #define SLIDER_JOINT_SW_H
 
-#include "servers/physics/joints_sw.h"
 #include "servers/physics/joints/jacobian_entry_sw.h"
-
+#include "servers/physics/joints_sw.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
@@ -59,15 +58,14 @@ April 04, 2008
 
 */
 
-#define SLIDER_CONSTRAINT_DEF_SOFTNESS		(real_t(1.0))
-#define SLIDER_CONSTRAINT_DEF_DAMPING		(real_t(1.0))
-#define SLIDER_CONSTRAINT_DEF_RESTITUTION	(real_t(0.7))
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
 
 //-----------------------------------------------------------------------------
 
 class SliderJointSW : public JointSW {
 protected:
-
 	union {
 		struct {
 			BodySW *A;
@@ -77,8 +75,8 @@ protected:
 		BodySW *_arr[2];
 	};
 
-	Transform	m_frameInA;
-    Transform	m_frameInB;
+	Transform m_frameInA;
+	Transform m_frameInB;
 
 	// linear limits
 	real_t m_lowerLinLimit;
@@ -115,14 +113,14 @@ protected:
 	bool m_solveLinLim;
 	bool m_solveAngLim;
 
-	JacobianEntrySW	m_jacLin[3];
-	real_t		m_jacLinDiagABInv[3];
+	JacobianEntrySW m_jacLin[3];
+	real_t m_jacLinDiagABInv[3];
 
-    JacobianEntrySW	m_jacAng[3];
+	JacobianEntrySW m_jacAng[3];
 
 	real_t m_timeStep;
-    Transform m_calculatedTransformA;
-    Transform m_calculatedTransformB;
+	Transform m_calculatedTransformA;
+	Transform m_calculatedTransformB;
 
 	Vector3 m_sliderAxis;
 	Vector3 m_realPivotAInW;
@@ -138,45 +136,46 @@ protected:
 	real_t m_angDepth;
 	real_t m_kAngle;
 
-	bool	 m_poweredLinMotor;
-    real_t m_targetLinMotorVelocity;
-    real_t m_maxLinMotorForce;
-    real_t m_accumulatedLinMotorImpulse;
+	bool m_poweredLinMotor;
+	real_t m_targetLinMotorVelocity;
+	real_t m_maxLinMotorForce;
+	real_t m_accumulatedLinMotorImpulse;
 
-	bool	 m_poweredAngMotor;
-    real_t m_targetAngMotorVelocity;
-    real_t m_maxAngMotorForce;
-    real_t m_accumulatedAngMotorImpulse;
+	bool m_poweredAngMotor;
+	real_t m_targetAngMotorVelocity;
+	real_t m_maxAngMotorForce;
+	real_t m_accumulatedAngMotorImpulse;
 
 	//------------------------
 	void initParams();
+
 public:
 	// constructors
-    SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB);
-    //SliderJointSW();
+	SliderJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB);
+	//SliderJointSW();
 	// overrides
 
 	// access
-    const BodySW* getRigidBodyA() const { return A; }
-    const BodySW* getRigidBodyB() const { return B; }
-    const Transform & getCalculatedTransformA() const { return m_calculatedTransformA; }
-    const Transform & getCalculatedTransformB() const { return m_calculatedTransformB; }
-    const Transform & getFrameOffsetA() const { return m_frameInA; }
-    const Transform & getFrameOffsetB() const { return m_frameInB; }
-    Transform & getFrameOffsetA() { return m_frameInA; }
-    Transform & getFrameOffsetB() { return m_frameInB; }
-    real_t getLowerLinLimit() { return m_lowerLinLimit; }
-    void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
-    real_t getUpperLinLimit() { return m_upperLinLimit; }
-    void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
-    real_t getLowerAngLimit() { return m_lowerAngLimit; }
-    void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
-    real_t getUpperAngLimit() { return m_upperAngLimit; }
-    void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
+	const BodySW *getRigidBodyA() const { return A; }
+	const BodySW *getRigidBodyB() const { return B; }
+	const Transform &getCalculatedTransformA() const { return m_calculatedTransformA; }
+	const Transform &getCalculatedTransformB() const { return m_calculatedTransformB; }
+	const Transform &getFrameOffsetA() const { return m_frameInA; }
+	const Transform &getFrameOffsetB() const { return m_frameInB; }
+	Transform &getFrameOffsetA() { return m_frameInA; }
+	Transform &getFrameOffsetB() { return m_frameInB; }
+	real_t getLowerLinLimit() { return m_lowerLinLimit; }
+	void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
+	real_t getUpperLinLimit() { return m_upperLinLimit; }
+	void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
+	real_t getLowerAngLimit() { return m_lowerAngLimit; }
+	void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
+	real_t getUpperAngLimit() { return m_upperAngLimit; }
+	void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
 
 	real_t getSoftnessDirLin() { return m_softnessDirLin; }
 	real_t getRestitutionDirLin() { return m_restitutionDirLin; }
-	real_t getDampingDirLin() { return m_dampingDirLin ; }
+	real_t getDampingDirLin() { return m_dampingDirLin; }
 	real_t getSoftnessDirAng() { return m_softnessDirAng; }
 	real_t getRestitutionDirAng() { return m_restitutionDirAng; }
 	real_t getDampingDirAng() { return m_dampingDirAng; }
@@ -230,9 +229,9 @@ public:
 	bool getSolveAngLimit() { return m_solveAngLim; }
 	real_t getAngDepth() { return m_angDepth; }
 	// shared code used by ODE solver
-	void	calculateTransforms(void);
-	void	testLinLimits(void);
-	void	testAngLimits(void);
+	void calculateTransforms(void);
+	void testLinLimits(void);
+	void testAngLimits(void);
 	// access for PE Solver
 	Vector3 getAncorInA(void);
 	Vector3 getAncorInB(void);
@@ -244,8 +243,6 @@ public:
 	void solve(float p_step);
 
 	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
-
 };
 
-
 #endif // SLIDER_JOINT_SW_H
diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h
index c87c86b59..0f637faf7 100644
--- a/servers/physics/joints_sw.h
+++ b/servers/physics/joints_sw.h
@@ -29,19 +29,16 @@
 #ifndef JOINTS_SW_H
 #define JOINTS_SW_H
 
-#include "constraint_sw.h"
 #include "body_sw.h"
-
+#include "constraint_sw.h"
 
 class JointSW : public ConstraintSW {
 
-
 public:
-
-	virtual PhysicsServer::JointType get_type() const=0;
-	_FORCE_INLINE_ JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) {
+	virtual PhysicsServer::JointType get_type() const = 0;
+	_FORCE_INLINE_ JointSW(BodySW **p_body_ptr = NULL, int p_body_count = 0)
+		: ConstraintSW(p_body_ptr, p_body_count) {
 	}
-
 };
 
 #endif // JOINTS_SW_H
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index ab54497ea..4cf6d7f09 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -29,57 +29,56 @@
 #include "physics_server_sw.h"
 #include "broad_phase_basic.h"
 #include "broad_phase_octree.h"
-#include "joints/pin_joint_sw.h"
-#include "joints/hinge_joint_sw.h"
-#include "joints/slider_joint_sw.h"
 #include "joints/cone_twist_joint_sw.h"
 #include "joints/generic_6dof_joint_sw.h"
-#include "script_language.h"
+#include "joints/hinge_joint_sw.h"
+#include "joints/pin_joint_sw.h"
+#include "joints/slider_joint_sw.h"
 #include "os/os.h"
+#include "script_language.h"
 
 RID PhysicsServerSW::shape_create(ShapeType p_shape) {
 
-	ShapeSW *shape=NULL;
-	switch(p_shape) {
+	ShapeSW *shape = NULL;
+	switch (p_shape) {
 
 		case SHAPE_PLANE: {
 
-			shape=memnew( PlaneShapeSW );
+			shape = memnew(PlaneShapeSW);
 		} break;
 		case SHAPE_RAY: {
 
-			shape=memnew( RayShapeSW );
+			shape = memnew(RayShapeSW);
 		} break;
 		case SHAPE_SPHERE: {
 
-			shape=memnew( SphereShapeSW);
+			shape = memnew(SphereShapeSW);
 		} break;
 		case SHAPE_BOX: {
 
-			shape=memnew( BoxShapeSW);
+			shape = memnew(BoxShapeSW);
 		} break;
 		case SHAPE_CAPSULE: {
 
-			shape=memnew( CapsuleShapeSW );
+			shape = memnew(CapsuleShapeSW);
 		} break;
 		case SHAPE_CONVEX_POLYGON: {
 
-			shape=memnew( ConvexPolygonShapeSW );
+			shape = memnew(ConvexPolygonShapeSW);
 		} break;
 		case SHAPE_CONCAVE_POLYGON: {
 
-			shape=memnew( ConcavePolygonShapeSW );
+			shape = memnew(ConcavePolygonShapeSW);
 		} break;
 		case SHAPE_HEIGHTMAP: {
 
-			shape=memnew( HeightMapShapeSW );
+			shape = memnew(HeightMapShapeSW);
 		} break;
 		case SHAPE_CUSTOM: {
 
 			ERR_FAIL_V(RID());
 
 		} break;
-
 	}
 
 	RID id = shape_owner.make_rid(shape);
@@ -88,71 +87,62 @@ RID PhysicsServerSW::shape_create(ShapeType p_shape) {
 	return id;
 };
 
-void PhysicsServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
+void PhysicsServerSW::shape_set_data(RID p_shape, const Variant &p_data) {
 
 	ShapeSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 	shape->set_data(p_data);
-
-
 };
 
-
 void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
 
 	ShapeSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 	shape->set_custom_bias(p_bias);
-
 }
 
-
 PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const {
 
 	const ShapeSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM);
+	ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM);
 	return shape->get_type();
-
 };
 
 Variant PhysicsServerSW::shape_get_data(RID p_shape) const {
 
 	const ShapeSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,Variant());
-	ERR_FAIL_COND_V(!shape->is_configured(),Variant());
+	ERR_FAIL_COND_V(!shape, Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(), Variant());
 	return shape->get_data();
-
 };
 
 real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const {
 
 	const ShapeSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ERR_FAIL_COND_V(!shape, 0);
 	return shape->get_custom_bias();
-
 }
 
-
 RID PhysicsServerSW::space_create() {
 
-	SpaceSW *space = memnew( SpaceSW );
+	SpaceSW *space = memnew(SpaceSW);
 	RID id = space_owner.make_rid(space);
 	space->set_self(id);
 	RID area_id = area_create();
 	AreaSW *area = area_owner.get(area_id);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 	space->set_default_area(area);
 	area->set_space(space);
 	area->set_priority(-1);
 	RID sgb = body_create();
-	body_set_space(sgb,id);
-	body_set_mode(sgb,BODY_MODE_STATIC);
+	body_set_space(sgb, id);
+	body_set_mode(sgb, BODY_MODE_STATIC);
 	space->set_static_global_body(sgb);
 
 	return id;
 };
 
-void PhysicsServerSW::space_set_active(RID p_space,bool p_active) {
+void PhysicsServerSW::space_set_active(RID p_space, bool p_active) {
 
 	SpaceSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
@@ -165,70 +155,63 @@ void PhysicsServerSW::space_set_active(RID p_space,bool p_active) {
 bool PhysicsServerSW::space_is_active(RID p_space) const {
 
 	const SpaceSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,false);
+	ERR_FAIL_COND_V(!space, false);
 
 	return active_spaces.has(space);
-
 }
 
-void PhysicsServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) {
+void PhysicsServerSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
 
 	SpaceSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
 
-	space->set_param(p_param,p_value);
-
+	space->set_param(p_param, p_value);
 }
 
-real_t PhysicsServerSW::space_get_param(RID p_space,SpaceParameter p_param) const {
+real_t PhysicsServerSW::space_get_param(RID p_space, SpaceParameter p_param) const {
 
 	const SpaceSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,0);
+	ERR_FAIL_COND_V(!space, 0);
 	return space->get_param(p_param);
 }
 
-PhysicsDirectSpaceState* PhysicsServerSW::space_get_direct_state(RID p_space) {
+PhysicsDirectSpaceState *PhysicsServerSW::space_get_direct_state(RID p_space) {
 
 	SpaceSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,NULL);
+	ERR_FAIL_COND_V(!space, NULL);
 	if (!doing_sync || space->is_locked()) {
 
 		ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification.");
 		ERR_FAIL_V(NULL);
-
-
 	}
 
 	return space->get_direct_state();
 }
 
-void PhysicsServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) {
+void PhysicsServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) {
 
 	SpaceSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
 	space->set_debug_contacts(p_max_contacts);
-
 }
 
 Vector<Vector3> PhysicsServerSW::space_get_contacts(RID p_space) const {
 
 	SpaceSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,Vector<Vector3>());
+	ERR_FAIL_COND_V(!space, Vector<Vector3>());
 	return space->get_debug_contacts();
-
 }
 
 int PhysicsServerSW::space_get_contact_count(RID p_space) const {
 
 	SpaceSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,0);
+	ERR_FAIL_COND_V(!space, 0);
 	return space->get_debug_contact_count();
-
 }
 
 RID PhysicsServerSW::area_create() {
 
-	AreaSW *area = memnew( AreaSW );
+	AreaSW *area = memnew(AreaSW);
 	RID rid = area_owner.make_rid(area);
 	area->set_self(rid);
 	return rid;
@@ -238,20 +221,19 @@ void PhysicsServerSW::area_set_space(RID p_area, RID p_space) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	SpaceSW *space=NULL;
+	SpaceSW *space = NULL;
 	if (p_space.is_valid()) {
 		space = space_owner.get(p_space);
 		ERR_FAIL_COND(!space);
 	}
 
 	area->set_space(space);
-
 };
 
 RID PhysicsServerSW::area_get_space(RID p_area) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 
 	SpaceSW *space = area->get_space();
 	if (!space)
@@ -261,7 +243,6 @@ RID PhysicsServerSW::area_get_space(RID p_area) const {
 
 void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
 
-
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
@@ -271,13 +252,12 @@ void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverride
 PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const {
 
 	const AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED);
+	ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED);
 
 	return area->get_space_override_mode();
 }
 
-
-void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p_transform) {
+void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform &p_transform) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -285,11 +265,10 @@ void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p
 	ShapeSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 
-	area->add_shape(shape,p_transform);
-
+	area->add_shape(shape, p_transform);
 }
 
-void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) {
+void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -298,39 +277,37 @@ void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) {
 	ERR_FAIL_COND(!shape);
 	ERR_FAIL_COND(!shape->is_configured());
 
-	area->set_shape(p_shape_idx,shape);
-
+	area->set_shape(p_shape_idx, shape);
 }
-void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform) {
+void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_shape_transform(p_shape_idx,p_transform);
+	area->set_shape_transform(p_shape_idx, p_transform);
 }
 
 int PhysicsServerSW::area_get_shape_count(RID p_area) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,-1);
+	ERR_FAIL_COND_V(!area, -1);
 
 	return area->get_shape_count();
-
 }
 RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 
 	ShapeSW *shape = area->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape,RID());
+	ERR_FAIL_COND_V(!shape, RID());
 
 	return shape->get_self();
 }
 Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Transform());
+	ERR_FAIL_COND_V(!area, Transform());
 
 	return area->get_shape_transform(p_shape_idx);
 }
@@ -348,65 +325,57 @@ void PhysicsServerSW::area_clear_shapes(RID p_area) {
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	while(area->get_shape_count())
+	while (area->get_shape_count())
 		area->remove_shape(0);
-
 }
 
-void PhysicsServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) {
+void PhysicsServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) {
 
 	if (space_owner.owns(p_area)) {
-		SpaceSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		SpaceSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 	area->set_instance_id(p_ID);
-
 }
 ObjectID PhysicsServerSW::area_get_object_instance_ID(RID p_area) const {
 
 	if (space_owner.owns(p_area)) {
-		SpaceSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		SpaceSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,0);
+	ERR_FAIL_COND_V(!area, 0);
 	return area->get_instance_id();
-
-
 }
 
-
-void PhysicsServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) {
+void PhysicsServerSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
 
 	if (space_owner.owns(p_area)) {
-		SpaceSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		SpaceSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	area->set_param(p_param,p_value);
-
+	area->set_param(p_param, p_value);
 };
 
-
-void PhysicsServerSW::area_set_transform(RID p_area, const Transform& p_transform) {
+void PhysicsServerSW::area_set_transform(RID p_area, const Transform &p_transform) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 	area->set_transform(p_transform);
-
 };
 
-Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const {
+Variant PhysicsServerSW::area_get_param(RID p_area, AreaParameter p_param) const {
 
 	if (space_owner.owns(p_area)) {
-		SpaceSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		SpaceSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Variant());
+	ERR_FAIL_COND_V(!area, Variant());
 
 	return area->get_param(p_param);
 };
@@ -414,12 +383,12 @@ Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const
 Transform PhysicsServerSW::area_get_transform(RID p_area) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Transform());
+	ERR_FAIL_COND_V(!area, Transform());
 
 	return area->get_transform();
 };
 
-void PhysicsServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) {
+void PhysicsServerSW::area_set_layer_mask(RID p_area, uint32_t p_mask) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -427,7 +396,7 @@ void PhysicsServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) {
 	area->set_layer_mask(p_mask);
 }
 
-void PhysicsServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) {
+void PhysicsServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -435,7 +404,7 @@ void PhysicsServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) {
 	area->set_collision_mask(p_mask);
 }
 
-void PhysicsServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
+void PhysicsServerSW::area_set_monitorable(RID p_area, bool p_monitorable) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -443,81 +412,73 @@ void PhysicsServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
 	area->set_monitorable(p_monitorable);
 }
 
-void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+void PhysicsServerSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
-
-
+	area->set_monitor_callback(p_receiver ? p_receiver->get_instance_ID() : 0, p_method);
 }
 
-void PhysicsServerSW::area_set_ray_pickable(RID p_area,bool p_enable) {
+void PhysicsServerSW::area_set_ray_pickable(RID p_area, bool p_enable) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
 	area->set_ray_pickable(p_enable);
-
 }
 
-bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const{
+bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const {
 
 	AreaSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,false);
+	ERR_FAIL_COND_V(!area, false);
 
 	return area->is_ray_pickable();
-
 }
 
-
-void PhysicsServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
-
+void PhysicsServerSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
 
 	AreaSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_area_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
+	area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_ID() : 0, p_method);
 }
 
 /* BODY API */
 
-RID PhysicsServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) {
+RID PhysicsServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) {
 
-	BodySW *body = memnew( BodySW );
-	if (p_mode!=BODY_MODE_RIGID)
+	BodySW *body = memnew(BodySW);
+	if (p_mode != BODY_MODE_RIGID)
 		body->set_mode(p_mode);
 	if (p_init_sleeping)
-		body->set_state(BODY_STATE_SLEEPING,p_init_sleeping);
+		body->set_state(BODY_STATE_SLEEPING, p_init_sleeping);
 	RID rid = body_owner.make_rid(body);
 	body->set_self(rid);
 	return rid;
 };
 
-
 void PhysicsServerSW::body_set_space(RID p_body, RID p_space) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	SpaceSW *space=NULL;
+	SpaceSW *space = NULL;
 
 	if (p_space.is_valid()) {
 		space = space_owner.get(p_space);
 		ERR_FAIL_COND(!space);
 	}
 
-	if (body->get_space()==space)
+	if (body->get_space() == space)
 		return; //pointles
 
 	body->set_space(space);
-
 };
 
 RID PhysicsServerSW::body_get_space(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,RID());
+	ERR_FAIL_COND_V(!body, RID());
 
 	SpaceSW *space = body->get_space();
 	if (!space)
@@ -525,7 +486,6 @@ RID PhysicsServerSW::body_get_space(RID p_body) const {
 	return space->get_self();
 };
 
-
 void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -537,12 +497,12 @@ void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
 PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
+	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
 
 	return body->get_mode();
 };
 
-void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p_transform) {
+void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform &p_transform) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -550,11 +510,10 @@ void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p
 	ShapeSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 
-	body->add_shape(shape,p_transform);
-
+	body->add_shape(shape, p_transform);
 }
 
-void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) {
+void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -563,59 +522,55 @@ void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) {
 	ERR_FAIL_COND(!shape);
 	ERR_FAIL_COND(!shape->is_configured());
 
-	body->set_shape(p_shape_idx,shape);
-
+	body->set_shape(p_shape_idx, shape);
 }
-void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform) {
+void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_shape_transform(p_shape_idx,p_transform);
+	body->set_shape_transform(p_shape_idx, p_transform);
 }
 
 int PhysicsServerSW::body_get_shape_count(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,-1);
+	ERR_FAIL_COND_V(!body, -1);
 
 	return body->get_shape_count();
-
 }
 RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,RID());
+	ERR_FAIL_COND_V(!body, RID());
 
 	ShapeSW *shape = body->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape,RID());
+	ERR_FAIL_COND_V(!shape, RID());
 
 	return shape->get_self();
 }
 
-void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) {
+void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count());
-	body->set_shape_as_trigger(p_shape_idx,p_enable);
-
+	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+	body->set_shape_as_trigger(p_shape_idx, p_enable);
 }
 
-bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const{
+bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
-	ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false);
 
 	return body->is_shape_set_as_trigger(p_shape_idx);
 }
 
-
 Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Transform());
+	ERR_FAIL_COND_V(!body, Transform());
 
 	return body->get_shape_transform(p_shape_idx);
 }
@@ -633,24 +588,22 @@ void PhysicsServerSW::body_clear_shapes(RID p_body) {
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	while(body->get_shape_count())
+	while (body->get_shape_count())
 		body->remove_shape(0);
-
 }
 
-void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) {
+void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_continuous_collision_detection(p_enable);
-
 }
 
 bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body, false);
 
 	return body->is_continuous_collision_detection_enabled();
 }
@@ -662,16 +615,14 @@ void PhysicsServerSW::body_set_layer_mask(RID p_body, uint32_t p_mask) {
 
 	body->set_layer_mask(p_mask);
 	body->wakeup();
-
 }
 
-uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const{
+uint32_t PhysicsServerSW::body_get_layer_mask(RID p_body, uint32_t p_mask) const {
 
 	const BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_layer_mask();
-
 }
 
 void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) {
@@ -681,48 +632,42 @@ void PhysicsServerSW::body_set_collision_mask(RID p_body, uint32_t p_mask) {
 
 	body->set_collision_mask(p_mask);
 	body->wakeup();
-
 }
 
-uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body, uint32_t p_mask) const{
+uint32_t PhysicsServerSW::body_get_collision_mask(RID p_body, uint32_t p_mask) const {
 
 	const BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_collision_mask();
-
 }
 
-
-void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
+void PhysicsServerSW::body_attach_object_instance_ID(RID p_body, uint32_t p_ID) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_instance_id(p_ID);
-
 };
 
 uint32_t PhysicsServerSW::body_get_object_instance_ID(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_instance_id();
 };
 
-
 void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-
 };
 
 uint32_t PhysicsServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return 0;
 };
@@ -732,38 +677,34 @@ void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_param(p_param,p_value);
+	body->set_param(p_param, p_value);
 };
 
 float PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
 };
 
-
-
-void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
+void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_state(p_state,p_variant);
-
+	body->set_state(p_state, p_variant);
 };
 
 Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Variant());
+	ERR_FAIL_COND_V(!body, Variant());
 
 	return body->get_state(p_state);
 };
 
-
-void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force) {
+void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3 &p_force) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -775,11 +716,11 @@ void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force)
 Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Vector3());
+	ERR_FAIL_COND_V(!body, Vector3());
 	return body->get_applied_force();
 };
 
-void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torque) {
+void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -791,54 +732,48 @@ void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torqu
 Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Vector3());
+	ERR_FAIL_COND_V(!body, Vector3());
 
 	return body->get_applied_torque();
 };
 
-void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse) {
+void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->apply_impulse(p_pos,p_impulse);
+	body->apply_impulse(p_pos, p_impulse);
 	body->wakeup();
 };
 
-void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) {
+void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	Vector3 v = body->get_linear_velocity();
 	Vector3 axis = p_axis_velocity.normalized();
-	v-=axis*axis.dot(v);
-	v+=p_axis_velocity;
+	v -= axis * axis.dot(v);
+	v += p_axis_velocity;
 	body->set_linear_velocity(v);
 	body->wakeup();
-
 };
 
-
-void PhysicsServerSW::body_set_axis_lock(RID p_body,BodyAxisLock p_lock) {
+void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxisLock p_lock) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_axis_lock(p_lock);
 	body->wakeup();
-
 }
 
-PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const{
+PhysicsServerSW::BodyAxisLock PhysicsServerSW::body_get_axis_lock(RID p_body) const {
 
 	const BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,BODY_AXIS_LOCK_DISABLED);
+	ERR_FAIL_COND_V(!body, BODY_AXIS_LOCK_DISABLED);
 	return body->get_axis_lock();
-
 }
 
-
-
 void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -846,7 +781,6 @@ void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) {
 
 	body->add_exception(p_body_b);
 	body->wakeup();
-
 };
 
 void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
@@ -856,7 +790,6 @@ void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b)
 
 	body->remove_exception(p_body_b);
 	body->wakeup();
-
 };
 
 void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
@@ -864,27 +797,25 @@ void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exc
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	for(int i=0;i<body->get_exceptions().size();i++) {
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
 		p_exceptions->push_back(body->get_exceptions()[i]);
 	}
-
 };
 
 void PhysicsServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-
 };
 
 float PhysicsServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 	return 0;
 };
 
-void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) {
+void PhysicsServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -895,7 +826,7 @@ void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) {
 bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body, false);
 	return body->get_omit_force_integration();
 };
 
@@ -909,355 +840,333 @@ void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts)
 int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,-1);
+	ERR_FAIL_COND_V(!body, -1);
 	return body->get_max_contacts_reported();
 }
 
-void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) {
-
+void PhysicsServerSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata);
-
+	body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_ID() : ObjectID(0), p_method, p_udata);
 }
 
-void PhysicsServerSW::body_set_ray_pickable(RID p_body,bool p_enable) {
+void PhysicsServerSW::body_set_ray_pickable(RID p_body, bool p_enable) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_ray_pickable(p_enable);
-
 }
 
-bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const{
+bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const {
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body, false);
 	return body->is_ray_pickable();
-
 }
 
-
 /* JOINT API */
 
-RID PhysicsServerSW::joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B) {
+RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( PinJointSW(body_A,p_local_A,body_B,p_local_B) );
+	JointSW *joint = memnew(PinJointSW(body_A, p_local_A, body_B, p_local_B));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
 }
 
-void PhysicsServerSW::pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value){
+void PhysicsServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
-	pin_joint->set_param(p_param,p_value);
-
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
+	pin_joint->set_param(p_param, p_value);
 }
-float PhysicsServerSW::pin_joint_get_param(RID p_joint,PinJointParam p_param) const{
+float PhysicsServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,0);
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0);
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
 	return pin_joint->get_param(p_param);
-
 }
 
-void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3& p_A){
+void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3 &p_A) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
 	pin_joint->set_pos_A(p_A);
-
 }
-Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const{
+Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,Vector3());
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3());
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
 	return pin_joint->get_pos_A();
-
 }
 
-void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3& p_B){
+void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3 &p_B) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
 	pin_joint->set_pos_B(p_B);
-
 }
-Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const{
+Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,Vector3());
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3());
-	PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	PinJointSW *pin_joint = static_cast<PinJointSW *>(joint);
 	return pin_joint->get_pos_B();
-
 }
 
-
-RID PhysicsServerSW::joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B) {
+RID PhysicsServerSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_frame_A,p_frame_B) );
+	JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_frame_A, p_frame_B));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
-
 }
 
-
-RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B) {
+RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_pivot_A,p_pivot_B,p_axis_A,p_axis_B) );
+	JointSW *joint = memnew(HingeJointSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
-
 }
 
-void PhysicsServerSW::hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value){
+void PhysicsServerSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE);
-	HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
-	hinge_joint->set_param(p_param,p_value);
-
+	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint);
+	hinge_joint->set_param(p_param, p_value);
 }
-float PhysicsServerSW::hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const{
+float PhysicsServerSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,0);
-	HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0);
+	HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint);
 	return hinge_joint->get_param(p_param);
-
 }
 
-void PhysicsServerSW::hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value){
+void PhysicsServerSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE);
-	HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
-	hinge_joint->set_flag(p_flag,p_value);
-
+	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint);
+	hinge_joint->set_flag(p_flag, p_value);
 }
-bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const{
+bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,false);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,false);
-	HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false);
+	HingeJointSW *hinge_joint = static_cast<HingeJointSW *>(joint);
 	return hinge_joint->get_flag(p_flag);
 }
 
-void PhysicsServerSW::joint_set_solver_priority(RID p_joint,int p_priority) {
+void PhysicsServerSW::joint_set_solver_priority(RID p_joint, int p_priority) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
 	joint->set_priority(p_priority);
 }
 
-int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const{
+int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
+	ERR_FAIL_COND_V(!joint, 0);
 	return joint->get_priority();
-
 }
 
 PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,JOINT_PIN);
+	ERR_FAIL_COND_V(!joint, JOINT_PIN);
 	return joint->get_type();
 }
 
-RID PhysicsServerSW::joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
+RID PhysicsServerSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( SliderJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) );
+	JointSW *joint = memnew(SliderJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
 }
 
-void PhysicsServerSW::slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value){
+void PhysicsServerSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_SLIDER);
-	SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint);
-	slider_joint->set_param(p_param,p_value);
+	ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER);
+	SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint);
+	slider_joint->set_param(p_param, p_value);
 }
-float PhysicsServerSW::slider_joint_get_param(RID p_joint,SliderJointParam p_param) const{
+float PhysicsServerSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0);
-	SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0);
+	SliderJointSW *slider_joint = static_cast<SliderJointSW *>(joint);
 	return slider_joint->get_param(p_param);
 }
 
-
-RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
+RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( ConeTwistJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) );
+	JointSW *joint = memnew(ConeTwistJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
 }
 
-void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value) {
+void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_CONE_TWIST);
-	ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint);
-	cone_twist_joint->set_param(p_param,p_value);
+	ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST);
+	ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint);
+	cone_twist_joint->set_param(p_param, p_value);
 }
-float PhysicsServerSW::cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const {
+float PhysicsServerSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0);
-	ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0);
+	ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW *>(joint);
 	return cone_twist_joint->get_param(p_param);
 }
 
-
-RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
+RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 
 	BodySW *body_A = body_owner.get(p_body_A);
-	ERR_FAIL_COND_V(!body_A,RID());
+	ERR_FAIL_COND_V(!body_A, RID());
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(),RID());
-		p_body_B=body_A->get_space()->get_static_global_body();
+		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	BodySW *body_B = body_owner.get(p_body_B);
-	ERR_FAIL_COND_V(!body_B,RID());
+	ERR_FAIL_COND_V(!body_B, RID());
 
-	ERR_FAIL_COND_V(body_A==body_B,RID());
+	ERR_FAIL_COND_V(body_A == body_B, RID());
 
-	JointSW *joint = memnew( Generic6DOFJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B,true) );
+	JointSW *joint = memnew(Generic6DOFJointSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true));
 	RID rid = joint_owner.make_rid(joint);
 	joint->set_self(rid);
 	return rid;
 }
 
-void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param, float p_value){
+void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, float p_value) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF);
-	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
-	generic_6dof_joint->set_param(p_axis,p_param,p_value);
+	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint);
+	generic_6dof_joint->set_param(p_axis, p_param, p_value);
 }
-float PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param){
+float PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,0);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,0);
-	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
-	return generic_6dof_joint->get_param(p_axis,p_param);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0);
+	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint);
+	return generic_6dof_joint->get_param(p_axis, p_param);
 }
 
-void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag, bool p_enable){
+void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) {
 
 	JointSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF);
-	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
-	generic_6dof_joint->set_flag(p_axis,p_flag,p_enable);
+	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint);
+	generic_6dof_joint->set_flag(p_axis, p_flag, p_enable);
 }
-bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag){
+bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) {
 
 	JointSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,false);
-	ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,false);
-	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
-	return generic_6dof_joint->get_flag(p_axis,p_flag);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false);
+	Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW *>(joint);
+	return generic_6dof_joint->get_flag(p_axis, p_flag);
 }
 
-
 #if 0
 void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
 
@@ -1375,8 +1284,8 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		ShapeSW *shape = shape_owner.get(p_rid);
 
-		while(shape->get_owners().size()) {
-			ShapeOwnerSW *so=shape->get_owners().front()->key();
+		while (shape->get_owners().size()) {
+			ShapeOwnerSW *so = shape->get_owners().front()->key();
 			so->remove_shape(shape);
 		}
 
@@ -1386,16 +1295,15 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		BodySW *body = body_owner.get(p_rid);
 
-//		if (body->get_state_query())
-//			_clear_query(body->get_state_query());
+		//		if (body->get_state_query())
+		//			_clear_query(body->get_state_query());
 
-//		if (body->get_direct_state_query())
-//			_clear_query(body->get_direct_state_query());
+		//		if (body->get_direct_state_query())
+		//			_clear_query(body->get_direct_state_query());
 
 		body->set_space(NULL);
 
-
-		while( body->get_shape_count() ) {
+		while (body->get_shape_count()) {
 
 			body->remove_shape(0);
 		}
@@ -1413,12 +1321,12 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		AreaSW *area = area_owner.get(p_rid);
 
-//		if (area->get_monitor_query())
-//			_clear_query(area->get_monitor_query());
+		//		if (area->get_monitor_query())
+		//			_clear_query(area->get_monitor_query());
 
 		area->set_space(NULL);
 
-		while( area->get_shape_count() ) {
+		while (area->get_shape_count()) {
 
 			area->remove_shape(0);
 		}
@@ -1429,7 +1337,7 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		SpaceSW *space = space_owner.get(p_rid);
 
-		while(space->get_objects().size()) {
+		while (space->get_objects().size()) {
 			CollisionObjectSW *co = (CollisionObjectSW *)space->get_objects().front()->get();
 			co->set_space(NULL);
 		}
@@ -1444,7 +1352,7 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		JointSW *joint = joint_owner.get(p_rid);
 
-		for(int i=0;i<joint->get_body_count();i++) {
+		for (int i = 0; i < joint->get_body_count(); i++) {
 
 			joint->get_body_ptr()[i]->remove_constraint(joint);
 		}
@@ -1456,51 +1364,45 @@ void PhysicsServerSW::free(RID p_rid) {
 		ERR_EXPLAIN("Invalid ID");
 		ERR_FAIL();
 	}
-
-
 };
 
 void PhysicsServerSW::set_active(bool p_active) {
 
-	active=p_active;
+	active = p_active;
 };
 
 void PhysicsServerSW::init() {
 
-	doing_sync=true;
-	last_step=0.001;
-	iterations=8;// 8?
-	stepper = memnew( StepSW );
-	direct_state = memnew( PhysicsDirectBodyStateSW );
+	doing_sync = true;
+	last_step = 0.001;
+	iterations = 8; // 8?
+	stepper = memnew(StepSW);
+	direct_state = memnew(PhysicsDirectBodyStateSW);
 };
 
-
 void PhysicsServerSW::step(float p_step) {
 
-
 	if (!active)
 		return;
 
+	doing_sync = false;
 
-	doing_sync=false;
-
-	last_step=p_step;
-	PhysicsDirectBodyStateSW::singleton->step=p_step;
+	last_step = p_step;
+	PhysicsDirectBodyStateSW::singleton->step = p_step;
 
-	island_count=0;
-	active_objects=0;
-	collision_pairs=0;
-	for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
+	for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-		stepper->step((SpaceSW*)E->get(),p_step,iterations);
-		island_count+=E->get()->get_island_count();
-		active_objects+=E->get()->get_active_objects();
-		collision_pairs+=E->get()->get_collision_pairs();
+		stepper->step((SpaceSW *)E->get(), p_step, iterations);
+		island_count += E->get()->get_island_count();
+		active_objects += E->get()->get_active_objects();
+		collision_pairs += E->get()->get_collision_pairs();
 	}
-
 }
 
-void PhysicsServerSW::sync() {
+void PhysicsServerSW::sync(){
 
 };
 
@@ -1509,21 +1411,20 @@ void PhysicsServerSW::flush_queries() {
 	if (!active)
 		return;
 
-	doing_sync=true;
+	doing_sync = true;
 
 	uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
 
-	for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+	for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-		SpaceSW *space=(SpaceSW *)E->get();
+		SpaceSW *space = (SpaceSW *)E->get();
 		space->call_queries();
 	}
 
-
 	if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) {
 
 		uint64_t total_time[SpaceSW::ELAPSED_TIME_MAX];
-		static const char* time_name[SpaceSW::ELAPSED_TIME_MAX]={
+		static const char *time_name[SpaceSW::ELAPSED_TIME_MAX] = {
 			"integrate_forces",
 			"generate_islands",
 			"setup_constraints",
@@ -1531,44 +1432,39 @@ void PhysicsServerSW::flush_queries() {
 			"integrate_velocities"
 		};
 
-		for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) {
-			total_time[i]=0;
+		for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) {
+			total_time[i] = 0;
 		}
 
-		for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+		for (Set<const SpaceSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-			for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) {
-				total_time[i]+=E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i));
+			for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) {
+				total_time[i] += E->get()->get_elapsed_time(SpaceSW::ElapsedTime(i));
 			}
-
 		}
 
 		Array values;
-		values.resize(SpaceSW::ELAPSED_TIME_MAX*2);
-		for(int i=0;i<SpaceSW::ELAPSED_TIME_MAX;i++) {
-			values[i*2+0]=time_name[i];
-			values[i*2+1]=USEC_TO_SEC(total_time[i]);
+		values.resize(SpaceSW::ELAPSED_TIME_MAX * 2);
+		for (int i = 0; i < SpaceSW::ELAPSED_TIME_MAX; i++) {
+			values[i * 2 + 0] = time_name[i];
+			values[i * 2 + 1] = USEC_TO_SEC(total_time[i]);
 		}
 		values.push_back("flush_queries");
-		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec()-time_beg));
-
-		ScriptDebugger::get_singleton()->add_profiling_frame_data("physics",values);
+		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg));
 
+		ScriptDebugger::get_singleton()->add_profiling_frame_data("physics", values);
 	}
 };
 
-
-
 void PhysicsServerSW::finish() {
 
 	memdelete(stepper);
 	memdelete(direct_state);
 };
 
-
 int PhysicsServerSW::get_process_info(ProcessInfo p_info) {
 
-	switch(p_info) {
+	switch (p_info) {
 
 		case INFO_ACTIVE_OBJECTS: {
 
@@ -1581,64 +1477,55 @@ int PhysicsServerSW::get_process_info(ProcessInfo p_info) {
 
 			return island_count;
 		} break;
-
 	}
 
 	return 0;
 }
 
+void PhysicsServerSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
 
-void PhysicsServerSW::_shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
-
+	CollCbkData *cbk = (CollCbkData *)p_userdata;
 
-	CollCbkData *cbk=(CollCbkData *)p_userdata;
-
-	if (cbk->max==0)
+	if (cbk->max == 0)
 		return;
 
 	if (cbk->amount == cbk->max) {
 		//find least deep
-		float min_depth=1e20;
-		int min_depth_idx=0;
-		for(int i=0;i<cbk->amount;i++) {
+		float min_depth = 1e20;
+		int min_depth_idx = 0;
+		for (int i = 0; i < cbk->amount; i++) {
 
-			float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]);
-			if (d<min_depth) {
-				min_depth=d;
-				min_depth_idx=i;
+			float d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]);
+			if (d < min_depth) {
+				min_depth = d;
+				min_depth_idx = i;
 			}
-
 		}
 
 		float d = p_point_A.distance_squared_to(p_point_B);
-		if (d<min_depth)
+		if (d < min_depth)
 			return;
-		cbk->ptr[min_depth_idx*2+0]=p_point_A;
-		cbk->ptr[min_depth_idx*2+1]=p_point_B;
-
+		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
+		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
 
 	} else {
 
-		cbk->ptr[cbk->amount*2+0]=p_point_A;
-		cbk->ptr[cbk->amount*2+1]=p_point_B;
+		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
+		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
 		cbk->amount++;
 	}
 }
 
-
 PhysicsServerSW::PhysicsServerSW() {
 
-	BroadPhaseSW::create_func=BroadPhaseOctree::_create;
-	island_count=0;
-	active_objects=0;
-	collision_pairs=0;
-
-	active=true;
+	BroadPhaseSW::create_func = BroadPhaseOctree::_create;
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
 
+	active = true;
 };
 
-PhysicsServerSW::~PhysicsServerSW() {
+PhysicsServerSW::~PhysicsServerSW(){
 
 };
-
-
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index 46c77dc72..dfc533b78 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -29,19 +29,17 @@
 #ifndef PHYSICS_SERVER_SW
 #define PHYSICS_SERVER_SW
 
-
+#include "joints_sw.h"
 #include "servers/physics_server.h"
 #include "shape_sw.h"
 #include "space_sw.h"
 #include "step_sw.h"
-#include "joints_sw.h"
-
 
 class PhysicsServerSW : public PhysicsServer {
 
-	OBJ_TYPE( PhysicsServerSW, PhysicsServer );
+	OBJ_TYPE(PhysicsServerSW, PhysicsServer);
 
-friend class PhysicsDirectSpaceStateSW;
+	friend class PhysicsDirectSpaceStateSW;
 	bool active;
 	int iterations;
 	bool doing_sync;
@@ -52,7 +50,7 @@ friend class PhysicsDirectSpaceStateSW;
 	int collision_pairs;
 
 	StepSW *stepper;
-	Set<const SpaceSW*> active_spaces;
+	Set<const SpaceSW *> active_spaces;
 
 	PhysicsDirectBodyStateSW *direct_state;
 
@@ -62,9 +60,8 @@ friend class PhysicsDirectSpaceStateSW;
 	mutable RID_Owner<BodySW> body_owner;
 	mutable RID_Owner<JointSW> joint_owner;
 
-//	void _clear_query(QuerySW *p_query);
+	//	void _clear_query(QuerySW *p_query);
 public:
-
 	struct CollCbkData {
 
 		int max;
@@ -72,10 +69,10 @@ public:
 		Vector3 *ptr;
 	};
 
-	static void _shape_col_cbk(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata);
+	static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
 
 	virtual RID shape_create(ShapeType p_shape);
-	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_data(RID p_shape, const Variant &p_data);
 	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
 
 	virtual ShapeType shape_get_type(RID p_shape) const;
@@ -85,16 +82,16 @@ public:
 	/* SPACE API */
 
 	virtual RID space_create();
-	virtual void space_set_active(RID p_space,bool p_active);
+	virtual void space_set_active(RID p_space, bool p_active);
 	virtual bool space_is_active(RID p_space) const;
 
-	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
-	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const;
 
 	// this function only works on fixed process, errors and returns null otherwise
-	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space);
+	virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space);
 
-	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts);
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts);
 	virtual Vector<Vector3> space_get_contacts(RID p_space) const;
 	virtual int space_get_contact_count(RID p_space) const;
 
@@ -108,9 +105,9 @@ public:
 	virtual void area_set_space(RID p_area, RID p_space);
 	virtual RID area_get_space(RID p_area) const;
 
-	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform());
-	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
-	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform);
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform());
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform);
 
 	virtual int area_get_shape_count(RID p_area) const;
 	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
@@ -119,31 +116,30 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx);
 	virtual void area_clear_shapes(RID p_area);
 
-	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID);
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
 
-	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
-	virtual void area_set_transform(RID p_area, const Transform& p_transform);
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value);
+	virtual void area_set_transform(RID p_area, const Transform &p_transform);
 
-	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const;
 	virtual Transform area_get_transform(RID p_area) const;
 
-	virtual void area_set_ray_pickable(RID p_area,bool p_enable);
+	virtual void area_set_ray_pickable(RID p_area, bool p_enable);
 	virtual bool area_is_ray_pickable(RID p_area) const;
 
-	virtual void area_set_collision_mask(RID p_area,uint32_t p_mask);
-	virtual void area_set_layer_mask(RID p_area,uint32_t p_mask);
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask);
+	virtual void area_set_layer_mask(RID p_area, uint32_t p_mask);
 
-	virtual void area_set_monitorable(RID p_area,bool p_monitorable);
-
-	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
-	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable);
 
+	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
+	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
 
 	/* BODY API */
 
 	// create a body of a given type
-	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+	virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false);
 
 	virtual void body_set_space(RID p_body, RID p_space);
 	virtual RID body_get_space(RID p_body) const;
@@ -151,24 +147,24 @@ public:
 	virtual void body_set_mode(RID p_body, BodyMode p_mode);
 	virtual BodyMode body_get_mode(RID p_body) const;
 
-	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform());
-	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
-	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform);
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform());
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform);
 
 	virtual int body_get_shape_count(RID p_body) const;
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
 	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable);
 	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
 
 	virtual void body_remove_shape(RID p_body, int p_shape_idx);
 	virtual void body_clear_shapes(RID p_body);
 
-	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID);
 	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
 
-	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable);
+	virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable);
 	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const;
 
 	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask);
@@ -183,19 +179,19 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
 
-	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
 
-	virtual void body_set_applied_force(RID p_body, const Vector3& p_force);
+	virtual void body_set_applied_force(RID p_body, const Vector3 &p_force);
 	virtual Vector3 body_get_applied_force(RID p_body) const;
 
-	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque);
+	virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque);
 	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_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity);
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, 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);
+	virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock);
 	virtual BodyAxisLock body_get_axis_lock(RID p_body) const;
 
 	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
@@ -205,61 +201,60 @@ public:
 	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
 	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
 
-	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
 	virtual bool body_is_omitting_force_integration(RID p_body) const;
 
 	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
 	virtual int body_get_max_contacts_reported(RID p_body) const;
 
-	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
+	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
 
-	virtual void body_set_ray_pickable(RID p_body,bool p_enable);
+	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool body_is_ray_pickable(RID p_body) const;
 
 	/* JOINT API */
 
-	virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B);
+	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
 
-	virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value);
-	virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const;
+	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value);
+	virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const;
 
-	virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A);
+	virtual void pin_joint_set_local_A(RID p_joint, const Vector3 &p_A);
 	virtual Vector3 pin_joint_get_local_A(RID p_joint) const;
 
-	virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B);
+	virtual void pin_joint_set_local_B(RID p_joint, const Vector3 &p_B);
 	virtual Vector3 pin_joint_get_local_B(RID p_joint) const;
 
-	virtual RID joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B);
-	virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B);
-
-	virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value);
-	virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const;
+	virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B);
+	virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B);
 
-	virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value);
-	virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const;
+	virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value);
+	virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const;
 
+	virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value);
+	virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const;
 
-	virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
+	virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A
 
-	virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value);
-	virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const;
+	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value);
+	virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const;
 
-	virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
+	virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A
 
-	virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value);
-	virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const;
+	virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value);
+	virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const;
 
-	virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
+	virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B); //reference frame is A
 
-	virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value);
-	virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param);
+	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, float p_value);
+	virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param);
 
-	virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable);
-	virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag);
+	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable);
+	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag);
 
 	virtual JointType joint_get_type(RID p_joint) const;
 
-	virtual void joint_set_solver_priority(RID p_joint,int p_priority);
+	virtual void joint_set_solver_priority(RID p_joint, int p_priority);
 	virtual int joint_get_solver_priority(RID p_joint) const;
 
 #if 0
@@ -289,8 +284,6 @@ public:
 
 	PhysicsServerSW();
 	~PhysicsServerSW();
-
 };
 
 #endif
-
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index f7454a199..8f38af4f8 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -28,100 +28,91 @@
 /*************************************************************************/
 #include "shape_sw.h"
 #include "geometry.h"
-#include "sort.h"
 #include "quick_hull.h"
+#include "sort.h"
 #define _POINT_SNAP 0.001953125
 #define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002
 #define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998
 
-
-void ShapeSW::configure(const AABB& p_aabb) {
-	aabb=p_aabb;
-	configured=true;
-	for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {
-		ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();
+void ShapeSW::configure(const AABB &p_aabb) {
+	aabb = p_aabb;
+	configured = true;
+	for (Map<ShapeOwnerSW *, int>::Element *E = owners.front(); E; E = E->next()) {
+		ShapeOwnerSW *co = (ShapeOwnerSW *)E->key();
 		co->_shape_changed();
 	}
 }
 
-
-Vector3 ShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 ShapeSW::get_support(const Vector3 &p_normal) const {
 
 	Vector3 res;
 	int amnt;
-	get_supports(p_normal,1,&res,amnt);
+	get_supports(p_normal, 1, &res, amnt);
 	return res;
 }
 
 void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {
 
-	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+	Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner);
 	if (E) {
 		E->get()++;
 	} else {
-		owners[p_owner]=1;
+		owners[p_owner] = 1;
 	}
 }
 
-void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){
+void ShapeSW::remove_owner(ShapeOwnerSW *p_owner) {
 
-	Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+	Map<ShapeOwnerSW *, int>::Element *E = owners.find(p_owner);
 	ERR_FAIL_COND(!E);
 	E->get()--;
-	if (E->get()==0) {
+	if (E->get() == 0) {
 		owners.erase(E);
 	}
-
 }
 
-bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{
+bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const {
 
 	return owners.has(p_owner);
-
 }
 
-const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{
+const Map<ShapeOwnerSW *, int> &ShapeSW::get_owners() const {
 	return owners;
 }
 
-
 ShapeSW::ShapeSW() {
 
-	custom_bias=0;
-	configured=false;
+	custom_bias = 0;
+	configured = false;
 }
 
-
 ShapeSW::~ShapeSW() {
 
 	ERR_FAIL_COND(owners.size());
 }
 
-
-
 Plane PlaneShapeSW::get_plane() const {
 
 	return plane;
 }
 
-void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void PlaneShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 	// gibberish, a plane is infinity
-	r_min=-1e7;
-	r_max=1e7;
+	r_min = -1e7;
+	r_max = 1e7;
 }
 
-Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 PlaneShapeSW::get_support(const Vector3 &p_normal) const {
 
-	return p_normal*1e15;
+	return p_normal * 1e15;
 }
 
+bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
-
-	bool inters=plane.intersects_segment(p_begin,p_end,&r_result);
-	if(inters)
-		r_normal=plane.normal;
+	bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
+	if (inters)
+		r_normal = plane.normal;
 	return inters;
 }
 
@@ -130,16 +121,15 @@ Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {
 	return Vector3(); //wtf
 }
 
-void PlaneShapeSW::_setup(const Plane& p_plane)  {
+void PlaneShapeSW::_setup(const Plane &p_plane) {
 
-	plane=p_plane;
-	configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));
+	plane = p_plane;
+	configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
 }
 
-void PlaneShapeSW::set_data(const Variant& p_data) {
+void PlaneShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
-
 }
 
 Variant PlaneShapeSW::get_data() const {
@@ -147,9 +137,7 @@ Variant PlaneShapeSW::get_data() const {
 	return plane;
 }
 
-PlaneShapeSW::PlaneShapeSW()  {
-
-
+PlaneShapeSW::PlaneShapeSW() {
 }
 
 //
@@ -159,39 +147,39 @@ float RayShapeSW::get_length() const {
 	return length;
 }
 
-void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void RayShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 	// don't think this will be even used
-	r_min=0;
-	r_max=1;
+	r_min = 0;
+	r_max = 1;
 }
 
-Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const {
 
-	if (p_normal.z>0)
-		return Vector3(0,0,length);
+	if (p_normal.z > 0)
+		return Vector3(0, 0, length);
 	else
-		return Vector3(0,0,0);
+		return Vector3(0, 0, 0);
 }
 
-void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
 	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
 
-		r_amount=2;
-		r_supports[0]=Vector3(0,0,0);
-		r_supports[1]=Vector3(0,0,length);
-	} if (p_normal.z>0) {
-		r_amount=1;
-		*r_supports=Vector3(0,0,length);
+		r_amount = 2;
+		r_supports[0] = Vector3(0, 0, 0);
+		r_supports[1] = Vector3(0, 0, length);
+	}
+	if (p_normal.z > 0) {
+		r_amount = 1;
+		*r_supports = Vector3(0, 0, length);
 	} else {
-		r_amount=1;
-		*r_supports=Vector3(0,0,0);
+		r_amount = 1;
+		*r_supports = Vector3(0, 0, 0);
 	}
 }
 
-
-bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
 	return false; //simply not possible
 }
@@ -201,16 +189,15 @@ Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {
 	return Vector3();
 }
 
-void RayShapeSW::_setup(float p_length)  {
+void RayShapeSW::_setup(float p_length) {
 
-	length=p_length;
-	configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));
+	length = p_length;
+	configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
 }
 
-void RayShapeSW::set_data(const Variant& p_data) {
+void RayShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
-
 }
 
 Variant RayShapeSW::get_data() const {
@@ -218,13 +205,11 @@ Variant RayShapeSW::get_data() const {
 	return length;
 }
 
-RayShapeSW::RayShapeSW()  {
+RayShapeSW::RayShapeSW() {
 
-	length=1;
+	length = 1;
 }
 
-
-
 /********** SPHERE *************/
 
 real_t SphereShapeSW::get_radius() const {
@@ -232,50 +217,47 @@ real_t SphereShapeSW::get_radius() const {
 	return radius;
 }
 
-void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void SphereShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
-	float d = p_normal.dot( p_transform.origin );
+	float d = p_normal.dot(p_transform.origin);
 
 	// figure out scale at point
 	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
 	float scale = local_normal.length();
 
-	r_min = d - (radius) * scale;
-	r_max = d + (radius) * scale;
-
+	r_min = d - (radius)*scale;
+	r_max = d + (radius)*scale;
 }
 
-Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const {
 
-	return p_normal*radius;
+	return p_normal * radius;
 }
 
-void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
-	*r_supports=p_normal*radius;
-	r_amount=1;
+	*r_supports = p_normal * radius;
+	r_amount = 1;
 }
 
-bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-	return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);
+	return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
 }
 
 Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {
 
 	float s = 0.4 * p_mass * radius * radius;
-	return Vector3(s,s,s);
+	return Vector3(s, s, s);
 }
 
 void SphereShapeSW::_setup(real_t p_radius) {
 
-
-	radius=p_radius;
-	configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));
-
+	radius = p_radius;
+	configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
 }
 
-void SphereShapeSW::set_data(const Variant& p_data) {
+void SphereShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
 }
@@ -287,113 +269,105 @@ Variant SphereShapeSW::get_data() const {
 
 SphereShapeSW::SphereShapeSW() {
 
-	radius=0;
+	radius = 0;
 }
 
-
 /********** BOX *************/
 
-
-void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void BoxShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 	// no matter the angle, the box is mirrored anyway
-	Vector3 local_normal=p_transform.basis.xform_inv(p_normal);
+	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
 
 	float length = local_normal.abs().dot(half_extents);
-	float distance = p_normal.dot( p_transform.origin );
+	float distance = p_normal.dot(p_transform.origin);
 
 	r_min = distance - length;
 	r_max = distance + length;
-
-
 }
 
-Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {
-
+Vector3 BoxShapeSW::get_support(const Vector3 &p_normal) const {
 
 	Vector3 point(
-		(p_normal.x<0) ? -half_extents.x : half_extents.x,
-		(p_normal.y<0) ? -half_extents.y : half_extents.y,
-		(p_normal.z<0) ? -half_extents.z : half_extents.z
-	);
+			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
+			(p_normal.y < 0) ? -half_extents.y : half_extents.y,
+			(p_normal.z < 0) ? -half_extents.z : half_extents.z);
 
 	return point;
 }
 
-void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
-	static const int next[3]={1,2,0};
-	static const int next2[3]={2,0,1};
+	static const int next[3] = { 1, 2, 0 };
+	static const int next2[3] = { 2, 0, 1 };
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis;
-		axis[i]=1.0;
-		float dot = p_normal.dot( axis );
-		if ( Math::abs( dot ) >  _FACE_IS_VALID_SUPPORT_TRESHOLD ) {
+		axis[i] = 1.0;
+		float dot = p_normal.dot(axis);
+		if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
 
 			//Vector3 axis_b;
 
-			bool neg = dot<0;
+			bool neg = dot < 0;
 			r_amount = 4;
 
 			Vector3 point;
-			point[i]=half_extents[i];
+			point[i] = half_extents[i];
 
-			int i_n=next[i];
-			int i_n2=next2[i];
+			int i_n = next[i];
+			int i_n2 = next2[i];
 
-			static const float sign[4][2]={
+			static const float sign[4][2] = {
 
-				{-1.0, 1.0},
-				{ 1.0, 1.0},
-				{ 1.0,-1.0},
-				{-1.0,-1.0},
+				{ -1.0, 1.0 },
+				{ 1.0, 1.0 },
+				{ 1.0, -1.0 },
+				{ -1.0, -1.0 },
 			};
 
-			for (int j=0;j<4;j++) {
-
-				point[i_n]=sign[j][0]*half_extents[i_n];
-				point[i_n2]=sign[j][1]*half_extents[i_n2];
-				r_supports[j]=neg?-point:point;
+			for (int j = 0; j < 4; j++) {
 
+				point[i_n] = sign[j][0] * half_extents[i_n];
+				point[i_n2] = sign[j][1] * half_extents[i_n2];
+				r_supports[j] = neg ? -point : point;
 			}
 
 			if (neg) {
-				SWAP( r_supports[1], r_supports[2] );
-				SWAP( r_supports[0], r_supports[3] );
+				SWAP(r_supports[1], r_supports[2]);
+				SWAP(r_supports[0], r_supports[3]);
 			}
 
 			return;
 		}
 
-		r_amount=0;
-
+		r_amount = 0;
 	}
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
 		Vector3 axis;
-		axis[i]=1.0;
+		axis[i] = 1.0;
 
-		if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+		if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
 
-			r_amount= 2;
+			r_amount = 2;
 
-			int i_n=next[i];
-			int i_n2=next2[i];
+			int i_n = next[i];
+			int i_n2 = next2[i];
 
-			Vector3 point=half_extents;
+			Vector3 point = half_extents;
 
-			if (p_normal[i_n]<0) {
-				point[i_n]=-point[i_n];
+			if (p_normal[i_n] < 0) {
+				point[i_n] = -point[i_n];
 			}
-			if (p_normal[i_n2]<0) {
-				point[i_n2]=-point[i_n2];
+			if (p_normal[i_n2] < 0) {
+				point[i_n2] = -point[i_n2];
 			}
 
 			r_supports[0] = point;
-			point[i]=-point[i];
+			point[i] = -point[i];
 			r_supports[1] = point;
 			return;
 		}
@@ -401,44 +375,38 @@ void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_suppo
 	/* USE POINT */
 
 	Vector3 point(
-		(p_normal.x<0) ? -half_extents.x : half_extents.x,
-		(p_normal.y<0) ? -half_extents.y : half_extents.y,
-		(p_normal.z<0) ? -half_extents.z : half_extents.z
-	);
+			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
+			(p_normal.y < 0) ? -half_extents.y : half_extents.y,
+			(p_normal.z < 0) ? -half_extents.z : half_extents.z);
 
-	r_amount=1;
-	r_supports[0]=point;
+	r_amount = 1;
+	r_supports[0] = point;
 }
 
-bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-	AABB aabb(-half_extents,half_extents*2.0);
-
-	return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);
+	AABB aabb(-half_extents, half_extents * 2.0);
 
+	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
 }
 
 Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {
 
-	float lx=half_extents.x;
-	float ly=half_extents.y;
-	float lz=half_extents.z;
-
-	return Vector3(	(p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );
+	float lx = half_extents.x;
+	float ly = half_extents.y;
+	float lz = half_extents.z;
 
+	return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly));
 }
 
-void BoxShapeSW::_setup(const Vector3& p_half_extents)  {
-
-	half_extents=p_half_extents.abs();
-
-	configure(AABB(-half_extents,half_extents*2));
+void BoxShapeSW::_setup(const Vector3 &p_half_extents) {
 
+	half_extents = p_half_extents.abs();
 
+	configure(AABB(-half_extents, half_extents * 2));
 }
 
-void BoxShapeSW::set_data(const Variant& p_data) {
-
+void BoxShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
 }
@@ -448,138 +416,128 @@ Variant BoxShapeSW::get_data() const {
 	return half_extents;
 }
 
-BoxShapeSW::BoxShapeSW()  {
-
-
+BoxShapeSW::BoxShapeSW() {
 }
 
-
 /********** CAPSULE *************/
 
+void CapsuleShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
-void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
-
-	Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();
+	Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
 	float h = (n.z > 0) ? height : -height;
 
 	n *= radius;
 	n.z += h * 0.5;
 
-	r_max=p_normal.dot(p_transform.xform(n));
-	r_min=p_normal.dot(p_transform.xform(-n));
+	r_max = p_normal.dot(p_transform.xform(n));
+	r_min = p_normal.dot(p_transform.xform(-n));
 	return;
 
 	n = p_transform.basis.xform(n);
 
-	float distance = p_normal.dot( p_transform.origin );
+	float distance = p_normal.dot(p_transform.origin);
 	float length = Math::abs(p_normal.dot(n));
 	r_min = distance - length;
 	r_max = distance + length;
 
-	ERR_FAIL_COND( r_max < r_min );
-
+	ERR_FAIL_COND(r_max < r_min);
 }
 
-Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const {
 
-	Vector3 n=p_normal;
+	Vector3 n = p_normal;
 
 	float h = (n.z > 0) ? height : -height;
 
-	n*=radius;
-	n.z += h*0.5;
+	n *= radius;
+	n.z += h * 0.5;
 	return n;
 }
 
-void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
-
-	Vector3 n=p_normal;
+	Vector3 n = p_normal;
 
 	float d = n.z;
 
-	if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
+	if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
 
 		// make it flat
-		n.z=0.0;
+		n.z = 0.0;
 		n.normalize();
-		n*=radius;
+		n *= radius;
 
-		r_amount=2;
-		r_supports[0]=n;
-		r_supports[0].z+=height*0.5;
-		r_supports[1]=n;
-		r_supports[1].z-=height*0.5;
+		r_amount = 2;
+		r_supports[0] = n;
+		r_supports[0].z += height * 0.5;
+		r_supports[1] = n;
+		r_supports[1].z -= height * 0.5;
 
 	} else {
 
 		float h = (d > 0) ? height : -height;
 
-		n*=radius;
-		n.z += h*0.5;
-		r_amount=1;
-		*r_supports=n;
-
+		n *= radius;
+		n.z += h * 0.5;
+		r_amount = 1;
+		*r_supports = n;
 	}
-
 }
 
+bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
-
-	Vector3 norm=(p_end-p_begin).normalized();
-	float min_d=1e20;
-
+	Vector3 norm = (p_end - p_begin).normalized();
+	float min_d = 1e20;
 
-	Vector3 res,n;
-	bool collision=false;
+	Vector3 res, n;
+	bool collision = false;
 
-	Vector3 auxres,auxn;
+	Vector3 auxres, auxn;
 	bool collided;
 
 	// test against cylinder and spheres :-|
 
-	collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);
+	collided = Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn);
 
 	if (collided) {
-		float d=norm.dot(auxres);
-		if (d<min_d) {
-			min_d=d;
-			res=auxres;
-			n=auxn;
-			collision=true;
+		float d = norm.dot(auxres);
+		if (d < min_d) {
+			min_d = d;
+			res = auxres;
+			n = auxn;
+			collision = true;
 		}
 	}
 
-	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);
+	collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn);
 
 	if (collided) {
-		float d=norm.dot(auxres);
-		if (d<min_d) {
-			min_d=d;
-			res=auxres;
-			n=auxn;
-			collision=true;
+		float d = norm.dot(auxres);
+		if (d < min_d) {
+			min_d = d;
+			res = auxres;
+			n = auxn;
+			collision = true;
 		}
 	}
 
-	collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);
+	collided = Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn);
 
 	if (collided) {
-		float d=norm.dot(auxres);
+		float d = norm.dot(auxres);
 
-		if (d<min_d) {
-			min_d=d;
-			res=auxres;
-			n=auxn;
-			collision=true;
+		if (d < min_d) {
+			min_d = d;
+			res = auxres;
+			n = auxn;
+			collision = true;
 		}
 	}
 
 	if (collision) {
 
-		r_result=res;
-		r_normal=n;
+		r_result = res;
+		r_normal = n;
 	}
 	return collision;
 }
@@ -587,105 +545,90 @@ bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_e
 Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {
 
 	// use crappy AABB approximation
-	Vector3 extents=get_aabb().size*0.5;
+	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
-	);
-
+			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
 }
 
+void CapsuleShapeSW::_setup(real_t p_height, real_t p_radius) {
 
-
-
-void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius)  {
-
-	height=p_height;
-	radius=p_radius;
-	configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));
-
+	height = p_height;
+	radius = p_radius;
+	configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0)));
 }
 
-void CapsuleShapeSW::set_data(const Variant& p_data) {
+void CapsuleShapeSW::set_data(const Variant &p_data) {
 
 	Dictionary d = p_data;
 	ERR_FAIL_COND(!d.has("radius"));
 	ERR_FAIL_COND(!d.has("height"));
-	_setup(d["height"],d["radius"]);
-
+	_setup(d["height"], d["radius"]);
 }
 
 Variant CapsuleShapeSW::get_data() const {
 
 	Dictionary d;
-	d["radius"]=radius;
-	d["height"]=height;
+	d["radius"] = radius;
+	d["height"] = height;
 	return d;
-
 }
 
+CapsuleShapeSW::CapsuleShapeSW() {
 
-CapsuleShapeSW::CapsuleShapeSW()  {
-
-	height=radius=0;
-
+	height = radius = 0;
 }
 
 /********** CONVEX POLYGON *************/
 
+void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
-void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
-
-
-	int vertex_count=mesh.vertices.size();
-	if (vertex_count==0)
+	int vertex_count = mesh.vertices.size();
+	if (vertex_count == 0)
 		return;
 
-	const Vector3 *vrts=&mesh.vertices[0];
+	const Vector3 *vrts = &mesh.vertices[0];
 
-	for (int i=0;i<vertex_count;i++) {
+	for (int i = 0; i < vertex_count; i++) {
 
-		float d=p_normal.dot( p_transform.xform( vrts[i] ) );
+		float d = p_normal.dot(p_transform.xform(vrts[i]));
 
-		if (i==0 || d > r_max)
-			r_max=d;
-		if (i==0 || d < r_min)
-			r_min=d;
+		if (i == 0 || d > r_max)
+			r_max = d;
+		if (i == 0 || d < r_min)
+			r_min = d;
 	}
 }
 
-Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {
+Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const {
 
-	Vector3 n=p_normal;
+	Vector3 n = p_normal;
 
-	int vert_support_idx=-1;
+	int vert_support_idx = -1;
 	float support_max;
 
-	int vertex_count=mesh.vertices.size();
-	if (vertex_count==0)
+	int vertex_count = mesh.vertices.size();
+	if (vertex_count == 0)
 		return Vector3();
 
-	const Vector3 *vrts=&mesh.vertices[0];
+	const Vector3 *vrts = &mesh.vertices[0];
 
-	for (int i=0;i<vertex_count;i++) {
+	for (int i = 0; i < vertex_count; i++) {
 
-		float d=n.dot(vrts[i]);
+		float d = n.dot(vrts[i]);
 
-		if (i==0 || d > support_max) {
-			support_max=d;
-			vert_support_idx=i;
+		if (i == 0 || d > support_max) {
+			support_max = d;
+			vert_support_idx = i;
 		}
 	}
 
-	return  vrts[vert_support_idx];
-
+	return vrts[vert_support_idx];
 }
 
-
-
-void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
 	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
@@ -700,28 +643,27 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector
 	real_t max;
 	int vtx;
 
-	for (int i=0;i<vc;i++) {
+	for (int i = 0; i < vc; i++) {
 
-		float d=p_normal.dot(vertices[i]);
+		float d = p_normal.dot(vertices[i]);
 
-		if (i==0 || d > max) {
-			max=d;
-			vtx=i;
+		if (i == 0 || d > max) {
+			max = d;
+			vtx = i;
 		}
 	}
 
+	for (int i = 0; i < fc; i++) {
 
-	for(int i=0;i<fc;i++) {
-
-		if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {
+		if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
 
 			int ic = faces[i].indices.size();
-			const int *ind=faces[i].indices.ptr();
+			const int *ind = faces[i].indices.ptr();
 
-			bool valid=false;
-			for(int j=0;j<ic;j++) {
-				if (ind[j]==vtx) {
-					valid=true;
+			bool valid = false;
+			for (int j = 0; j < ic; j++) {
+				if (ind[j] == vtx) {
+					valid = true;
 					break;
 				}
 			}
@@ -729,114 +671,103 @@ void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector
 			if (!valid)
 				continue;
 
-			int m = MIN(p_max,ic);
-			for(int j=0;j<m;j++) {
+			int m = MIN(p_max, ic);
+			for (int j = 0; j < m; j++) {
 
-				r_supports[j]=vertices[ind[j]];
+				r_supports[j] = vertices[ind[j]];
 			}
-			r_amount=m;
+			r_amount = m;
 			return;
 		}
 	}
 
-	for(int i=0;i<ec;i++) {
-
+	for (int i = 0; i < ec; i++) {
 
-		float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);
-		dot=ABS(dot);
-		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {
+		float dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
+		dot = ABS(dot);
+		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) {
 
-			r_amount=2;
-			r_supports[0]=vertices[edges[i].a];
-			r_supports[1]=vertices[edges[i].b];
+			r_amount = 2;
+			r_supports[0] = vertices[edges[i].a];
+			r_supports[1] = vertices[edges[i].b];
 			return;
 		}
 	}
 
-
-	r_supports[0]=vertices[vtx];
-	r_amount=1;
+	r_supports[0] = vertices[vtx];
+	r_amount = 1;
 }
 
-bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
-
-
+bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
 	const Geometry::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 
 	const Vector3 *vertices = mesh.vertices.ptr();
 
-	Vector3 n = p_end-p_begin;
+	Vector3 n = p_end - p_begin;
 	float min = 1e20;
-	bool col=false;
+	bool col = false;
 
-	for(int i=0;i<fc;i++) {
+	for (int i = 0; i < fc; i++) {
 
 		if (faces[i].plane.normal.dot(n) > 0)
 			continue; //opposing face
 
 		int ic = faces[i].indices.size();
-		const int *ind=faces[i].indices.ptr();
+		const int *ind = faces[i].indices.ptr();
 
-		for(int j=1;j<ic-1;j++) {
+		for (int j = 1; j < ic - 1; j++) {
 
-			Face3 f(vertices[ind[0]],vertices[ind[j]],vertices[ind[j+1]]);
+			Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]);
 			Vector3 result;
-			if (f.intersects_segment(p_begin,p_end,&result)) {
+			if (f.intersects_segment(p_begin, p_end, &result)) {
 				float d = n.dot(result);
-				if (d<min) {
-					min=d;
-					r_result=result;
-					r_normal=faces[i].plane.normal;
-					col=true;
+				if (d < min) {
+					min = d;
+					r_result = result;
+					r_normal = faces[i].plane.normal;
+					col = true;
 				}
 
 				break;
 			}
-
 		}
 	}
 
 	return col;
-
 }
 
 Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {
 
 	// use crappy AABB approximation
-	Vector3 extents=get_aabb().size*0.5;
-
-	return  Vector3(
-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
-	);
+	Vector3 extents = get_aabb().size * 0.5;
 
+	return Vector3(
+			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
 }
 
-void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices)  {
+void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) {
 
-	Error err = QuickHull::build(p_vertices,mesh);
+	Error err = QuickHull::build(p_vertices, mesh);
 	AABB _aabb;
 
-	for(int i=0;i<mesh.vertices.size();i++) {
+	for (int i = 0; i < mesh.vertices.size(); i++) {
 
-		if (i==0)
-			_aabb.pos=mesh.vertices[i];
+		if (i == 0)
+			_aabb.pos = mesh.vertices[i];
 		else
 			_aabb.expand_to(mesh.vertices[i]);
 	}
 
 	configure(_aabb);
-
-
 }
 
-void ConvexPolygonShapeSW::set_data(const Variant& p_data) {
+void ConvexPolygonShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
-
 }
 
 Variant ConvexPolygonShapeSW::get_data() const {
@@ -844,113 +775,105 @@ Variant ConvexPolygonShapeSW::get_data() const {
 	return mesh.vertices;
 }
 
-
-ConvexPolygonShapeSW::ConvexPolygonShapeSW()  {
-
-
+ConvexPolygonShapeSW::ConvexPolygonShapeSW() {
 }
 
-
 /********** FACE POLYGON *************/
 
+void FaceShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
-void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
-
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		Vector3 v=p_transform.xform(vertex[i]);
-		float d=p_normal.dot(v);
+		Vector3 v = p_transform.xform(vertex[i]);
+		float d = p_normal.dot(v);
 
-		if (i==0 || d > r_max)
-			r_max=d;
+		if (i == 0 || d > r_max)
+			r_max = d;
 
-		if (i==0 || d < r_min)
-			r_min=d;
+		if (i == 0 || d < r_min)
+			r_min = d;
 	}
 }
 
-Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {
-
+Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const {
 
-	int vert_support_idx=-1;
+	int vert_support_idx = -1;
 	float support_max;
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		float d=p_normal.dot(vertex[i]);
+		float d = p_normal.dot(vertex[i]);
 
-		if (i==0 || d > support_max) {
-			support_max=d;
-			vert_support_idx=i;
+		if (i == 0 || d > support_max) {
+			support_max = d;
+			vert_support_idx = i;
 		}
 	}
 
 	return vertex[vert_support_idx];
 }
 
-void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
 
-	Vector3 n=p_normal;
+	Vector3 n = p_normal;
 
 	/** TEST FACE AS SUPPORT **/
 	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
 
-		r_amount=3;
-		for (int i=0;i<3;i++) {
+		r_amount = 3;
+		for (int i = 0; i < 3; i++) {
 
-			r_supports[i]=vertex[i];
+			r_supports[i] = vertex[i];
 		}
 		return;
-
 	}
 
 	/** FIND SUPPORT VERTEX **/
 
-	int vert_support_idx=-1;
+	int vert_support_idx = -1;
 	float support_max;
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		float d=n.dot(vertex[i]);
+		float d = n.dot(vertex[i]);
 
-		if (i==0 || d > support_max) {
-			support_max=d;
-			vert_support_idx=i;
+		if (i == 0 || d > support_max) {
+			support_max = d;
+			vert_support_idx = i;
 		}
 	}
 
 	/** TEST EDGES AS SUPPORT **/
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		int nx=(i+1)%3;
-		if (i!=vert_support_idx && nx!=vert_support_idx)
+		int nx = (i + 1) % 3;
+		if (i != vert_support_idx && nx != vert_support_idx)
 			continue;
 
-	// check if edge is valid as a support
-		float dot=(vertex[i]-vertex[nx]).normalized().dot(n);
-		dot=ABS(dot);
+		// check if edge is valid as a support
+		float dot = (vertex[i] - vertex[nx]).normalized().dot(n);
+		dot = ABS(dot);
 		if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
 
-			r_amount=2;
-			r_supports[0]=vertex[i];
-			r_supports[1]=vertex[nx];
+			r_amount = 2;
+			r_supports[0] = vertex[i];
+			r_supports[1] = vertex[nx];
 			return;
 		}
 	}
 
-	r_amount=1;
-	r_supports[0]=vertex[vert_support_idx];
+	r_amount = 1;
+	r_supports[0] = vertex[vert_support_idx];
 }
 
-bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
-
+bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-	bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);
+	bool c = Geometry::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
 	if (c) {
-		r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;
-		if (r_normal.dot(p_end-p_begin)>0) {
-			r_normal=-r_normal;
+		r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
+		if (r_normal.dot(p_end - p_begin) > 0) {
+			r_normal = -r_normal;
 		}
 	}
 
@@ -960,181 +883,160 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,
 Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {
 
 	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
-
 }
 
-FaceShapeSW::FaceShapeSW()  {
+FaceShapeSW::FaceShapeSW() {
 
 	configure(AABB());
-
 }
 
-
-
 DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
 
-
 	DVector<Vector3> rfaces;
-	rfaces.resize(faces.size()*3);
+	rfaces.resize(faces.size() * 3);
 
-	for(int i=0;i<faces.size();i++) {
+	for (int i = 0; i < faces.size(); i++) {
 
-		Face f=faces.get(i);
+		Face f = faces.get(i);
 
-		for(int j=0;j<3;j++) {
+		for (int j = 0; j < 3; j++) {
 
-			rfaces.set(i*3+j, vertices.get( f.indices[j] ) );
+			rfaces.set(i * 3 + j, vertices.get(f.indices[j]));
 		}
 	}
 
 	return rfaces;
 }
 
-void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void ConcavePolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
-	int count=vertices.size();
-	if (count==0) {
-		r_min=0;
-		r_max=0;
+	int count = vertices.size();
+	if (count == 0) {
+		r_min = 0;
+		r_max = 0;
 		return;
 	}
-	DVector<Vector3>::Read r=vertices.read();
-	const Vector3 *vptr=r.ptr();
-
-	for (int i=0;i<count;i++) {
+	DVector<Vector3>::Read r = vertices.read();
+	const Vector3 *vptr = r.ptr();
 
-		float d=p_normal.dot( p_transform.xform( vptr[i] ) );
+	for (int i = 0; i < count; i++) {
 
-		if (i==0 || d > r_max)
-			r_max=d;
-		if (i==0 || d < r_min)
-			r_min=d;
+		float d = p_normal.dot(p_transform.xform(vptr[i]));
 
+		if (i == 0 || d > r_max)
+			r_max = d;
+		if (i == 0 || d < r_min)
+			r_min = d;
 	}
 }
 
-Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {
-
+Vector3 ConcavePolygonShapeSW::get_support(const Vector3 &p_normal) const {
 
-	int count=vertices.size();
-	if (count==0)
+	int count = vertices.size();
+	if (count == 0)
 		return Vector3();
 
-	DVector<Vector3>::Read r=vertices.read();
-	const Vector3 *vptr=r.ptr();
+	DVector<Vector3>::Read r = vertices.read();
+	const Vector3 *vptr = r.ptr();
 
-	Vector3 n=p_normal;
+	Vector3 n = p_normal;
 
-	int vert_support_idx=-1;
+	int vert_support_idx = -1;
 	float support_max;
 
-	for (int i=0;i<count;i++) {
+	for (int i = 0; i < count; i++) {
 
-		float d=n.dot(vptr[i]);
+		float d = n.dot(vptr[i]);
 
-		if (i==0 || d > support_max) {
-			support_max=d;
-			vert_support_idx=i;
+		if (i == 0 || d > support_max) {
+			support_max = d;
+			vert_support_idx = i;
 		}
 	}
 
-
 	return vptr[vert_support_idx];
-
 }
 
-void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {
-
-	const BVH *bvh=&p_params->bvh[p_idx];
+void ConcavePolygonShapeSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
 
+	const BVH *bvh = &p_params->bvh[p_idx];
 
 	//if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
 	//	return; //test against whole AABB, which isn't very costly
 
-
 	//printf("addr: %p\n",bvh);
-	if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {
+	if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
 
 		return;
 	}
 
-
-	if (bvh->face_index>=0) {
-
+	if (bvh->face_index >= 0) {
 
 		Vector3 res;
-		Vector3 vertices[3]={
-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],
-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],
-			p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]
+		Vector3 vertices[3] = {
+			p_params->vertices[p_params->faces[bvh->face_index].indices[0]],
+			p_params->vertices[p_params->faces[bvh->face_index].indices[1]],
+			p_params->vertices[p_params->faces[bvh->face_index].indices[2]]
 		};
 
 		if (Geometry::segment_intersects_triangle(
-				p_params->from,
-				p_params->to,
-				vertices[0],
-				vertices[1],
-				vertices[2],
-				&res)) {
-
+					p_params->from,
+					p_params->to,
+					vertices[0],
+					vertices[1],
+					vertices[2],
+					&res)) {
 
-			float d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
+			float d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
 			//TODO, seems segmen/triangle intersection is broken :(
-			if (d>0 && d<p_params->min_d) {
+			if (d > 0 && d < p_params->min_d) {
 
-				p_params->min_d=d;
-				p_params->result=res;
-				p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;
-				if (p_params->normal.dot(p_params->dir)>0)
-					p_params->normal=-p_params->normal;
+				p_params->min_d = d;
+				p_params->result = res;
+				p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal;
+				if (p_params->normal.dot(p_params->dir) > 0)
+					p_params->normal = -p_params->normal;
 				p_params->collisions++;
 			}
-
 		}
 
-
-
 	} else {
 
-		if (bvh->left>=0)
-			_cull_segment(bvh->left,p_params);
-		if (bvh->right>=0)
-			_cull_segment(bvh->right,p_params);
-
-
+		if (bvh->left >= 0)
+			_cull_segment(bvh->left, p_params);
+		if (bvh->right >= 0)
+			_cull_segment(bvh->right, p_params);
 	}
 }
 
-bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-	if (faces.size()==0)
+	if (faces.size() == 0)
 		return false;
 
 	// unlock data
-	DVector<Face>::Read fr=faces.read();
-	DVector<Vector3>::Read vr=vertices.read();
-	DVector<BVH>::Read br=bvh.read();
-
+	DVector<Face>::Read fr = faces.read();
+	DVector<Vector3>::Read vr = vertices.read();
+	DVector<BVH>::Read br = bvh.read();
 
 	_SegmentCullParams params;
-	params.from=p_begin;
-	params.to=p_end;
-	params.collisions=0;
-	params.dir=(p_end-p_begin).normalized();
+	params.from = p_begin;
+	params.to = p_end;
+	params.collisions = 0;
+	params.dir = (p_end - p_begin).normalized();
 
-	params.faces=fr.ptr();
-	params.vertices=vr.ptr();
-	params.bvh=br.ptr();
+	params.faces = fr.ptr();
+	params.vertices = vr.ptr();
+	params.bvh = br.ptr();
 
-	params.min_d=1e20;
+	params.min_d = 1e20;
 	// cull
-	_cull_segment(0,&params);
-
-	if (params.collisions>0) {
+	_cull_segment(0, &params);
 
+	if (params.collisions > 0) {
 
-		r_result=params.result;
-		r_normal=params.normal;
+		r_result = params.result;
+		r_normal = params.normal;
 		return true;
 	} else {
 
@@ -1142,81 +1044,76 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vecto
 	}
 }
 
-void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {
+void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const {
 
-	const BVH* bvh=&p_params->bvh[p_idx];
+	const BVH *bvh = &p_params->bvh[p_idx];
 
-	if (!p_params->aabb.intersects( bvh->aabb ))
+	if (!p_params->aabb.intersects(bvh->aabb))
 		return;
 
-	if (bvh->face_index>=0) {
+	if (bvh->face_index >= 0) {
 
-		const Face *f=&p_params->faces[ bvh->face_index ];
-		FaceShapeSW *face=p_params->face;
-		face->normal=f->normal;
-		face->vertex[0]=p_params->vertices[f->indices[0]];
-		face->vertex[1]=p_params->vertices[f->indices[1]];
-		face->vertex[2]=p_params->vertices[f->indices[2]];
-		p_params->callback(p_params->userdata,face);
+		const Face *f = &p_params->faces[bvh->face_index];
+		FaceShapeSW *face = p_params->face;
+		face->normal = f->normal;
+		face->vertex[0] = p_params->vertices[f->indices[0]];
+		face->vertex[1] = p_params->vertices[f->indices[1]];
+		face->vertex[2] = p_params->vertices[f->indices[2]];
+		p_params->callback(p_params->userdata, face);
 
 	} else {
 
-		if (bvh->left>=0) {
-
-			_cull(bvh->left,p_params);
+		if (bvh->left >= 0) {
 
+			_cull(bvh->left, p_params);
 		}
 
-		if (bvh->right>=0) {
+		if (bvh->right >= 0) {
 
-			_cull(bvh->right,p_params);
+			_cull(bvh->right, p_params);
 		}
-
 	}
 }
 
-void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
+void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
 
 	// make matrix local to concave
-	if (faces.size()==0)
+	if (faces.size() == 0)
 		return;
 
-	AABB local_aabb=p_local_aabb;
+	AABB local_aabb = p_local_aabb;
 
 	// unlock data
-	DVector<Face>::Read fr=faces.read();
-	DVector<Vector3>::Read vr=vertices.read();
-	DVector<BVH>::Read br=bvh.read();
+	DVector<Face>::Read fr = faces.read();
+	DVector<Vector3>::Read vr = vertices.read();
+	DVector<BVH>::Read br = bvh.read();
 
 	FaceShapeSW face; // use this to send in the callback
 
 	_CullParams params;
-	params.aabb=local_aabb;
-	params.face=&face;
-	params.faces=fr.ptr();
-	params.vertices=vr.ptr();
-	params.bvh=br.ptr();
-	params.callback=p_callback;
-	params.userdata=p_userdata;
+	params.aabb = local_aabb;
+	params.face = &face;
+	params.faces = fr.ptr();
+	params.vertices = vr.ptr();
+	params.bvh = br.ptr();
+	params.callback = p_callback;
+	params.userdata = p_userdata;
 
 	// cull
-	_cull(0,&params);
-
+	_cull(0, &params);
 }
 
 Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {
 
 	// use crappy AABB approximation
-	Vector3 extents=get_aabb().size*0.5;
+	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
-	);
+			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
 }
 
-
 struct _VolumeSW_BVH_Element {
 
 	AABB aabb;
@@ -1226,26 +1123,25 @@ struct _VolumeSW_BVH_Element {
 
 struct _VolumeSW_BVH_CompareX {
 
-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
 
-		return a.center.x<b.center.x;
+		return a.center.x < b.center.x;
 	}
 };
 
-
 struct _VolumeSW_BVH_CompareY {
 
-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
 
-		return a.center.y<b.center.y;
+		return a.center.y < b.center.y;
 	}
 };
 
 struct _VolumeSW_BVH_CompareZ {
 
-	_FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
 
-		return a.center.z<b.center.z;
+		return a.center.z < b.center.z;
 	}
 };
 
@@ -1258,107 +1154,102 @@ struct _VolumeSW_BVH {
 	int face_index;
 };
 
+_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
 
-_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {
-
-	_VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );
+	_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
 
-	if (p_size==1) {
+	if (p_size == 1) {
 		//leaf
-		bvh->aabb=p_elements[0].aabb;
-		bvh->left=NULL;
-		bvh->right=NULL;
-		bvh->face_index=p_elements->face_index;
+		bvh->aabb = p_elements[0].aabb;
+		bvh->left = NULL;
+		bvh->right = NULL;
+		bvh->face_index = p_elements->face_index;
 		count++;
 		return bvh;
 	} else {
 
-		bvh->face_index=-1;
+		bvh->face_index = -1;
 	}
 
 	AABB aabb;
-	for(int i=0;i<p_size;i++) {
+	for (int i = 0; i < p_size; i++) {
 
-		if (i==0)
-			aabb=p_elements[i].aabb;
+		if (i == 0)
+			aabb = p_elements[i].aabb;
 		else
 			aabb.merge_with(p_elements[i].aabb);
 	}
-	bvh->aabb=aabb;
-	switch(aabb.get_longest_axis_index()) {
+	bvh->aabb = aabb;
+	switch (aabb.get_longest_axis_index()) {
 
 		case 0: {
 
-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;
-			sort_x.sort(p_elements,p_size);
+			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
+			sort_x.sort(p_elements, p_size);
 
 		} break;
 		case 1: {
 
-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;
-			sort_y.sort(p_elements,p_size);
+			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
+			sort_y.sort(p_elements, p_size);
 		} break;
 		case 2: {
 
-			SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;
-			sort_z.sort(p_elements,p_size);
+			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
+			sort_z.sort(p_elements, p_size);
 		} break;
 	}
 
-	int split=p_size/2;
-	bvh->left=_volume_sw_build_bvh(p_elements,split,count);
-	bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);
+	int split = p_size / 2;
+	bvh->left = _volume_sw_build_bvh(p_elements, split, count);
+	bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
 
-//	printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
+	//	printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
 	count++;
 	return bvh;
 }
 
+void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
 
-void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {
-
-	int idx=p_idx;
-
-
-	p_bvh_array[idx].aabb=p_bvh_tree->aabb;
-	p_bvh_array[idx].face_index=p_bvh_tree->face_index;
-//	printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
+	int idx = p_idx;
 
+	p_bvh_array[idx].aabb = p_bvh_tree->aabb;
+	p_bvh_array[idx].face_index = p_bvh_tree->face_index;
+	//	printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
 
 	if (p_bvh_tree->left) {
-		p_bvh_array[idx].left=++p_idx;
-		_fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);
+		p_bvh_array[idx].left = ++p_idx;
+		_fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx);
 
 	} else {
 
-		p_bvh_array[p_idx].left=-1;
+		p_bvh_array[p_idx].left = -1;
 	}
 
 	if (p_bvh_tree->right) {
-		p_bvh_array[idx].right=++p_idx;
-		_fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);
+		p_bvh_array[idx].right = ++p_idx;
+		_fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx);
 
 	} else {
 
-		p_bvh_array[p_idx].right=-1;
+		p_bvh_array[p_idx].right = -1;
 	}
 
 	memdelete(p_bvh_tree);
-
 }
 
 void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {
 
-	int src_face_count=p_faces.size();
-	if (src_face_count==0) {
+	int src_face_count = p_faces.size();
+	if (src_face_count == 0) {
 		configure(AABB());
 		return;
 	}
-	ERR_FAIL_COND(src_face_count%3);
-	src_face_count/=3;
+	ERR_FAIL_COND(src_face_count % 3);
+	src_face_count /= 3;
 
 	DVector<Vector3>::Read r = p_faces.read();
-	const Vector3 * facesr= r.ptr();
+	const Vector3 *facesr = r.ptr();
 
 #if 0
 	Map<Vector3,int> point_map;
@@ -1474,67 +1365,62 @@ void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {
 
 #else
 	DVector<_VolumeSW_BVH_Element> bvh_array;
-	bvh_array.resize( src_face_count );
+	bvh_array.resize(src_face_count);
 
 	DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
-	_VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
+	_VolumeSW_BVH_Element *bvh_arrayw = bvhw.ptr();
 
 	faces.resize(src_face_count);
 	DVector<Face>::Write w = faces.write();
-	Face *facesw=w.ptr();
+	Face *facesw = w.ptr();
 
-	vertices.resize( src_face_count*3 );
+	vertices.resize(src_face_count * 3);
 
 	DVector<Vector3>::Write vw = vertices.write();
-	Vector3 *verticesw=vw.ptr();
+	Vector3 *verticesw = vw.ptr();
 
 	AABB _aabb;
 
+	for (int i = 0; i < src_face_count; i++) {
 
-	for(int i=0;i<src_face_count;i++) {
-
-		Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );
+		Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
 
-		bvh_arrayw[i].aabb=face.get_aabb();
+		bvh_arrayw[i].aabb = face.get_aabb();
 		bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;
-		bvh_arrayw[i].face_index=i;
-		facesw[i].indices[0]=i*3+0;
-		facesw[i].indices[1]=i*3+1;
-		facesw[i].indices[2]=i*3+2;
-		facesw[i].normal=face.get_plane().normal;
-		verticesw[i*3+0]=face.vertex[0];
-		verticesw[i*3+1]=face.vertex[1];
-		verticesw[i*3+2]=face.vertex[2];
-		if (i==0)
-			_aabb=bvh_arrayw[i].aabb;
+		bvh_arrayw[i].face_index = i;
+		facesw[i].indices[0] = i * 3 + 0;
+		facesw[i].indices[1] = i * 3 + 1;
+		facesw[i].indices[2] = i * 3 + 2;
+		facesw[i].normal = face.get_plane().normal;
+		verticesw[i * 3 + 0] = face.vertex[0];
+		verticesw[i * 3 + 1] = face.vertex[1];
+		verticesw[i * 3 + 2] = face.vertex[2];
+		if (i == 0)
+			_aabb = bvh_arrayw[i].aabb;
 		else
 			_aabb.merge_with(bvh_arrayw[i].aabb);
-
 	}
 
-	w=DVector<Face>::Write();
-	vw=DVector<Vector3>::Write();
+	w = DVector<Face>::Write();
+	vw = DVector<Vector3>::Write();
 
-	int count=0;
-	_VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);
+	int count = 0;
+	_VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
 
-	bvh.resize( count+1 );
+	bvh.resize(count + 1);
 
 	DVector<BVH>::Write bvhw2 = bvh.write();
-	BVH*bvh_arrayw2=bvhw2.ptr();
+	BVH *bvh_arrayw2 = bvhw2.ptr();
 
-	int idx=0;
-	_fill_bvh(bvh_tree,bvh_arrayw2,idx);
+	int idx = 0;
+	_fill_bvh(bvh_tree, bvh_arrayw2, idx);
 
 	configure(_aabb); // this type of shape has no margin
 
-
 #endif
 }
 
-
-void ConcavePolygonShapeSW::set_data(const Variant& p_data) {
-
+void ConcavePolygonShapeSW::set_data(const Variant &p_data) {
 
 	_setup(p_data);
 }
@@ -1545,12 +1431,8 @@ Variant ConcavePolygonShapeSW::get_data() const {
 }
 
 ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
-
-
 }
 
-
-
 /* HEIGHT MAP SHAPE */
 
 DVector<float> HeightMapShapeSW::get_heights() const {
@@ -1570,114 +1452,94 @@ float HeightMapShapeSW::get_cell_size() const {
 	return cell_size;
 }
 
-
-void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+void HeightMapShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 	//not very useful, but not very used either
-	p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );
-
+	p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
 }
 
-Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {
-
+Vector3 HeightMapShapeSW::get_support(const Vector3 &p_normal) const {
 
 	//not very useful, but not very used either
 	return get_aabb().get_support(p_normal);
-
 }
 
-bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {
-
+bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
 
 	return false;
 }
 
-
-void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
-
-
-
+void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
 }
 
-
 Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {
 
-
 	// use crappy AABB approximation
-	Vector3 extents=get_aabb().size*0.5;
+	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
-		(p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
-		(p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
-		(p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
-	);
+			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
 }
 
+void HeightMapShapeSW::_setup(DVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {
 
-void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {
+	heights = p_heights;
+	width = p_width;
+	depth = p_depth;
+	cell_size = p_cell_size;
 
-	heights=p_heights;
-	width=p_width;
-	depth=p_depth;
-	cell_size=p_cell_size;
-
-	DVector<real_t>::Read r = heights. read();
+	DVector<real_t>::Read r = heights.read();
 
 	AABB aabb;
 
-	for(int i=0;i<depth;i++) {
+	for (int i = 0; i < depth; i++) {
 
-		for(int j=0;j<width;j++) {
+		for (int j = 0; j < width; j++) {
 
-			float h = r[i*width+j];
+			float h = r[i * width + j];
 
-			Vector3 pos( j*cell_size, h, i*cell_size );
-			if (i==0 || j==0)
-				aabb.pos=pos;
+			Vector3 pos(j * cell_size, h, i * cell_size);
+			if (i == 0 || j == 0)
+				aabb.pos = pos;
 			else
 				aabb.expand_to(pos);
-
 		}
 	}
 
-
 	configure(aabb);
 }
 
-void HeightMapShapeSW::set_data(const Variant& p_data) {
+void HeightMapShapeSW::set_data(const Variant &p_data) {
 
-	ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );
-	Dictionary d=p_data;
-	ERR_FAIL_COND( !d.has("width") );
-	ERR_FAIL_COND( !d.has("depth") );
-	ERR_FAIL_COND( !d.has("cell_size") );
-	ERR_FAIL_COND( !d.has("heights") );
-
-	int width=d["width"];
-	int depth=d["depth"];
-	float cell_size=d["cell_size"];
-	DVector<float> heights=d["heights"];
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("width"));
+	ERR_FAIL_COND(!d.has("depth"));
+	ERR_FAIL_COND(!d.has("cell_size"));
+	ERR_FAIL_COND(!d.has("heights"));
 
-	ERR_FAIL_COND( width<= 0);
-	ERR_FAIL_COND( depth<= 0);
-	ERR_FAIL_COND( cell_size<= CMP_EPSILON);
-	ERR_FAIL_COND( heights.size() != (width*depth) );
-	_setup(heights, width, depth, cell_size );
+	int width = d["width"];
+	int depth = d["depth"];
+	float cell_size = d["cell_size"];
+	DVector<float> heights = d["heights"];
 
+	ERR_FAIL_COND(width <= 0);
+	ERR_FAIL_COND(depth <= 0);
+	ERR_FAIL_COND(cell_size <= CMP_EPSILON);
+	ERR_FAIL_COND(heights.size() != (width * depth));
+	_setup(heights, width, depth, cell_size);
 }
 
 Variant HeightMapShapeSW::get_data() const {
 
 	ERR_FAIL_V(Variant());
-
 }
 
 HeightMapShapeSW::HeightMapShapeSW() {
 
-	width=0;
-	depth=0;
-	cell_size=0;
+	width = 0;
+	depth = 0;
+	cell_size = 0;
 }
-
-
-
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h
index 5328a0b97..17a33bc04 100644
--- a/servers/physics/shape_sw.h
+++ b/servers/physics/shape_sw.h
@@ -29,9 +29,9 @@
 #ifndef SHAPE_SW_H
 #define SHAPE_SW_H
 
-#include "servers/physics_server.h"
 #include "bsp_tree.h"
 #include "geometry.h"
+#include "servers/physics_server.h"
 /*
 
 SHAPE_LINE, ///< plane:"plane"
@@ -48,14 +48,12 @@ class ShapeSW;
 
 class ShapeOwnerSW {
 public:
-
-	virtual void _shape_changed()=0;
-	virtual void remove_shape(ShapeSW *p_shape)=0;
+	virtual void _shape_changed() = 0;
+	virtual void remove_shape(ShapeSW *p_shape) = 0;
 
 	virtual ~ShapeOwnerSW() {}
 };
 
-
 class ShapeSW {
 
 	RID self;
@@ -63,58 +61,56 @@ class ShapeSW {
 	bool configured;
 	real_t custom_bias;
 
-	Map<ShapeOwnerSW*,int> owners;
+	Map<ShapeOwnerSW *, int> owners;
+
 protected:
+	void configure(const AABB &p_aabb);
 
-	void configure(const AABB& p_aabb);
 public:
-
 	enum {
-		MAX_SUPPORTS=8
+		MAX_SUPPORTS = 8
 	};
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
-	_FORCE_INLINE_ RID get_self() const {return  self; }
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	virtual PhysicsServer::ShapeType get_type() const=0;
+	virtual PhysicsServer::ShapeType get_type() const = 0;
 
 	_FORCE_INLINE_ AABB get_aabb() const { return aabb; }
 	_FORCE_INLINE_ bool is_configured() const { return configured; }
 
 	virtual bool is_concave() const { return false; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0;
 
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
-	virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0;
+	virtual Vector3 get_moment_of_inertia(float p_mass) const = 0;
 
-	virtual void set_data(const Variant& p_data)=0;
-	virtual Variant get_data() const=0;
+	virtual void set_data(const Variant &p_data) = 0;
+	virtual Variant get_data() const = 0;
 
-	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
 
 	void add_owner(ShapeOwnerSW *p_owner);
 	void remove_owner(ShapeOwnerSW *p_owner);
 	bool is_owner(ShapeOwnerSW *p_owner) const;
-	const Map<ShapeOwnerSW*,int>& get_owners() const;
+	const Map<ShapeOwnerSW *, int> &get_owners() const;
 
 	ShapeSW();
 	virtual ~ShapeSW();
 };
 
-
 class ConcaveShapeSW : public ShapeSW {
 
 public:
-
 	virtual bool is_concave() const { return true; }
-	typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+	typedef void (*Callback)(void *p_userdata, ShapeSW *p_convex);
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 
-	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
 
 	ConcaveShapeSW() {}
 };
@@ -123,21 +119,21 @@ class PlaneShapeSW : public ShapeSW {
 
 	Plane plane;
 
-	void _setup(const Plane& p_plane);
-public:
+	void _setup(const Plane &p_plane);
 
+public:
 	Plane get_plane() const;
 
 	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;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
 
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	PlaneShapeSW();
@@ -148,20 +144,20 @@ class RayShapeSW : public ShapeSW {
 	float length;
 
 	void _setup(float p_length);
-public:
 
+public:
 	float get_length() const;
 
 	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;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
 
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	RayShapeSW();
@@ -172,20 +168,20 @@ class SphereShapeSW : public ShapeSW {
 	real_t radius;
 
 	void _setup(real_t p_radius);
-public:
 
+public:
 	real_t get_radius() const;
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	SphereShapeSW();
@@ -194,21 +190,21 @@ public:
 class BoxShapeSW : public ShapeSW {
 
 	Vector3 half_extents;
-	void _setup(const Vector3& p_half_extents);
-public:
+	void _setup(const Vector3 &p_half_extents);
 
+public:
 	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	BoxShapeSW();
@@ -219,23 +215,22 @@ class CapsuleShapeSW : public ShapeSW {
 	real_t height;
 	real_t radius;
 
+	void _setup(real_t p_height, real_t p_radius);
 
-	void _setup(real_t p_height,real_t p_radius);
 public:
-
 	_FORCE_INLINE_ real_t get_height() const { return height; }
 	_FORCE_INLINE_ real_t get_radius() const { return 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;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	CapsuleShapeSW();
@@ -245,28 +240,26 @@ struct ConvexPolygonShapeSW : public ShapeSW {
 
 	Geometry::MeshData mesh;
 
-	void _setup(const Vector<Vector3>& p_vertices);
-public:
+	void _setup(const Vector<Vector3> &p_vertices);
 
-	const Geometry::MeshData& get_mesh() const { return mesh; }
+public:
+	const Geometry::MeshData &get_mesh() const { return mesh; }
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	ConvexPolygonShapeSW();
-
 };
 
-
 struct _VolumeSW_BVH;
 struct FaceShapeSW;
 
@@ -317,39 +310,35 @@ struct ConcavePolygonShapeSW : public ConcaveShapeSW {
 		Vector3 normal;
 		real_t min_d;
 		int collisions;
-
 	};
 
-	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-	void _cull(int p_idx,_CullParams *p_params) const;
-
-	void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
+	void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
+	void _cull(int p_idx, _CullParams *p_params) const;
 
+	void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
 
 	void _setup(DVector<Vector3> p_faces);
-public:
 
+public:
 	DVector<Vector3> get_faces() const;
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
 
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
-	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	ConcavePolygonShapeSW();
-
 };
 
-
 struct HeightMapShapeSW : public ConcaveShapeSW {
 
 	DVector<real_t> heights;
@@ -357,12 +346,12 @@ struct HeightMapShapeSW : public ConcaveShapeSW {
 	int depth;
 	float cell_size;
 
-//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
-//	void _cull(int p_idx,_CullParams *p_params) const;
+	//	void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+	//	void _cull(int p_idx,_CullParams *p_params) const;
 
-	void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
-public:
+	void _setup(DVector<float> p_heights, int p_width, int p_depth, float p_cell_size);
 
+public:
 	DVector<real_t> get_heights() const;
 	int get_width() const;
 	int get_depth() const;
@@ -370,19 +359,18 @@ public:
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
-	virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
 	HeightMapShapeSW();
-
 };
 
 //used internally
@@ -393,22 +381,21 @@ struct FaceShapeSW : public ShapeSW {
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
 
-	const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
+	const Vector3 &get_vertex(int p_idx) const { return vertex[p_idx]; }
 
-	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	Vector3 get_support(const Vector3& p_normal) const;
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
-	bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+	void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 
 	Vector3 get_moment_of_inertia(float p_mass) const;
 
-	virtual void set_data(const Variant& p_data) {}
+	virtual void set_data(const Variant &p_data) {}
 	virtual Variant get_data() const { return Variant(); }
 
 	FaceShapeSW();
 };
 
-
 struct MotionShapeSW : public ShapeSW {
 
 	ShapeSW *shape;
@@ -416,56 +403,48 @@ struct MotionShapeSW : public ShapeSW {
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
 
-
-	void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+	void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
 
 		Vector3 cast = p_transform.basis.xform(motion);
-		real_t mina,maxa;
-		real_t minb,maxb;
+		real_t mina, maxa;
+		real_t minb, maxb;
 		Transform ofsb = p_transform;
-		ofsb.origin+=cast;
-		shape->project_range(p_normal,p_transform,mina,maxa);
-		shape->project_range(p_normal,ofsb,minb,maxb);
-		r_min=MIN(mina,minb);
-		r_max=MAX(maxa,maxb);
+		ofsb.origin += cast;
+		shape->project_range(p_normal, p_transform, mina, maxa);
+		shape->project_range(p_normal, ofsb, minb, maxb);
+		r_min = MIN(mina, minb);
+		r_max = MAX(maxa, maxb);
 	}
 
-	Vector3 get_support(const Vector3& p_normal) const {
+	Vector3 get_support(const Vector3 &p_normal) const {
 
 		Vector3 support = shape->get_support(p_normal);
-		if (p_normal.dot(motion)>0) {
-			support+=motion;
+		if (p_normal.dot(motion) > 0) {
+			support += motion;
 		}
 		return support;
 	}
-	virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
-	bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const { return false; }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
+	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; }
 
 	Vector3 get_moment_of_inertia(float p_mass) const { return Vector3(); }
 
-	virtual void set_data(const Variant& p_data) {}
+	virtual void set_data(const Variant &p_data) {}
 	virtual Variant get_data() const { return Variant(); }
 
-	MotionShapeSW()  { configure(AABB()); }
+	MotionShapeSW() { configure(AABB()); }
 };
 
-
-
-
 struct _ShapeTestConvexBSPSW {
 
 	const BSP_Tree *bsp;
 	const ShapeSW *shape;
 	Transform transform;
 
-	_FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector3 &p_normal, real_t &r_min, real_t &r_max) const {
 
-		shape->project_range(p_normal,transform,r_min,r_max);
+		shape->project_range(p_normal, transform, r_min, r_max);
 	}
-
 };
 
-
-
-
 #endif // SHAPESW_H
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 237b6a7ce..06234ead5 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -26,66 +26,58 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
-#include "globals.h"
 #include "space_sw.h"
 #include "collision_solver_sw.h"
+#include "globals.h"
 #include "physics_server_sw.h"
 
-
 _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {
 
-	if (p_object->get_type()==CollisionObjectSW::TYPE_AREA)
-		return p_type_mask&PhysicsDirectSpaceState::TYPE_MASK_AREA;
+	if (p_object->get_type() == CollisionObjectSW::TYPE_AREA)
+		return p_type_mask & PhysicsDirectSpaceState::TYPE_MASK_AREA;
 
-	if ((p_object->get_layer_mask()&p_layer_mask)==0)
+	if ((p_object->get_layer_mask() & p_layer_mask) == 0)
 		return false;
 
-	BodySW *body = static_cast<BodySW*>(p_object);
-
-	return (1<<body->get_mode())&p_type_mask;
+	BodySW *body = static_cast<BodySW *>(p_object);
 
+	return (1 << body->get_mode()) & p_type_mask;
 }
 
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_ray) {
 
-bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to, RayResult &r_result, const Set<RID>& p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_ray) {
-
+	ERR_FAIL_COND_V(space->locked, false);
 
-	ERR_FAIL_COND_V(space->locked,false);
-
-	Vector3 begin,end;
+	Vector3 begin, end;
 	Vector3 normal;
-	begin=p_from;
-	end=p_to;
-	normal=(end-begin).normalized();
-
-
-	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	begin = p_from;
+	end = p_to;
+	normal = (end - begin).normalized();
 
+	int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
 
-	bool collided=false;
-	Vector3 res_point,res_normal;
+	bool collided = false;
+	Vector3 res_point, res_normal;
 	int res_shape;
 	const CollisionObjectSW *res_obj;
-	real_t min_d=1e10;
-
-
+	real_t min_d = 1e10;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_pick_ray && !(static_cast<CollisionObjectSW*>(space->intersection_query_results[i])->is_ray_pickable()))
+		if (p_pick_ray && !(static_cast<CollisionObjectSW *>(space->intersection_query_results[i])->is_ray_pickable()))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue;
 
-		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
 
-		int shape_idx=space->intersection_query_subindex_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 		Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
 
 		Vector3 local_from = inv_xform.xform(begin);
@@ -93,291 +85,266 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
 
 		const ShapeSW *shape = col_obj->get_shape(shape_idx);
 
-		Vector3 shape_point,shape_normal;
-
-
-		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
-
+		Vector3 shape_point, shape_normal;
 
+		if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
 
 			Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
-			shape_point=xform.xform(shape_point);
+			shape_point = xform.xform(shape_point);
 
 			real_t ld = normal.dot(shape_point);
 
+			if (ld < min_d) {
 
-			if (ld<min_d) {
-
-				min_d=ld;
-				res_point=shape_point;
-				res_normal=inv_xform.basis.xform_inv(shape_normal).normalized();
-				res_shape=shape_idx;
-				res_obj=col_obj;
-				collided=true;
+				min_d = ld;
+				res_point = shape_point;
+				res_normal = inv_xform.basis.xform_inv(shape_normal).normalized();
+				res_shape = shape_idx;
+				res_obj = col_obj;
+				collided = true;
 			}
 		}
-
 	}
 
 	if (!collided)
 		return false;
 
-
-	r_result.collider_id=res_obj->get_instance_id();
-	if (r_result.collider_id!=0)
-		r_result.collider=ObjectDB::get_instance(r_result.collider_id);
+	r_result.collider_id = res_obj->get_instance_id();
+	if (r_result.collider_id != 0)
+		r_result.collider = ObjectDB::get_instance(r_result.collider_id);
 	else
-		r_result.collider=NULL;
-	r_result.normal=res_normal;
-	r_result.position=res_point;
-	r_result.rid=res_obj->get_self();
-	r_result.shape=res_shape;
+		r_result.collider = NULL;
+	r_result.normal = res_normal;
+	r_result.position = res_point;
+	r_result.rid = res_obj->get_self();
+	r_result.shape = res_shape;
 
 	return true;
-
 }
 
+int PhysicsDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
-	if (p_result_max<=0)
+	if (p_result_max <= 0)
 		return 0;
 
-	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	AABB aabb = p_xform.xform(shape->get_aabb());
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	int cc=0;
+	int cc = 0;
 
 	//Transform ai = p_xform.affine_inverse();
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-		if (cc>=p_result_max)
+		if (cc >= p_result_max)
 			break;
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
 		//area cant be picked by ray (default)
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue;
 
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
-
-		if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL,p_margin,0))
+		if (!CollisionSolverSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL, NULL, NULL, p_margin, 0))
 			continue;
 
 		if (r_results) {
-			r_results[cc].collider_id=col_obj->get_instance_id();
-			if (r_results[cc].collider_id!=0)
-				r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+			r_results[cc].collider_id = col_obj->get_instance_id();
+			if (r_results[cc].collider_id != 0)
+				r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
 			else
-				r_results[cc].collider=NULL;
-			r_results[cc].rid=col_obj->get_self();
-			r_results[cc].shape=shape_idx;
+				r_results[cc].collider = NULL;
+			r_results[cc].rid = col_obj->get_self();
+			r_results[cc].shape = shape_idx;
 		}
 
 		cc++;
-
 	}
 
 	return cc;
-
 }
 
+bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, ShapeRestInfo *r_info) {
 
-bool PhysicsDirectSpaceStateSW::cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,ShapeRestInfo *r_info) {
-
-
-
-	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,false);
+	ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, false);
 
 	AABB aabb = p_xform.xform(shape->get_aabb());
-	aabb=aabb.merge(AABB(aabb.pos+p_motion,aabb.size)); //motion
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.merge(AABB(aabb.pos + p_motion, aabb.size)); //motion
+	aabb = aabb.grow(p_margin);
 
 	//if (p_motion!=Vector3())
 	//	print_line(p_motion);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	float best_safe=1;
-	float best_unsafe=1;
+	float best_safe = 1;
+	float best_unsafe = 1;
 
 	Transform xform_inv = p_xform.affine_inverse();
 	MotionShapeSW mshape;
-	mshape.shape=shape;
-	mshape.motion=xform_inv.basis.xform(p_motion);
+	mshape.shape = shape;
+	mshape.motion = xform_inv.basis.xform(p_motion);
 
-	bool best_first=true;
+	bool best_first = true;
 
-	Vector3 closest_A,closest_B;
+	Vector3 closest_A, closest_B;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue; //ignore excluded
 
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
-
-		Vector3 point_A,point_B;
-		Vector3 sep_axis=p_motion.normalized();
+		Vector3 point_A, point_B;
+		Vector3 sep_axis = p_motion.normalized();
 
 		Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		//test initial overlap, does it collide if going all the way?
-		if (CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+		if (CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
 			//print_line("failed motion cast (no collision)");
 			continue;
 		}
 
-
-		//test initial overlap
+//test initial overlap
 #if 0
 		if (CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,NULL,NULL,&sep_axis)) {
 			print_line("failed initial cast (collision at begining)");
 			return false;
 		}
 #else
-		sep_axis=p_motion.normalized();
+		sep_axis = p_motion.normalized();
 
-		if (!CollisionSolverSW::solve_distance(shape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,point_A,point_B,aabb,&sep_axis)) {
+		if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
 			//print_line("failed motion cast (no collision)");
 			return false;
 		}
 #endif
 
-
 		//just do kinematic solving
-		float low=0;
-		float hi=1;
-		Vector3 mnormal=p_motion.normalized();
+		float low = 0;
+		float hi = 1;
+		Vector3 mnormal = p_motion.normalized();
 
-		for(int i=0;i<8;i++) { //steps should be customizable..
+		for (int i = 0; i < 8; i++) { //steps should be customizable..
 
-			float ofs = (low+hi)*0.5;
+			float ofs = (low + hi) * 0.5;
 
-			Vector3 sep=mnormal; //important optimization for this to work fast enough
+			Vector3 sep = mnormal; //important optimization for this to work fast enough
 
-			mshape.motion=xform_inv.basis.xform(p_motion*ofs);
+			mshape.motion = xform_inv.basis.xform(p_motion * ofs);
 
-			Vector3 lA,lB;
+			Vector3 lA, lB;
 
-			bool collided = !CollisionSolverSW::solve_distance(&mshape,p_xform,col_obj->get_shape(shape_idx),col_obj_xform,lA,lB,aabb,&sep);
+			bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
 
 			if (collided) {
 
 				//print_line(itos(i)+": "+rtos(ofs));
-				hi=ofs;
+				hi = ofs;
 			} else {
 
-				point_A=lA;
-				point_B=lB;
-				low=ofs;
+				point_A = lA;
+				point_B = lB;
+				low = ofs;
 			}
 		}
 
-		if (low<best_safe) {
-			best_first=true; //force reset
-			best_safe=low;
-			best_unsafe=hi;
+		if (low < best_safe) {
+			best_first = true; //force reset
+			best_safe = low;
+			best_unsafe = hi;
 		}
 
-		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low<=best_safe))) {
-			closest_A=point_A;
-			closest_B=point_B;
-			r_info->collider_id=col_obj->get_instance_id();
-			r_info->rid=col_obj->get_self();
-			r_info->shape=shape_idx;
-			r_info->point=closest_B;
-			r_info->normal=(closest_A-closest_B).normalized();
-			best_first=false;
-			if (col_obj->get_type()==CollisionObjectSW::TYPE_BODY) {
-				const BodySW *body=static_cast<const BodySW*>(col_obj);
-				r_info->linear_velocity= body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
+		if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) {
+			closest_A = point_A;
+			closest_B = point_B;
+			r_info->collider_id = col_obj->get_instance_id();
+			r_info->rid = col_obj->get_self();
+			r_info->shape = shape_idx;
+			r_info->point = closest_B;
+			r_info->normal = (closest_A - closest_B).normalized();
+			best_first = false;
+			if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
+				const BodySW *body = static_cast<const BodySW *>(col_obj);
+				r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
 			}
-
 		}
-
-
 	}
 
-	p_closest_safe=best_safe;
-	p_closest_unsafe=best_unsafe;
+	p_closest_safe = best_safe;
+	p_closest_unsafe = best_unsafe;
 
 	return true;
 }
 
-bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask){
+bool PhysicsDirectSpaceStateSW::collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-	if (p_result_max<=0)
+	if (p_result_max <= 0)
 		return 0;
 
-	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	AABB aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.grow(p_margin);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	bool collided=false;
-	r_result_count=0;
+	bool collided = false;
+	r_result_count = 0;
 
 	PhysicsServerSW::CollCbkData cbk;
-	cbk.max=p_result_max;
-	cbk.amount=0;
-	cbk.ptr=r_results;
-	CollisionSolverSW::CallbackResult cbkres=NULL;
+	cbk.max = p_result_max;
+	cbk.amount = 0;
+	cbk.ptr = r_results;
+	CollisionSolverSW::CallbackResult cbkres = NULL;
 
-	PhysicsServerSW::CollCbkData *cbkptr=NULL;
-	if (p_result_max>0) {
-		cbkptr=&cbk;
-		cbkres=PhysicsServerSW::_shape_col_cbk;
+	PhysicsServerSW::CollCbkData *cbkptr = NULL;
+	if (p_result_max > 0) {
+		cbkptr = &cbk;
+		cbkres = PhysicsServerSW::_shape_col_cbk;
 	}
 
+	for (int i = 0; i < amount; i++) {
 
-	for(int i=0;i<amount;i++) {
-
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (p_exclude.has( col_obj->get_self() )) {
+		if (p_exclude.has(col_obj->get_self())) {
 			continue;
 		}
 
 		//print_line("AGAINST: "+itos(col_obj->get_self().get_id())+":"+itos(shape_idx));
 		//print_line("THE ABBB: "+(col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).xform(col_obj->get_shape(shape_idx)->get_aabb()));
 
-		if (CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),cbkres,cbkptr,NULL,p_margin)) {
-			collided=true;
+		if (CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
+			collided = true;
 		}
-
 	}
 
-	r_result_count=cbk.amount;
+	r_result_count = cbk.amount;
 
 	return collided;
-
 }
 
-
 struct _RestCallbackData {
 
 	const CollisionObjectSW *object;
@@ -389,173 +356,147 @@ struct _RestCallbackData {
 	float best_len;
 };
 
-static void _rest_cbk_result(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) {
-
+static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
 
-	_RestCallbackData *rd=(_RestCallbackData*)p_userdata;
+	_RestCallbackData *rd = (_RestCallbackData *)p_userdata;
 
 	Vector3 contact_rel = p_point_B - p_point_A;
 	float len = contact_rel.length();
 	if (len <= rd->best_len)
 		return;
 
-	rd->best_len=len;
-	rd->best_contact=p_point_B;
-	rd->best_normal=contact_rel/len;
-	rd->best_object=rd->object;
-	rd->best_shape=rd->shape;
-
+	rd->best_len = len;
+	rd->best_contact = p_point_B;
+	rd->best_normal = contact_rel / len;
+	rd->best_object = rd->object;
+	rd->best_shape = rd->shape;
 }
-bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
+bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-	ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	AABB aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.grow(p_margin);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	_RestCallbackData rcd;
-	rcd.best_len=0;
-	rcd.best_object=NULL;
-	rcd.best_shape=0;
+	rcd.best_len = 0;
+	rcd.best_object = NULL;
+	rcd.best_shape = 0;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		const CollisionObjectSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
+		const CollisionObjectSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (p_exclude.has( col_obj->get_self() ))
+		if (p_exclude.has(col_obj->get_self()))
 			continue;
 
-		rcd.object=col_obj;
-		rcd.shape=shape_idx;
-		bool sc = CollisionSolverSW::solve_static(shape,p_shape_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),_rest_cbk_result,&rcd,NULL,p_margin);
+		rcd.object = col_obj;
+		rcd.shape = shape_idx;
+		bool sc = CollisionSolverSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin);
 		if (!sc)
 			continue;
-
-
 	}
 
-	if (rcd.best_len==0)
+	if (rcd.best_len == 0)
 		return false;
 
-	r_info->collider_id=rcd.best_object->get_instance_id();
-	r_info->shape=rcd.best_shape;
-	r_info->normal=rcd.best_normal;
-	r_info->point=rcd.best_contact;
-	r_info->rid=rcd.best_object->get_self();
-	if (rcd.best_object->get_type()==CollisionObjectSW::TYPE_BODY) {
+	r_info->collider_id = rcd.best_object->get_instance_id();
+	r_info->shape = rcd.best_shape;
+	r_info->normal = rcd.best_normal;
+	r_info->point = rcd.best_contact;
+	r_info->rid = rcd.best_object->get_self();
+	if (rcd.best_object->get_type() == CollisionObjectSW::TYPE_BODY) {
 
-		const BodySW *body = static_cast<const BodySW*>(rcd.best_object);
+		const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
 		r_info->linear_velocity = body->get_linear_velocity() +
-				(body->get_angular_velocity()).cross(body->get_transform().origin-rcd.best_contact);// * mPos);
-
+								  (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
 
 	} else {
-		r_info->linear_velocity=Vector3();
+		r_info->linear_velocity = Vector3();
 	}
 
 	return true;
 }
 
-
 PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
 
-
-	space=NULL;
+	space = NULL;
 }
 
-
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
 
+	CollisionObjectSW::Type type_A = A->get_type();
+	CollisionObjectSW::Type type_B = B->get_type();
+	if (type_A > type_B) {
 
-
-
-
-
-
-
-
-void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self) {
-
-	CollisionObjectSW::Type type_A=A->get_type();
-	CollisionObjectSW::Type type_B=B->get_type();
-	if (type_A>type_B) {
-
-		SWAP(A,B);
-		SWAP(p_subindex_A,p_subindex_B);
-		SWAP(type_A,type_B);
+		SWAP(A, B);
+		SWAP(p_subindex_A, p_subindex_B);
+		SWAP(type_A, type_B);
 	}
 
-	SpaceSW *self = (SpaceSW*)p_self;
+	SpaceSW *self = (SpaceSW *)p_self;
 
 	self->collision_pairs++;
 
-	if (type_A==CollisionObjectSW::TYPE_AREA) {
+	if (type_A == CollisionObjectSW::TYPE_AREA) {
 
-		AreaSW *area=static_cast<AreaSW*>(A);
-		if (type_B==CollisionObjectSW::TYPE_AREA) {
+		AreaSW *area = static_cast<AreaSW *>(A);
+		if (type_B == CollisionObjectSW::TYPE_AREA) {
 
-			AreaSW *area_b=static_cast<AreaSW*>(B);
-			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b,p_subindex_B,area,p_subindex_A) );
+			AreaSW *area_b = static_cast<AreaSW *>(B);
+			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A));
 			return area2_pair;
 		} else {
 
-			BodySW *body=static_cast<BodySW*>(B);
-			AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );
+			BodySW *body = static_cast<BodySW *>(B);
+			AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A));
 			return area_pair;
 		}
 	} else {
 
-
-		BodyPairSW *b = memnew( BodyPairSW((BodySW*)A,p_subindex_A,(BodySW*)B,p_subindex_B) );
+		BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B));
 		return b;
-
 	}
 
 	return NULL;
 }
 
-void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self) {
-
+void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) {
 
-
-	SpaceSW *self = (SpaceSW*)p_self;
+	SpaceSW *self = (SpaceSW *)p_self;
 	self->collision_pairs--;
-	ConstraintSW *c = (ConstraintSW*)p_data;
+	ConstraintSW *c = (ConstraintSW *)p_data;
 	memdelete(c);
 }
 
-
-const SelfList<BodySW>::List& SpaceSW::get_active_body_list() const {
+const SelfList<BodySW>::List &SpaceSW::get_active_body_list() const {
 
 	return active_list;
 }
-void SpaceSW::body_add_to_active_list(SelfList<BodySW>* p_body) {
+void SpaceSW::body_add_to_active_list(SelfList<BodySW> *p_body) {
 
 	active_list.add(p_body);
 }
-void SpaceSW::body_remove_from_active_list(SelfList<BodySW>* p_body) {
+void SpaceSW::body_remove_from_active_list(SelfList<BodySW> *p_body) {
 
 	active_list.remove(p_body);
-
 }
 
-void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW>* p_body) {
-
+void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW> *p_body) {
 
 	inertia_update_list.add(p_body);
 }
 
-void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW>* p_body) {
+void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW> *p_body) {
 
 	inertia_update_list.remove(p_body);
 }
@@ -567,112 +508,103 @@ BroadPhaseSW *SpaceSW::get_broadphase() {
 
 void SpaceSW::add_object(CollisionObjectSW *p_object) {
 
-	ERR_FAIL_COND( objects.has(p_object) );
+	ERR_FAIL_COND(objects.has(p_object));
 	objects.insert(p_object);
 }
 
 void SpaceSW::remove_object(CollisionObjectSW *p_object) {
 
-	ERR_FAIL_COND( !objects.has(p_object) );
+	ERR_FAIL_COND(!objects.has(p_object));
 	objects.erase(p_object);
 }
 
-const Set<CollisionObjectSW*> &SpaceSW::get_objects() const {
+const Set<CollisionObjectSW *> &SpaceSW::get_objects() const {
 
 	return objects;
 }
 
-void SpaceSW::body_add_to_state_query_list(SelfList<BodySW>* p_body) {
+void SpaceSW::body_add_to_state_query_list(SelfList<BodySW> *p_body) {
 
 	state_query_list.add(p_body);
 }
-void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW>* p_body) {
+void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW> *p_body) {
 
 	state_query_list.remove(p_body);
 }
 
-void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW>* p_area) {
+void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW> *p_area) {
 
 	monitor_query_list.add(p_area);
 }
-void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area) {
+void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area) {
 
 	monitor_query_list.remove(p_area);
 }
 
-void SpaceSW::area_add_to_moved_list(SelfList<AreaSW>* p_area) {
+void SpaceSW::area_add_to_moved_list(SelfList<AreaSW> *p_area) {
 
 	area_moved_list.add(p_area);
 }
 
-void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW>* p_area) {
+void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW> *p_area) {
 
 	area_moved_list.remove(p_area);
 }
 
-const SelfList<AreaSW>::List& SpaceSW::get_moved_area_list() const {
+const SelfList<AreaSW>::List &SpaceSW::get_moved_area_list() const {
 
 	return area_moved_list;
 }
 
-
-
-
 void SpaceSW::call_queries() {
 
-	while(state_query_list.first()) {
+	while (state_query_list.first()) {
 
-		BodySW * b = state_query_list.first()->self();
+		BodySW *b = state_query_list.first()->self();
 		b->call_queries();
 		state_query_list.remove(state_query_list.first());
 	}
 
-	while(monitor_query_list.first()) {
+	while (monitor_query_list.first()) {
 
-		AreaSW * a = monitor_query_list.first()->self();
+		AreaSW *a = monitor_query_list.first()->self();
 		a->call_queries();
 		monitor_query_list.remove(monitor_query_list.first());
 	}
-
 }
 
 void SpaceSW::setup() {
 
-	contact_debug_count=0;
-	while(inertia_update_list.first()) {
+	contact_debug_count = 0;
+	while (inertia_update_list.first()) {
 		inertia_update_list.first()->self()->update_inertias();
 		inertia_update_list.remove(inertia_update_list.first());
 	}
-
-
 }
 
 void SpaceSW::update() {
 
-
 	broadphase->update();
-
 }
 
-
 void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 
-		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
-		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
-		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
-		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_threshold=p_value; break;
-		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold=p_value; break;
-		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
-		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
-		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break;
+		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_threshold = p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_threshold = p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
+		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break;
+		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
 	}
 }
 
 real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 
 		case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
 		case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
@@ -688,12 +620,12 @@ real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
 
 void SpaceSW::lock() {
 
-	locked=true;
+	locked = true;
 }
 
 void SpaceSW::unlock() {
 
-	locked=false;
+	locked = false;
 }
 
 bool SpaceSW::is_locked() const {
@@ -708,41 +640,36 @@ PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {
 
 SpaceSW::SpaceSW() {
 
-	collision_pairs=0;
-	active_objects=0;
-	island_count=0;
-	contact_debug_count=0;
+	collision_pairs = 0;
+	active_objects = 0;
+	island_count = 0;
+	contact_debug_count = 0;
 
-	locked=false;
-	contact_recycle_radius=0.01;
-	contact_max_separation=0.05;
-	contact_max_allowed_penetration= 0.01;
+	locked = false;
+	contact_recycle_radius = 0.01;
+	contact_max_separation = 0.05;
+	contact_max_allowed_penetration = 0.01;
 
 	constraint_bias = 0.01;
-	body_linear_velocity_sleep_threshold=GLOBAL_DEF("physics/sleep_threshold_linear",0.1);
-	body_angular_velocity_sleep_threshold=GLOBAL_DEF("physics/sleep_threshold_angular", (8.0 / 180.0 * Math_PI) );
-	body_time_to_sleep=GLOBAL_DEF("physics/time_before_sleep",0.5);
-	body_angular_velocity_damp_ratio=10;
-
+	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/sleep_threshold_linear", 0.1);
+	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/sleep_threshold_angular", (8.0 / 180.0 * Math_PI));
+	body_time_to_sleep = GLOBAL_DEF("physics/time_before_sleep", 0.5);
+	body_angular_velocity_damp_ratio = 10;
 
 	broadphase = BroadPhaseSW::create_func();
-	broadphase->set_pair_callback(_broadphase_pair,this);
-	broadphase->set_unpair_callback(_broadphase_unpair,this);
-	area=NULL;
+	broadphase->set_pair_callback(_broadphase_pair, this);
+	broadphase->set_unpair_callback(_broadphase_unpair, this);
+	area = NULL;
 
-	direct_access = memnew( PhysicsDirectSpaceStateSW );
-	direct_access->space=this;
-
-	for(int i=0;i<ELAPSED_TIME_MAX;i++)
-		elapsed_time[i]=0;
+	direct_access = memnew(PhysicsDirectSpaceStateSW);
+	direct_access->space = this;
 
+	for (int i = 0; i < ELAPSED_TIME_MAX; i++)
+		elapsed_time[i] = 0;
 }
 
 SpaceSW::~SpaceSW() {
 
 	memdelete(broadphase);
-	memdelete( direct_access );
+	memdelete(direct_access);
 }
-
-
-
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h
index 1180d787e..b5bc1ab5a 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -29,39 +29,35 @@
 #ifndef SPACE_SW_H
 #define SPACE_SW_H
 
-#include "typedefs.h"
-#include "hash_map.h"
-#include "body_sw.h"
+#include "area_pair_sw.h"
 #include "area_sw.h"
 #include "body_pair_sw.h"
-#include "area_pair_sw.h"
+#include "body_sw.h"
 #include "broad_phase_sw.h"
 #include "collision_object_sw.h"
 #include "globals.h"
-
+#include "hash_map.h"
+#include "typedefs.h"
 
 class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
 
-	OBJ_TYPE( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState );
-public:
+	OBJ_TYPE(PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState);
 
+public:
 	SpaceSW *space;
 
-	virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,bool p_pick_ray=false);
-	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL);
-	virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false);
+	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL);
+	virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 
 	PhysicsDirectSpaceStateSW();
 };
 
-
-
 class SpaceSW {
 
 public:
-
 	enum ElapsedTime {
 		ELAPSED_TIME_INTEGRATE_FORCES,
 		ELAPSED_TIME_GENERATE_ISLANDS,
@@ -71,8 +67,8 @@ public:
 		ELAPSED_TIME_MAX
 
 	};
-private:
 
+private:
 	uint64_t elapsed_time[ELAPSED_TIME_MAX];
 
 	PhysicsDirectSpaceStateSW *direct_access;
@@ -85,10 +81,10 @@ private:
 	SelfList<AreaSW>::List monitor_query_list;
 	SelfList<AreaSW>::List area_moved_list;
 
-	static void* _broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self);
-	static void _broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self);
+	static void *_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self);
+	static void _broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self);
 
-	Set<CollisionObjectSW*> objects;
+	Set<CollisionObjectSW *> objects;
 
 	AreaSW *area;
 
@@ -99,7 +95,7 @@ private:
 
 	enum {
 
-		INTERSECTION_QUERY_MAX=2048
+		INTERSECTION_QUERY_MAX = 2048
 	};
 
 	CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX];
@@ -121,36 +117,35 @@ private:
 	Vector<Vector3> contact_debug;
 	int contact_debug_count;
 
-friend class PhysicsDirectSpaceStateSW;
+	friend class PhysicsDirectSpaceStateSW;
 
 public:
-
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	void set_default_area(AreaSW *p_area) { area=p_area; }
+	void set_default_area(AreaSW *p_area) { area = p_area; }
 	AreaSW *get_default_area() const { return area; }
 
-	const SelfList<BodySW>::List& get_active_body_list() const;
-	void body_add_to_active_list(SelfList<BodySW>* p_body);
-	void body_remove_from_active_list(SelfList<BodySW>* p_body);
-	void body_add_to_inertia_update_list(SelfList<BodySW>* p_body);
-	void body_remove_from_inertia_update_list(SelfList<BodySW>* p_body);
+	const SelfList<BodySW>::List &get_active_body_list() const;
+	void body_add_to_active_list(SelfList<BodySW> *p_body);
+	void body_remove_from_active_list(SelfList<BodySW> *p_body);
+	void body_add_to_inertia_update_list(SelfList<BodySW> *p_body);
+	void body_remove_from_inertia_update_list(SelfList<BodySW> *p_body);
 
-	void body_add_to_state_query_list(SelfList<BodySW>* p_body);
-	void body_remove_from_state_query_list(SelfList<BodySW>* p_body);
+	void body_add_to_state_query_list(SelfList<BodySW> *p_body);
+	void body_remove_from_state_query_list(SelfList<BodySW> *p_body);
 
-	void area_add_to_monitor_query_list(SelfList<AreaSW>* p_area);
-	void area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area);
-	void area_add_to_moved_list(SelfList<AreaSW>* p_area);
-	void area_remove_from_moved_list(SelfList<AreaSW>* p_area);
-	const SelfList<AreaSW>::List& get_moved_area_list() const;
+	void area_add_to_monitor_query_list(SelfList<AreaSW> *p_area);
+	void area_remove_from_monitor_query_list(SelfList<AreaSW> *p_area);
+	void area_add_to_moved_list(SelfList<AreaSW> *p_area);
+	void area_remove_from_moved_list(SelfList<AreaSW> *p_area);
+	const SelfList<AreaSW>::List &get_moved_area_list() const;
 
 	BroadPhaseSW *get_broadphase();
 
 	void add_object(CollisionObjectSW *p_object);
 	void remove_object(CollisionObjectSW *p_object);
-	const Set<CollisionObjectSW*> &get_objects() const;
+	const Set<CollisionObjectSW *> &get_objects() const;
 
 	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
 	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
@@ -161,12 +156,10 @@ public:
 	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
 	_FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
 
-
 	void update();
 	void setup();
 	void call_queries();
 
-
 	bool is_locked() const;
 	void lock();
 	void unlock();
@@ -174,10 +167,10 @@ public:
 	void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
 	real_t get_param(PhysicsServer::SpaceParameter p_param) const;
 
-	void set_island_count(int p_island_count) { island_count=p_island_count; }
+	void set_island_count(int p_island_count) { island_count = p_island_count; }
 	int get_island_count() const { return island_count; }
 
-	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+	void set_active_objects(int p_active_objects) { active_objects = p_active_objects; }
 	int get_active_objects() const { return active_objects; }
 
 	int get_collision_pairs() const { return collision_pairs; }
@@ -186,19 +179,20 @@ public:
 
 	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
 	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }
-	_FORCE_INLINE_ void add_debug_contact(const Vector3& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }
+	_FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) {
+		if (contact_debug_count < contact_debug.size()) contact_debug[contact_debug_count++] = p_contact;
+	}
 	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contact_debug; }
 	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
 
-	void set_static_global_body(RID p_body) { static_global_body=p_body; }
+	void set_static_global_body(RID p_body) { static_global_body = p_body; }
 	RID get_static_global_body() { return static_global_body; }
 
-	void set_elapsed_time(ElapsedTime p_time,uint64_t p_msec) { elapsed_time[p_time]=p_msec; }
+	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
 	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
 
 	SpaceSW();
 	~SpaceSW();
 };
 
-
 #endif // SPACE__SW_H
diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp
index e24081761..74afb272d 100644
--- a/servers/physics/step_sw.cpp
+++ b/servers/physics/step_sw.cpp
@@ -31,279 +31,268 @@
 
 #include "os/os.h"
 
-void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) {
+void StepSW::_populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island) {
 
 	p_body->set_island_step(_step);
 	p_body->set_island_next(*p_island);
-	*p_island=p_body;
+	*p_island = p_body;
 
-	for(Map<ConstraintSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {
+	for (Map<ConstraintSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
 
-		ConstraintSW *c=(ConstraintSW*)E->key();
-		if (c->get_island_step()==_step)
+		ConstraintSW *c = (ConstraintSW *)E->key();
+		if (c->get_island_step() == _step)
 			continue; //already processed
 		c->set_island_step(_step);
 		c->set_island_next(*p_constraint_island);
-		*p_constraint_island=c;
+		*p_constraint_island = c;
 
-
-		for(int i=0;i<c->get_body_count();i++) {
-			if (i==E->get())
+		for (int i = 0; i < c->get_body_count(); i++) {
+			if (i == E->get())
 				continue;
 			BodySW *b = c->get_body_ptr()[i];
-			if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC)
+			if (b->get_island_step() == _step || b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC)
 				continue; //no go
-			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+			_populate_island(c->get_body_ptr()[i], p_island, p_constraint_island);
 		}
 	}
 }
 
-void StepSW::_setup_island(ConstraintSW *p_island,float p_delta) {
+void StepSW::_setup_island(ConstraintSW *p_island, float p_delta) {
 
-	ConstraintSW *ci=p_island;
-	while(ci) {
+	ConstraintSW *ci = p_island;
+	while (ci) {
 		bool process = ci->setup(p_delta);
 		//todo remove from island if process fails
-		ci=ci->get_island_next();
+		ci = ci->get_island_next();
 	}
 }
 
-void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,float p_delta){
+void StepSW::_solve_island(ConstraintSW *p_island, int p_iterations, float p_delta) {
 
-	int at_priority=1;
+	int at_priority = 1;
 
-	while(p_island) {
+	while (p_island) {
 
-		for(int i=0;i<p_iterations;i++) {
+		for (int i = 0; i < p_iterations; i++) {
 
-			ConstraintSW *ci=p_island;
-			while(ci) {
+			ConstraintSW *ci = p_island;
+			while (ci) {
 				ci->solve(p_delta);
-				ci=ci->get_island_next();
+				ci = ci->get_island_next();
 			}
 		}
 
 		at_priority++;
 
 		{
-			ConstraintSW *ci=p_island;
-			ConstraintSW *prev=NULL;
-			while(ci) {
-				if (ci->get_priority()<at_priority) {
+			ConstraintSW *ci = p_island;
+			ConstraintSW *prev = NULL;
+			while (ci) {
+				if (ci->get_priority() < at_priority) {
 					if (prev) {
 						prev->set_island_next(ci->get_island_next()); //remove
 					} else {
-						p_island=ci->get_island_next();
+						p_island = ci->get_island_next();
 					}
 				} else {
 
-					prev=ci;
+					prev = ci;
 				}
 
-				ci=ci->get_island_next();
+				ci = ci->get_island_next();
 			}
 		}
 	}
-
 }
 
-void StepSW::_check_suspend(BodySW *p_island,float p_delta) {
-
+void StepSW::_check_suspend(BodySW *p_island, float p_delta) {
 
-	bool can_sleep=true;
+	bool can_sleep = true;
 
 	BodySW *b = p_island;
-	while(b) {
+	while (b) {
 
-		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) {
-			b=b->get_island_next();
+		if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) {
+			b = b->get_island_next();
 			continue; //ignore for static
 		}
 
 		if (!b->sleep_test(p_delta))
-			can_sleep=false;
+			can_sleep = false;
 
-		b=b->get_island_next();
+		b = b->get_island_next();
 	}
 
 	//put all to sleep or wake up everyoen
 
 	b = p_island;
-	while(b) {
+	while (b) {
 
-		if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC || b->get_mode()==PhysicsServer::BODY_MODE_KINEMATIC) {
-			b=b->get_island_next();
+		if (b->get_mode() == PhysicsServer::BODY_MODE_STATIC || b->get_mode() == PhysicsServer::BODY_MODE_KINEMATIC) {
+			b = b->get_island_next();
 			continue; //ignore for static
 		}
 
 		bool active = b->is_active();
 
-		if (active==can_sleep)
+		if (active == can_sleep)
 			b->set_active(!can_sleep);
 
-		b=b->get_island_next();
+		b = b->get_island_next();
 	}
 }
 
-void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) {
+void StepSW::step(SpaceSW *p_space, float p_delta, int p_iterations) {
 
 	p_space->lock(); // can't access space during this
 
 	p_space->setup(); //update inertias, etc
 
-	const SelfList<BodySW>::List * body_list = &p_space->get_active_body_list();
+	const SelfList<BodySW>::List *body_list = &p_space->get_active_body_list();
 
 	/* INTEGRATE FORCES */
 
 	uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
-	uint64_t profile_endtime=0;
+	uint64_t profile_endtime = 0;
 
-	int active_count=0;
+	int active_count = 0;
 
-	const SelfList<BodySW>*b = body_list->first();
-	while(b) {
+	const SelfList<BodySW> *b = body_list->first();
+	while (b) {
 
 		b->self()->integrate_forces(p_delta);
-		b=b->next();
+		b = b->next();
 		active_count++;
 	}
 
 	p_space->set_active_objects(active_count);
 
-
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* GENERATE CONSTRAINT ISLANDS */
 
-	BodySW *island_list=NULL;
-	ConstraintSW *constraint_island_list=NULL;
+	BodySW *island_list = NULL;
+	ConstraintSW *constraint_island_list = NULL;
 	b = body_list->first();
 
-	int island_count=0;
+	int island_count = 0;
 
-	while(b) {
+	while (b) {
 		BodySW *body = b->self();
 
+		if (body->get_island_step() != _step) {
 
-		if (body->get_island_step()!=_step) {
-
-			BodySW *island=NULL;
-			ConstraintSW *constraint_island=NULL;
-			_populate_island(body,&island,&constraint_island);
+			BodySW *island = NULL;
+			ConstraintSW *constraint_island = NULL;
+			_populate_island(body, &island, &constraint_island);
 
 			island->set_island_list_next(island_list);
-			island_list=island;
+			island_list = island;
 
 			if (constraint_island) {
 				constraint_island->set_island_list_next(constraint_island_list);
-				constraint_island_list=constraint_island;
+				constraint_island_list = constraint_island;
 				island_count++;
 			}
-
 		}
-		b=b->next();
+		b = b->next();
 	}
 
 	p_space->set_island_count(island_count);
 
 	const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list();
 
-	while(aml.first()) {
-		for(const Set<ConstraintSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
+	while (aml.first()) {
+		for (const Set<ConstraintSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
 
-			ConstraintSW*c=E->get();
-			if (c->get_island_step()==_step)
+			ConstraintSW *c = E->get();
+			if (c->get_island_step() == _step)
 				continue;
 			c->set_island_step(_step);
 			c->set_island_next(NULL);
 			c->set_island_list_next(constraint_island_list);
-			constraint_island_list=c;
+			constraint_island_list = c;
 		}
-		p_space->area_remove_from_moved_list((SelfList<AreaSW>*)aml.first()); //faster to remove here
+		p_space->area_remove_from_moved_list((SelfList<AreaSW> *)aml.first()); //faster to remove here
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
-
-//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
 	/* SETUP CONSTRAINT ISLANDS */
 
 	{
-		ConstraintSW *ci=constraint_island_list;
-		while(ci) {
+		ConstraintSW *ci = constraint_island_list;
+		while (ci) {
 
-			_setup_island(ci,p_delta);
-			ci=ci->get_island_list_next();
+			_setup_island(ci, p_delta);
+			ci = ci->get_island_list_next();
 		}
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* SOLVE CONSTRAINT ISLANDS */
 
 	{
-		ConstraintSW *ci=constraint_island_list;
-		while(ci) {
+		ConstraintSW *ci = constraint_island_list;
+		while (ci) {
 			//iterating each island separatedly improves cache efficiency
-			_solve_island(ci,p_iterations,p_delta);
-			ci=ci->get_island_list_next();
+			_solve_island(ci, p_iterations, p_delta);
+			ci = ci->get_island_list_next();
 		}
 	}
 
-
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* INTEGRATE VELOCITIES */
 
 	b = body_list->first();
-	while(b) {
-		const SelfList<BodySW>*n=b->next();
+	while (b) {
+		const SelfList<BodySW> *n = b->next();
 		b->self()->integrate_velocities(p_delta);
-		b=n;
+		b = n;
 	}
 
 	/* SLEEP / WAKE UP ISLANDS */
 
 	{
-		BodySW *bi=island_list;
-		while(bi) {
+		BodySW *bi = island_list;
+		while (bi) {
 
-			_check_suspend(bi,p_delta);
-			bi=bi->get_island_list_next();
+			_check_suspend(bi, p_delta);
+			bi = bi->get_island_list_next();
 		}
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	p_space->update();
 	p_space->unlock();
 	_step++;
-
-
-
 }
 
 StepSW::StepSW() {
 
-	_step=1;
+	_step = 1;
 }
diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h
index 2f67b3c8d..0f4b9ff50 100644
--- a/servers/physics/step_sw.h
+++ b/servers/physics/step_sw.h
@@ -35,13 +35,13 @@ class StepSW {
 
 	uint64_t _step;
 
-	void _populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island);
-	void _setup_island(ConstraintSW *p_island,float p_delta);
-	void _solve_island(ConstraintSW *p_island,int p_iterations,float p_delta);
-	void _check_suspend(BodySW *p_island,float p_delta);
-public:
+	void _populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island);
+	void _setup_island(ConstraintSW *p_island, float p_delta);
+	void _solve_island(ConstraintSW *p_island, int p_iterations, float p_delta);
+	void _check_suspend(BodySW *p_island, float p_delta);
 
-	void step(SpaceSW* p_space,float p_delta,int p_iterations);
+public:
+	void step(SpaceSW *p_space, float p_delta, int p_iterations);
 	StepSW();
 };
 
diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 5ee983d70..a09a37961 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -27,18 +27,26 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 #include "area_2d_sw.h"
-#include "space_2d_sw.h"
 #include "body_2d_sw.h"
+#include "space_2d_sw.h"
 
-Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
-Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
+Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+	rid = p_body->get_self();
+	instance_id = p_body->get_instance_id();
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+}
 
 void Area2DSW::_shapes_changed() {
-
-
 }
 
-void Area2DSW::set_transform(const Matrix32& p_transform) {
+void Area2DSW::set_transform(const Matrix32 &p_transform) {
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
@@ -54,7 +62,6 @@ void Area2DSW::set_space(Space2DSW *p_space) {
 			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
 		if (moved_list.in_list())
 			get_space()->area_remove_from_moved_list(&moved_list);
-
 	}
 
 	monitored_bodies.clear();
@@ -63,19 +70,17 @@ void Area2DSW::set_space(Space2DSW *p_space) {
 	_set_space(p_space);
 }
 
+void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
 
-void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
-
-
-	if (p_id==monitor_callback_id) {
-		monitor_callback_method=p_method;
+	if (p_id == monitor_callback_id) {
+		monitor_callback_method = p_method;
 		return;
 	}
 
 	_unregister_shapes();
 
-	monitor_callback_id=p_id;
-	monitor_callback_method=p_method;
+	monitor_callback_id = p_id;
+	monitor_callback_method = p_method;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
@@ -84,22 +89,19 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
-
-
 }
 
-void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) {
+void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) {
 
-
-	if (p_id==area_monitor_callback_id) {
-		area_monitor_callback_method=p_method;
+	if (p_id == area_monitor_callback_id) {
+		area_monitor_callback_method = p_method;
 		return;
 	}
 
 	_unregister_shapes();
 
-	area_monitor_callback_id=p_id;
-	area_monitor_callback_method=p_method;
+	area_monitor_callback_id = p_id;
+	area_monitor_callback_method = p_method;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
@@ -108,45 +110,63 @@ void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_meth
 
 	if (!moved_list.in_list() && get_space())
 		get_space()->area_add_to_moved_list(&moved_list);
-
-
 }
 
-
 void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) {
-	bool do_override=p_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
-	if (do_override==(space_override_mode!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED))
+	bool do_override = p_mode != Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override == (space_override_mode != Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED))
 		return;
 	_unregister_shapes();
-	space_override_mode=p_mode;
+	space_override_mode = p_mode;
 	_shape_changed();
 }
 
-void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value) {
+void Area2DSW::set_param(Physics2DServer::AreaParameter p_param, const Variant &p_value) {
 
-	switch(p_param) {
-		case Physics2DServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_LINEAR_DAMP: linear_damp=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: angular_damp=p_value; ; break;
-		case Physics2DServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
+	switch (p_param) {
+		case Physics2DServer::AREA_PARAM_GRAVITY:
+			gravity = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR:
+			gravity_vector = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT:
+			gravity_is_point = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE:
+			gravity_distance_scale = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
+			point_attenuation = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_LINEAR_DAMP:
+			linear_damp = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_ANGULAR_DAMP:
+			angular_damp = p_value;
+			;
+			break;
+		case Physics2DServer::AREA_PARAM_PRIORITY:
+			priority = p_value;
+			;
+			break;
 	}
-
-
 }
 
 Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const {
 
-
-	switch(p_param) {
+	switch (p_param) {
 		case Physics2DServer::AREA_PARAM_GRAVITY: return gravity;
 		case Physics2DServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
 		case Physics2DServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
 		case Physics2DServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale;
-		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
+		case Physics2DServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation;
 		case Physics2DServer::AREA_PARAM_LINEAR_DAMP: return linear_damp;
 		case Physics2DServer::AREA_PARAM_ANGULAR_DAMP: return angular_damp;
 		case Physics2DServer::AREA_PARAM_PRIORITY: return priority;
@@ -155,23 +175,20 @@ Variant Area2DSW::get_param(Physics2DServer::AreaParameter p_param) const {
 	return Variant();
 }
 
-
 void Area2DSW::_queue_monitor_update() {
 
 	ERR_FAIL_COND(!get_space());
 
 	if (!monitor_query_list.in_list())
 		get_space()->area_add_to_monitor_query_list(&monitor_query_list);
-
-
 }
 
 void Area2DSW::set_monitorable(bool p_monitorable) {
 
-	if (monitorable==p_monitorable)
+	if (monitorable == p_monitorable)
 		return;
 
-	monitorable=p_monitorable;
+	monitorable = p_monitorable;
 	_set_static(!monitorable);
 }
 
@@ -181,31 +198,29 @@ void Area2DSW::call_queries() {
 
 		Variant res[5];
 		Variant *resptr[5];
-		for(int i=0;i<5;i++)
-			resptr[i]=&res[i];
+		for (int i = 0; i < 5; i++)
+			resptr[i] = &res[i];
 
 		Object *obj = ObjectDB::get_instance(monitor_callback_id);
 		if (!obj) {
 			monitored_bodies.clear();
-			monitor_callback_id=0;
+			monitor_callback_id = 0;
 			return;
 		}
 
+		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E; E = E->next()) {
 
-
-		for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) {
-
-			if (E->get().state==0)
+			if (E->get().state == 0)
 				continue; //nothing happened
 
-			res[0]=E->get().state>0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
-			res[1]=E->key().rid;
-			res[2]=E->key().instance_id;
-			res[3]=E->key().body_shape;
-			res[4]=E->key().area_shape;
+			res[0] = E->get().state > 0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
+			res[1] = E->key().rid;
+			res[2] = E->key().instance_id;
+			res[3] = E->key().body_shape;
+			res[4] = E->key().area_shape;
 
 			Variant::CallError ce;
-			obj->call(monitor_callback_method,(const Variant**)resptr,5,ce);
+			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
@@ -213,66 +228,57 @@ void Area2DSW::call_queries() {
 
 	if (area_monitor_callback_id && !monitored_areas.empty()) {
 
-
 		Variant res[5];
 		Variant *resptr[5];
-		for(int i=0;i<5;i++)
-			resptr[i]=&res[i];
+		for (int i = 0; i < 5; i++)
+			resptr[i] = &res[i];
 
 		Object *obj = ObjectDB::get_instance(area_monitor_callback_id);
 		if (!obj) {
 			monitored_areas.clear();
-			area_monitor_callback_id=0;
+			area_monitor_callback_id = 0;
 			return;
 		}
 
+		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E; E = E->next()) {
 
-
-		for (Map<BodyKey,BodyState>::Element *E=monitored_areas.front();E;E=E->next()) {
-
-			if (E->get().state==0)
+			if (E->get().state == 0)
 				continue; //nothing happened
 
-			res[0]=E->get().state>0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
-			res[1]=E->key().rid;
-			res[2]=E->key().instance_id;
-			res[3]=E->key().body_shape;
-			res[4]=E->key().area_shape;
-
+			res[0] = E->get().state > 0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
+			res[1] = E->key().rid;
+			res[2] = E->key().instance_id;
+			res[3] = E->key().body_shape;
+			res[4] = E->key().area_shape;
 
 			Variant::CallError ce;
-			obj->call(area_monitor_callback_method,(const Variant**)resptr,5,ce);
+			obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
 		}
 	}
 
 	monitored_areas.clear();
 
 	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
-
 }
 
-Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this)  {
+Area2DSW::Area2DSW()
+	: CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this) {
 
 	_set_static(true); //areas are not active by default
-	space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
-	gravity=9.80665;
-	gravity_vector=Vector2(0,-1);
-	gravity_is_point=false;
-	gravity_distance_scale=0;
-	point_attenuation=1;
-
-	angular_damp=1.0;
-	linear_damp=0.1;
-	priority=0;
-	monitor_callback_id=0;
-	area_monitor_callback_id=0;
-	monitorable=false;
-
+	space_override_mode = Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
+	gravity = 9.80665;
+	gravity_vector = Vector2(0, -1);
+	gravity_is_point = false;
+	gravity_distance_scale = 0;
+	point_attenuation = 1;
 
+	angular_damp = 1.0;
+	linear_damp = 0.1;
+	priority = 0;
+	monitor_callback_id = 0;
+	area_monitor_callback_id = 0;
+	monitorable = false;
 }
 
 Area2DSW::~Area2DSW() {
-
-
 }
-
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index ea991e8f6..da69485a4 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/servers/physics_2d/area_2d_sw.h
@@ -29,18 +29,16 @@
 #ifndef AREA_2D_SW_H
 #define AREA_2D_SW_H
 
-#include "servers/physics_2d_server.h"
 #include "collision_object_2d_sw.h"
 #include "self_list.h"
+#include "servers/physics_2d_server.h"
 //#include "servers/physics/query_sw.h"
 
 class Space2DSW;
 class Body2DSW;
 class Constraint2DSW;
 
-
-class Area2DSW : public CollisionObject2DSW{
-
+class Area2DSW : public CollisionObject2DSW {
 
 	Physics2DServer::AreaSpaceOverrideMode space_override_mode;
 	float gravity;
@@ -69,23 +67,22 @@ class Area2DSW : public CollisionObject2DSW{
 		uint32_t body_shape;
 		uint32_t area_shape;
 
-		_FORCE_INLINE_ bool operator<( const BodyKey& p_key) const {
+		_FORCE_INLINE_ bool operator<(const BodyKey &p_key) const {
 
-			if (rid==p_key.rid) {
+			if (rid == p_key.rid) {
 
-				if (body_shape==p_key.body_shape) {
+				if (body_shape == p_key.body_shape) {
 
 					return area_shape < p_key.area_shape;
 				} else
 					return body_shape < p_key.body_shape;
 			} else
 				return rid < p_key.rid;
-
 		}
 
 		_FORCE_INLINE_ BodyKey() {}
-		BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
-		BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+		BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 	};
 
 	struct BodyState {
@@ -93,122 +90,110 @@ class Area2DSW : public CollisionObject2DSW{
 		int state;
 		_FORCE_INLINE_ void inc() { state++; }
 		_FORCE_INLINE_ void dec() { state--; }
-		_FORCE_INLINE_ BodyState() { state=0; }
+		_FORCE_INLINE_ BodyState() { state = 0; }
 	};
 
-	Map<BodyKey,BodyState> monitored_bodies;
-	Map<BodyKey,BodyState> monitored_areas;
+	Map<BodyKey, BodyState> monitored_bodies;
+	Map<BodyKey, BodyState> monitored_areas;
 
 	//virtual void shape_changed_notify(Shape2DSW *p_shape);
 	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
-	Set<Constraint2DSW*> constraints;
-
+	Set<Constraint2DSW *> constraints;
 
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
 public:
-
 	//_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; }
 	//_FORCE_INLINE_ SpaceSW* get_owner() { return owner; }
 
-	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
+	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
 
-	void set_area_monitor_callback(ObjectID p_id, const StringName& p_method);
+	void set_area_monitor_callback(ObjectID p_id, const StringName &p_method);
 	_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id; }
 
+	_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 
-	_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
-	_FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+	_FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
 
-	_FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
-	_FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
-
-	void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value);
+	void set_param(Physics2DServer::AreaParameter p_param, const Variant &p_value);
 	Variant get_param(Physics2DServer::AreaParameter p_param) const;
 
 	void set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode);
 	Physics2DServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
 
-	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ void set_gravity(float p_gravity) { gravity = p_gravity; }
 	_FORCE_INLINE_ float get_gravity() const { return gravity; }
 
-	_FORCE_INLINE_ void set_gravity_vector(const Vector2& p_gravity) { gravity_vector=p_gravity; }
+	_FORCE_INLINE_ void set_gravity_vector(const Vector2 &p_gravity) { gravity_vector = p_gravity; }
 	_FORCE_INLINE_ Vector2 get_gravity_vector() const { return gravity_vector; }
 
-	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
+	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point = p_enable; }
 	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
 
-	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale=scale; }
+	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale = scale; }
 	_FORCE_INLINE_ float get_gravity_distance_scale() const { return gravity_distance_scale; }
 
-	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation = p_point_attenuation; }
 	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
 
-	_FORCE_INLINE_ void set_linear_damp(float p_linear_damp) { linear_damp=p_linear_damp; }
+	_FORCE_INLINE_ void set_linear_damp(float p_linear_damp) { linear_damp = p_linear_damp; }
 	_FORCE_INLINE_ float get_linear_damp() const { return linear_damp; }
 
-	_FORCE_INLINE_ void set_angular_damp(float p_angular_damp) { angular_damp=p_angular_damp; }
+	_FORCE_INLINE_ void set_angular_damp(float p_angular_damp) { angular_damp = p_angular_damp; }
 	_FORCE_INLINE_ float get_angular_damp() const { return angular_damp; }
 
-	_FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; }
+	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
-	_FORCE_INLINE_ void add_constraint( Constraint2DSW* p_constraint) { constraints.insert(p_constraint); }
-	_FORCE_INLINE_ void remove_constraint( Constraint2DSW* p_constraint) { constraints.erase(p_constraint); }
-	_FORCE_INLINE_ const Set<Constraint2DSW*>& get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<Constraint2DSW *> &get_constraints() const { return constraints; }
 
 	void set_monitorable(bool p_monitorable);
 	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 
-	void set_transform(const Matrix32& p_transform);
+	void set_transform(const Matrix32 &p_transform);
 
 	void set_space(Space2DSW *p_space);
 
-
 	void call_queries();
 
 	Area2DSW();
 	~Area2DSW();
 };
 
-void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 
-	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].inc();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
 }
-void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) {
+void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 
-	BodyKey bk(p_body,p_body_shape,p_area_shape);
+	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].dec();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
 }
 
-void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
+void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 
-
-	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].inc();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
-
-
 }
-void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
-
+void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 
-	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].dec();
 	if (!monitor_query_list.in_list())
 		_queue_monitor_update();
 }
 
-
-
-
-
 #endif // AREA_2D_SW_H
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index c26f6c45f..db845a6d7 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -29,124 +29,105 @@
 #include "area_pair_2d_sw.h"
 #include "collision_solver_2d_sw.h"
 
-
 bool AreaPair2DSW::setup(float p_step) {
 
-	bool result = area->test_collision_mask(body) &&  CollisionSolver2DSW::solve(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),Vector2(),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),Vector2(),NULL,this);
+	bool result = area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), NULL, this);
 
-	if (result!=colliding) {
+	if (result != colliding) {
 
 		if (result) {
 
-			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+			if (area->get_space_override_mode() != Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
 				body->add_area(area);
 			if (area->has_monitor_callback())
-				area->add_body_to_query(body,body_shape,area_shape);
+				area->add_body_to_query(body, body_shape, area_shape);
 
 		} else {
 
-			if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+			if (area->get_space_override_mode() != Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
 				body->remove_area(area);
 			if (area->has_monitor_callback())
-				area->remove_body_from_query(body,body_shape,area_shape);
-
+				area->remove_body_from_query(body, body_shape, area_shape);
 		}
 
-		colliding=result;
-
+		colliding = result;
 	}
 
 	return false; //never do any post solving
 }
 
 void AreaPair2DSW::solve(float p_step) {
-
-
 }
 
+AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) {
 
-AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape) {
-
-
-	body=p_body;
-	area=p_area;
-	body_shape=p_body_shape;
-	area_shape=p_area_shape;
-	colliding=false;
-	body->add_constraint(this,0);
+	body = p_body;
+	area = p_area;
+	body_shape = p_body_shape;
+	area_shape = p_area_shape;
+	colliding = false;
+	body->add_constraint(this, 0);
 	area->add_constraint(this);
-	if (p_body->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) //need to be active to process pair
+	if (p_body->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) //need to be active to process pair
 		p_body->set_active(true);
-
 }
 
 AreaPair2DSW::~AreaPair2DSW() {
 
 	if (colliding) {
 
-		if (area->get_space_override_mode()!=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
+		if (area->get_space_override_mode() != Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED)
 			body->remove_area(area);
 		if (area->has_monitor_callback())
-			area->remove_body_from_query(body,body_shape,area_shape);
-
-
+			area->remove_body_from_query(body, body_shape, area_shape);
 	}
 	body->remove_constraint(this);
 	area->remove_constraint(this);
 }
 
-
 //////////////////////////////////
 
-
-
 bool Area2Pair2DSW::setup(float p_step) {
 
-	bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
+	bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), NULL, this);
 
-	if (result!=colliding) {
+	if (result != colliding) {
 
 		if (result) {
 
 			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-				area_b->add_area_to_query(area_a,shape_a,shape_b);
+				area_b->add_area_to_query(area_a, shape_a, shape_b);
 
 			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-				area_a->add_area_to_query(area_b,shape_b,shape_a);
+				area_a->add_area_to_query(area_b, shape_b, shape_a);
 
 		} else {
 
 			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-				area_b->remove_area_from_query(area_a,shape_a,shape_b);
+				area_b->remove_area_from_query(area_a, shape_a, shape_b);
 
 			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-				area_a->remove_area_from_query(area_b,shape_b,shape_a);
+				area_a->remove_area_from_query(area_b, shape_b, shape_a);
 		}
 
-		colliding=result;
-
+		colliding = result;
 	}
 
 	return false; //never do any post solving
 }
 
 void Area2Pair2DSW::solve(float p_step) {
-
-
 }
 
+Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) {
 
-Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b) {
-
-
-	area_a=p_area_a;
-	area_b=p_area_b;
-	shape_a=p_shape_a;
-	shape_b=p_shape_b;
-	colliding=false;
+	area_a = p_area_a;
+	area_b = p_area_b;
+	shape_a = p_shape_a;
+	shape_b = p_shape_b;
+	colliding = false;
 	area_a->add_constraint(this);
 	area_b->add_constraint(this);
-
 }
 
 Area2Pair2DSW::~Area2Pair2DSW() {
@@ -154,10 +135,10 @@ Area2Pair2DSW::~Area2Pair2DSW() {
 	if (colliding) {
 
 		if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
-			area_b->remove_area_from_query(area_a,shape_a,shape_b);
+			area_b->remove_area_from_query(area_a, shape_a, shape_b);
 
 		if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
-			area_a->remove_area_from_query(area_b,shape_b,shape_a);
+			area_a->remove_area_from_query(area_b, shape_b, shape_a);
 	}
 
 	area_a->remove_constraint(this);
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
index db77bff5d..1478fcb82 100644
--- a/servers/physics_2d/area_pair_2d_sw.h
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -29,9 +29,9 @@
 #ifndef AREA_PAIR_2D_SW_H
 #define AREA_PAIR_2D_SW_H
 
-#include "constraint_2d_sw.h"
-#include "body_2d_sw.h"
 #include "area_2d_sw.h"
+#include "body_2d_sw.h"
+#include "constraint_2d_sw.h"
 
 class AreaPair2DSW : public Constraint2DSW {
 
@@ -40,16 +40,15 @@ class AreaPair2DSW : public Constraint2DSW {
 	int body_shape;
 	int area_shape;
 	bool colliding;
-public:
 
+public:
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	AreaPair2DSW(Body2DSW *p_body,int p_body_shape, Area2DSW *p_area,int p_area_shape);
+	AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape);
 	~AreaPair2DSW();
 };
 
-
 class Area2Pair2DSW : public Constraint2DSW {
 
 	Area2DSW *area_a;
@@ -57,15 +56,13 @@ class Area2Pair2DSW : public Constraint2DSW {
 	int shape_a;
 	int shape_b;
 	bool colliding;
-public:
 
+public:
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b);
+	Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b);
 	~Area2Pair2DSW();
 };
 
-
 #endif // AREA_PAIR_2D_SW_H
-
diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 0e3a0b7a7..f127d6881 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -27,107 +27,98 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 #include "body_2d_sw.h"
-#include "space_2d_sw.h"
 #include "area_2d_sw.h"
 #include "physics_2d_server_sw.h"
+#include "space_2d_sw.h"
 
 void Body2DSW::_update_inertia() {
 
 	if (!user_inertia && get_space() && !inertia_update_list.in_list())
 		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
-
 }
 
-
-
 void Body2DSW::update_inertias() {
 
 	//update shapes and motions
 
-	switch(mode) {
+	switch (mode) {
 
 		case Physics2DServer::BODY_MODE_RIGID: {
 
-			if(user_inertia) break;
+			if (user_inertia) break;
 
 			//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
-			float total_area=0;
+			float total_area = 0;
 
-			for (int i=0;i<get_shape_count();i++) {
+			for (int i = 0; i < get_shape_count(); i++) {
 
-				total_area+=get_shape_aabb(i).get_area();
+				total_area += get_shape_aabb(i).get_area();
 			}
 
-			real_t _inertia=0;
+			real_t _inertia = 0;
 
-			for (int i=0;i<get_shape_count();i++) {
+			for (int i = 0; i < get_shape_count(); i++) {
 
-				const Shape2DSW* shape=get_shape(i);
+				const Shape2DSW *shape = get_shape(i);
 
-				float area=get_shape_aabb(i).get_area();
+				float area = get_shape_aabb(i).get_area();
 
 				float mass = area * this->mass / total_area;
 
 				Matrix32 mtx = get_shape_transform(i);
 				Vector2 scale = mtx.get_scale();
-				_inertia += shape->get_moment_of_inertia(mass,scale) + mass * mtx.get_origin().length_squared();
+				_inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
 				//Rect2 ab = get_shape_aabb(i);
 				//_inertia+=mass*ab.size.dot(ab.size)/12.0f;
-
-
-
 			}
 
-			if (_inertia!=0)
-				_inv_inertia=1.0/_inertia;
+			if (_inertia != 0)
+				_inv_inertia = 1.0 / _inertia;
 			else
-				_inv_inertia=0.0; //wathever
+				_inv_inertia = 0.0; //wathever
 
 			if (mass)
-				_inv_mass=1.0/mass;
+				_inv_mass = 1.0 / mass;
 			else
-				_inv_mass=0;
+				_inv_mass = 0;
 
 		} break;
 		case Physics2DServer::BODY_MODE_KINEMATIC:
 		case Physics2DServer::BODY_MODE_STATIC: {
 
-			_inv_inertia=0;
-			_inv_mass=0;
+			_inv_inertia = 0;
+			_inv_mass = 0;
 		} break;
 		case Physics2DServer::BODY_MODE_CHARACTER: {
 
-			_inv_inertia=0;
-			_inv_mass=1.0/mass;
+			_inv_inertia = 0;
+			_inv_mass = 1.0 / mass;
 
 		} break;
 	}
 	//_update_inertia_tensor();
 
 	//_update_shapes();
-
 }
 
-
-
 void Body2DSW::set_active(bool p_active) {
 
-	if (active==p_active)
+	if (active == p_active)
 		return;
 
-	active=p_active;
+	active = p_active;
 	if (!p_active) {
 		if (get_space())
 			get_space()->body_remove_from_active_list(&active_list);
 	} else {
-		if (mode==Physics2DServer::BODY_MODE_STATIC)
+		if (mode == Physics2DServer::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;
 
@@ -140,27 +131,25 @@ void Body2DSW::set_active(bool p_active) {
 */
 }
 
-
-
 void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 		case Physics2DServer::BODY_PARAM_BOUNCE: {
 
-			bounce=p_value;
+			bounce = p_value;
 		} break;
 		case Physics2DServer::BODY_PARAM_FRICTION: {
 
-			friction=p_value;
+			friction = p_value;
 		} break;
 		case Physics2DServer::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value<=0);
-			mass=p_value;
+			ERR_FAIL_COND(p_value <= 0);
+			mass = p_value;
 			_update_inertia();
 
 		} break;
 		case Physics2DServer::BODY_PARAM_INERTIA: {
-			if(p_value<=0) {
+			if (p_value <= 0) {
 				user_inertia = false;
 				_update_inertia();
 			} else {
@@ -169,23 +158,23 @@ void Body2DSW::set_param(Physics2DServer::BodyParameter p_param, float p_value)
 			}
 		} break;
 		case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
-			gravity_scale=p_value;
+			gravity_scale = p_value;
 		} break;
 		case Physics2DServer::BODY_PARAM_LINEAR_DAMP: {
 
-			linear_damp=p_value;
+			linear_damp = p_value;
 		} break;
 		case Physics2DServer::BODY_PARAM_ANGULAR_DAMP: {
 
-			angular_damp=p_value;
+			angular_damp = p_value;
 		} break;
-		default:{}
+		default: {}
 	}
 }
 
 float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 		case Physics2DServer::BODY_PARAM_BOUNCE: {
 
 			return bounce;
@@ -198,7 +187,7 @@ float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 			return mass;
 		} break;
 		case Physics2DServer::BODY_PARAM_INERTIA: {
-			return _inv_inertia==0 ? 0 : 1.0 / _inv_inertia;
+			return _inv_inertia == 0 ? 0 : 1.0 / _inv_inertia;
 		} break;
 		case Physics2DServer::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
@@ -211,7 +200,7 @@ float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 
 			return angular_damp;
 		} break;
-		default:{}
+		default: {}
 	}
 
 	return 0;
@@ -219,41 +208,40 @@ float Body2DSW::get_param(Physics2DServer::BodyParameter p_param) const {
 
 void Body2DSW::set_mode(Physics2DServer::BodyMode p_mode) {
 
-	Physics2DServer::BodyMode prev=mode;
-	mode=p_mode;
+	Physics2DServer::BodyMode prev = mode;
+	mode = p_mode;
 
-	switch(p_mode) {
-	//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+	switch (p_mode) {
+		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
 		case Physics2DServer::BODY_MODE_STATIC:
 		case Physics2DServer::BODY_MODE_KINEMATIC: {
 
 			_set_inv_transform(get_transform().affine_inverse());
-			_inv_mass=0;
-			_set_static(p_mode==Physics2DServer::BODY_MODE_STATIC);
-			set_active(p_mode==Physics2DServer::BODY_MODE_KINEMATIC && contacts.size());
-			linear_velocity=Vector2();
-			angular_velocity=0;
-			if (mode==Physics2DServer::BODY_MODE_KINEMATIC && prev!=mode) {
-				first_time_kinematic=true;
+			_inv_mass = 0;
+			_set_static(p_mode == Physics2DServer::BODY_MODE_STATIC);
+			set_active(p_mode == Physics2DServer::BODY_MODE_KINEMATIC && contacts.size());
+			linear_velocity = Vector2();
+			angular_velocity = 0;
+			if (mode == Physics2DServer::BODY_MODE_KINEMATIC && prev != mode) {
+				first_time_kinematic = true;
 			}
 		} break;
 		case Physics2DServer::BODY_MODE_RIGID: {
 
-			_inv_mass=mass>0?(1.0/mass):0;
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 
 		} break;
 		case Physics2DServer::BODY_MODE_CHARACTER: {
 
-			_inv_mass=mass>0?(1.0/mass):0;
+			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
 			_set_static(false);
 		} break;
 	}
 
 	_update_inertia();
 	//if (get_space())
-//		_update_queries();
-
+	//		_update_queries();
 }
 Physics2DServer::BodyMode Body2DSW::get_mode() const {
 
@@ -266,35 +254,33 @@ void Body2DSW::_shapes_changed() {
 	wakeup_neighbours();
 }
 
-void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_variant) {
+void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant &p_variant) {
 
-	switch(p_state)	{
+	switch (p_state) {
 		case Physics2DServer::BODY_STATE_TRANSFORM: {
 
+			if (mode == Physics2DServer::BODY_MODE_KINEMATIC) {
 
-			if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
-
-				new_transform=p_variant;
+				new_transform = p_variant;
 				//wakeup_neighbours();
 				set_active(true);
 				if (first_time_kinematic) {
 					_set_transform(p_variant);
 					_set_inv_transform(get_transform().affine_inverse());
-					first_time_kinematic=false;
+					first_time_kinematic = false;
 				}
-			} else if (mode==Physics2DServer::BODY_MODE_STATIC) {
+			} else if (mode == Physics2DServer::BODY_MODE_STATIC) {
 				_set_transform(p_variant);
 				_set_inv_transform(get_transform().affine_inverse());
 				wakeup_neighbours();
 			} else {
 				Matrix32 t = p_variant;
 				t.orthonormalize();
-				new_transform=get_transform(); //used as old to compute motion
-				if (t==new_transform)
+				new_transform = get_transform(); //used as old to compute motion
+				if (t == new_transform)
 					break;
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
-
 			}
 			wakeup();
 
@@ -303,45 +289,44 @@ void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant& p_va
 
 			//if (mode==Physics2DServer::BODY_MODE_STATIC)
 			//	break;
-			linear_velocity=p_variant;
+			linear_velocity = p_variant;
 			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_ANGULAR_VELOCITY: {
 			//if (mode!=Physics2DServer::BODY_MODE_RIGID)
 			//	break;
-			angular_velocity=p_variant;
+			angular_velocity = p_variant;
 			wakeup();
 
 		} break;
 		case Physics2DServer::BODY_STATE_SLEEPING: {
 			//?
-			if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+			if (mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC)
 				break;
-			bool do_sleep=p_variant;
+			bool do_sleep = p_variant;
 			if (do_sleep) {
-				linear_velocity=Vector2();
+				linear_velocity = Vector2();
 				//biased_linear_velocity=Vector3();
-				angular_velocity=0;
+				angular_velocity = 0;
 				//biased_angular_velocity=Vector3();
 				set_active(false);
 			} else {
-				if (mode!=Physics2DServer::BODY_MODE_STATIC)
+				if (mode != Physics2DServer::BODY_MODE_STATIC)
 					set_active(true);
 			}
 		} break;
 		case Physics2DServer::BODY_STATE_CAN_SLEEP: {
-			can_sleep=p_variant;
-			if (mode==Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
+			can_sleep = p_variant;
+			if (mode == Physics2DServer::BODY_MODE_RIGID && !active && !can_sleep)
 				set_active(true);
 
 		} break;
 	}
-
 }
 Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
 
-	switch(p_state)	{
+	switch (p_state) {
 		case Physics2DServer::BODY_STATE_TRANSFORM: {
 			return get_transform();
 		} break;
@@ -362,8 +347,7 @@ Variant Body2DSW::get_state(Physics2DServer::BodyState p_state) const {
 	return Variant();
 }
 
-
-void Body2DSW::set_space(Space2DSW *p_space){
+void Body2DSW::set_space(Space2DSW *p_space) {
 
 	if (get_space()) {
 
@@ -375,7 +359,6 @@ void Body2DSW::set_space(Space2DSW *p_space){
 			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);
@@ -385,23 +368,22 @@ void Body2DSW::set_space(Space2DSW *p_space){
 		_update_inertia();
 		if (active)
 			get_space()->body_add_to_active_list(&active_list);
-//		_update_queries();
+		//		_update_queries();
 		//if (is_active()) {
 		//	active=false;
 		//	set_active(true);
 		//}
-
 	}
 
-	first_integration=false;
+	first_integration = false;
 }
 
 void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) {
 
 	if (p_area->is_gravity_point()) {
-		if(p_area->get_gravity_distance_scale() > 0) {
+		if (p_area->get_gravity_distance_scale() > 0) {
 			Vector2 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
-			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) );
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
 		} else {
 			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
 		}
@@ -415,7 +397,7 @@ void Body2DSW::_compute_area_gravity_and_dampenings(const Area2DSW *p_area) {
 
 void Body2DSW::integrate_forces(real_t p_step) {
 
-	if (mode==Physics2DServer::BODY_MODE_STATIC)
+	if (mode == Physics2DServer::BODY_MODE_STATIC)
 		return;
 
 	Area2DSW *def_area = get_space()->get_default_area();
@@ -424,62 +406,62 @@ void Body2DSW::integrate_forces(real_t p_step) {
 
 	int ac = areas.size();
 	bool stopped = false;
-	gravity = Vector2(0,0);
+	gravity = Vector2(0, 0);
 	area_angular_damp = 0;
 	area_linear_damp = 0;
 	if (ac) {
 		areas.sort();
 		const AreaCMP *aa = &areas[0];
 		// damp_area = aa[ac-1].area;
-		for(int i=ac-1;i>=0 && !stopped;i--) {
-			Physics2DServer::AreaSpaceOverrideMode mode=aa[i].area->get_space_override_mode();
+		for (int i = ac - 1; i >= 0 && !stopped; i--) {
+			Physics2DServer::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
 			switch (mode) {
 				case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE:
 				case Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
 					_compute_area_gravity_and_dampenings(aa[i].area);
-					stopped = mode==Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+					stopped = mode == Physics2DServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
 				} break;
 				case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE:
 				case Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
-					gravity = Vector2(0,0);
+					gravity = Vector2(0, 0);
 					area_angular_damp = 0;
 					area_linear_damp = 0;
 					_compute_area_gravity_and_dampenings(aa[i].area);
-					stopped = mode==Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE;
+					stopped = mode == Physics2DServer::AREA_SPACE_OVERRIDE_REPLACE;
 				} break;
 				default: {}
 			}
 		}
 	}
-	if( !stopped ) {
+	if (!stopped) {
 		_compute_area_gravity_and_dampenings(def_area);
 	}
-	gravity*=gravity_scale;
+	gravity *= gravity_scale;
 
 	// If less than 0, override dampenings with that of the Body2D
-	if (angular_damp>=0)
+	if (angular_damp >= 0)
 		area_angular_damp = angular_damp;
 	//else
 	//	area_angular_damp=damp_area->get_angular_damp();
 
-	if (linear_damp>=0)
+	if (linear_damp >= 0)
 		area_linear_damp = linear_damp;
 	//else
 	//	area_linear_damp=damp_area->get_linear_damp();
 
 	Vector2 motion;
-	bool do_motion=false;
+	bool do_motion = false;
 
-	if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
+	if (mode == Physics2DServer::BODY_MODE_KINEMATIC) {
 
 		//compute motion, angular and etc. velocities from prev transform
-		linear_velocity = (new_transform.elements[2] - get_transform().elements[2])/p_step;
+		linear_velocity = (new_transform.elements[2] - get_transform().elements[2]) / p_step;
 
 		real_t rot = new_transform.affine_inverse().basis_xform(get_transform().elements[1]).angle();
 		angular_velocity = rot / p_step;
 
 		motion = new_transform.elements[2] - get_transform().elements[2];
-		do_motion=true;
+		do_motion = true;
 
 		//for(int i=0;i<get_shape_count();i++) {
 		//	set_shape_kinematic_advance(i,Vector2());
@@ -490,102 +472,96 @@ void Body2DSW::integrate_forces(real_t p_step) {
 		if (!omit_force_integration && !first_integration) {
 			//overriden by direct state query
 
-			Vector2 force=gravity*mass;
-			force+=applied_force;
-			real_t torque=applied_torque;
+			Vector2 force = gravity * mass;
+			force += applied_force;
+			real_t torque = applied_torque;
 
 			real_t damp = 1.0 - p_step * area_linear_damp;
 
-			if (damp<0) // reached zero in the given time
-				damp=0;
+			if (damp < 0) // reached zero in the given time
+				damp = 0;
 
 			real_t angular_damp = 1.0 - p_step * area_angular_damp;
 
-			if (angular_damp<0) // reached zero in the given time
-				angular_damp=0;
+			if (angular_damp < 0) // reached zero in the given time
+				angular_damp = 0;
 
-			linear_velocity*=damp;
-			angular_velocity*=angular_damp;
+			linear_velocity *= damp;
+			angular_velocity *= angular_damp;
 
-			linear_velocity+=_inv_mass * force * p_step;
-			angular_velocity+=_inv_inertia * torque * p_step;
+			linear_velocity += _inv_mass * force * p_step;
+			angular_velocity += _inv_inertia * torque * p_step;
 		}
 
-		if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED) {
+		if (continuous_cd_mode != Physics2DServer::CCD_MODE_DISABLED) {
 
 			motion = new_transform.get_origin() - get_transform().get_origin();
 			//linear_velocity*p_step;
-			do_motion=true;
+			do_motion = true;
 		}
 	}
 
-
 	//motion=linear_velocity*p_step;
 
-	first_integration=false;
-	biased_angular_velocity=0;
-	biased_linear_velocity=Vector2();
+	first_integration = false;
+	biased_angular_velocity = 0;
+	biased_linear_velocity = Vector2();
 
-	if (do_motion) {//shapes temporarily extend for raycast
+	if (do_motion) { //shapes temporarily extend for raycast
 		_update_shapes_with_motion(motion);
 	}
 
 	// damp_area=NULL; // clear the area, so it is set in the next frame
-	def_area=NULL; // clear the area, so it is set in the next frame
-	contact_count=0;
-
+	def_area = NULL; // clear the area, so it is set in the next frame
+	contact_count = 0;
 }
 
 void Body2DSW::integrate_velocities(real_t p_step) {
 
-	if (mode==Physics2DServer::BODY_MODE_STATIC)
+	if (mode == Physics2DServer::BODY_MODE_STATIC)
 		return;
 
 	if (fi_callback)
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 
-	if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
+	if (mode == Physics2DServer::BODY_MODE_KINEMATIC) {
 
-		_set_transform(new_transform,false);
+		_set_transform(new_transform, false);
 		_set_inv_transform(new_transform.affine_inverse());
-		if (contacts.size()==0 && linear_velocity==Vector2() && angular_velocity==0)
+		if (contacts.size() == 0 && linear_velocity == Vector2() && angular_velocity == 0)
 			set_active(false); //stopped moving, deactivate
 		return;
 	}
 
-	real_t total_angular_velocity = angular_velocity+biased_angular_velocity;
-	Vector2 total_linear_velocity=linear_velocity+biased_linear_velocity;
+	real_t total_angular_velocity = angular_velocity + biased_angular_velocity;
+	Vector2 total_linear_velocity = linear_velocity + biased_linear_velocity;
 
 	real_t angle = get_transform().get_rotation() - total_angular_velocity * p_step;
 	Vector2 pos = get_transform().get_origin() + total_linear_velocity * p_step;
 
-	_set_transform(Matrix32(angle,pos),continuous_cd_mode==Physics2DServer::CCD_MODE_DISABLED);
+	_set_transform(Matrix32(angle, pos), continuous_cd_mode == Physics2DServer::CCD_MODE_DISABLED);
 	_set_inv_transform(get_transform().inverse());
 
-	if (continuous_cd_mode!=Physics2DServer::CCD_MODE_DISABLED)
-		new_transform=get_transform();
+	if (continuous_cd_mode != Physics2DServer::CCD_MODE_DISABLED)
+		new_transform = get_transform();
 
 	//_update_inertia_tensor();
 }
 
-
-
 void Body2DSW::wakeup_neighbours() {
 
+	for (Map<Constraint2DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) {
 
-
-	for(Map<Constraint2DSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
-
-		const Constraint2DSW *c=E->key();
+		const Constraint2DSW *c = E->key();
 		Body2DSW **n = c->get_body_ptr();
-		int bc=c->get_body_count();
+		int bc = c->get_body_count();
 
-		for(int i=0;i<bc;i++) {
+		for (int i = 0; i < bc; i++) {
 
-			if (i==E->get())
+			if (i == E->get())
 				continue;
 			Body2DSW *b = n[i];
-			if (b->mode!=Physics2DServer::BODY_MODE_RIGID)
+			if (b->mode != Physics2DServer::BODY_MODE_RIGID)
 				continue;
 
 			if (!b->is_active())
@@ -596,116 +572,103 @@ void Body2DSW::wakeup_neighbours() {
 
 void Body2DSW::call_queries() {
 
-
 	if (fi_callback) {
 
 		Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;
-		dbs->body=this;
-
-		Variant v=dbs;
-		const Variant *vp[2]={&v,&fi_callback->callback_udata};
+		dbs->body = this;
 
+		Variant v = dbs;
+		const Variant *vp[2] = { &v, &fi_callback->callback_udata };
 
 		Object *obj = ObjectDB::get_instance(fi_callback->id);
 		if (!obj) {
 
-			set_force_integration_callback(0,StringName());
+			set_force_integration_callback(0, StringName());
 		} else {
 			Variant::CallError ce;
 			if (fi_callback->callback_udata.get_type()) {
 
-				obj->call(fi_callback->method,vp,2,ce);
+				obj->call(fi_callback->method, vp, 2, ce);
 
 			} else {
-				obj->call(fi_callback->method,vp,1,ce);
+				obj->call(fi_callback->method, vp, 1, ce);
 			}
 		}
-
-
 	}
-
 }
 
+bool Body2DSW::sleep_test(real_t p_step) {
 
-bool Body2DSW::sleep_test(real_t p_step)  {
-
-	if (mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+	if (mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC)
 		return true; //
-	else if (mode==Physics2DServer::BODY_MODE_CHARACTER)
+	else if (mode == Physics2DServer::BODY_MODE_CHARACTER)
 		return !active; // characters and kinematic bodies don't sleep unless asked to sleep
 	else if (!can_sleep)
 		return false;
 
+	if (Math::abs(angular_velocity) < 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()) {
 
-
-
-	if (Math::abs(angular_velocity)<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;
+		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?
+		still_time = 0; //maybe this should be set to 0 on set_active?
 		return false;
 	}
 }
 
-
-void Body2DSW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+void Body2DSW::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
 
 	if (fi_callback) {
 
 		memdelete(fi_callback);
-		fi_callback=NULL;
+		fi_callback = NULL;
 	}
 
+	if (p_id != 0) {
 
-	if (p_id!=0) {
-
-		fi_callback=memnew(ForceIntegrationCallback);
-		fi_callback->id=p_id;
-		fi_callback->method=p_method;
-		fi_callback->callback_udata=p_udata;
+		fi_callback = memnew(ForceIntegrationCallback);
+		fi_callback->id = p_id;
+		fi_callback->method = p_method;
+		fi_callback->callback_udata = p_udata;
 	}
-
 }
 
-Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
-
+Body2DSW::Body2DSW()
+	: CollisionObject2DSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
 
-	mode=Physics2DServer::BODY_MODE_RIGID;
-	active=true;
-	angular_velocity=0;
-	biased_angular_velocity=0;
-	mass=1;
-	user_inertia=false;
-	_inv_inertia=0;
-	_inv_mass=1;
-	bounce=0;
-	friction=1;
-	omit_force_integration=false;
-	applied_torque=0;
-	island_step=0;
-	island_next=NULL;
-	island_list_next=NULL;
+	mode = Physics2DServer::BODY_MODE_RIGID;
+	active = true;
+	angular_velocity = 0;
+	biased_angular_velocity = 0;
+	mass = 1;
+	user_inertia = false;
+	_inv_inertia = 0;
+	_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);
-	first_time_kinematic=false;
-	linear_damp=-1;
-	angular_damp=-1;
-	area_angular_damp=0;
-	area_linear_damp=0;
-	contact_count=0;
-	gravity_scale=1.0;
-	using_one_way_cache=false;
-	one_way_collision_max_depth=0.1;
-	first_integration=false;
-
-	still_time=0;
-	continuous_cd_mode=Physics2DServer::CCD_MODE_DISABLED;
-	can_sleep=false;
-	fi_callback=NULL;
+	first_time_kinematic = false;
+	linear_damp = -1;
+	angular_damp = -1;
+	area_angular_damp = 0;
+	area_linear_damp = 0;
+	contact_count = 0;
+	gravity_scale = 1.0;
+	using_one_way_cache = false;
+	one_way_collision_max_depth = 0.1;
+	first_integration = false;
 
+	still_time = 0;
+	continuous_cd_mode = Physics2DServer::CCD_MODE_DISABLED;
+	can_sleep = false;
+	fi_callback = NULL;
 }
 
 Body2DSW::~Body2DSW() {
@@ -714,17 +677,16 @@ Body2DSW::~Body2DSW() {
 		memdelete(fi_callback);
 }
 
-Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton=NULL;
+Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton = NULL;
 
-Physics2DDirectSpaceState* Physics2DDirectBodyStateSW::get_space_state() {
+Physics2DDirectSpaceState *Physics2DDirectBodyStateSW::get_space_state() {
 
 	return body->get_space()->get_direct_state();
 }
 
-
 Variant Physics2DDirectBodyStateSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
 
-	ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Variant());
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
 
 	if (!Physics2DServerSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
 
@@ -733,11 +695,10 @@ Variant Physics2DDirectBodyStateSW::get_contact_collider_shape_metadata(int p_co
 	Body2DSW *other = Physics2DServerSW::singletonsw->body_owner.get(body->contacts[p_contact_idx].collider);
 
 	int sidx = body->contacts[p_contact_idx].collider_shape;
-	if (sidx<0 || sidx>=other->get_shape_count()) {
+	if (sidx < 0 || sidx >= other->get_shape_count()) {
 
 		return Variant();
 	}
 
-
 	return other->get_shape_metadata(sidx);
 }
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index ea42e604a..a8d8cc407 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -29,16 +29,14 @@
 #ifndef BODY_2D_SW_H
 #define BODY_2D_SW_H
 
+#include "area_2d_sw.h"
 #include "collision_object_2d_sw.h"
 #include "vset.h"
-#include "area_2d_sw.h"
 
 class Constraint2DSW;
 
-
 class Body2DSW : public CollisionObject2DSW {
 
-
 	Physics2DServer::BodyMode mode;
 
 	Vector2 biased_linear_velocity;
@@ -71,7 +69,6 @@ class Body2DSW : public CollisionObject2DSW {
 	Vector2 one_way_collision_direction;
 	float one_way_collision_max_depth;
 
-
 	SelfList<Body2DSW> active_list;
 	SelfList<Body2DSW> inertia_update_list;
 	SelfList<Body2DSW> direct_state_query_list;
@@ -88,25 +85,25 @@ class Body2DSW : public CollisionObject2DSW {
 	virtual void _shapes_changed();
 	Matrix32 new_transform;
 
-
-	Map<Constraint2DSW*,int> constraint_map;
+	Map<Constraint2DSW *, int> constraint_map;
 
 	struct AreaCMP {
 
 		Area2DSW *area;
 		int refCount;
-		_FORCE_INLINE_ bool operator==(const AreaCMP& p_cmp) const { return area->get_self() == p_cmp.area->get_self();}
-		_FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_priority() < p_cmp.area->get_priority();}
+		_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
+		_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
 		_FORCE_INLINE_ AreaCMP() {}
-		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { area=p_area; refCount=1;}
+		_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) {
+			area = p_area;
+			refCount = 1;
+		}
 	};
 
-
 	Vector<AreaCMP> areas;
 
 	struct Contact {
 
-
 		Vector2 local_pos;
 		Vector2 local_normal;
 		float depth;
@@ -130,24 +127,20 @@ class Body2DSW : public CollisionObject2DSW {
 
 	ForceIntegrationCallback *fi_callback;
 
-
 	uint64_t island_step;
 	Body2DSW *island_next;
 	Body2DSW *island_list_next;
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area);
 
-friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
+	friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
 
 public:
-
-
-	void set_force_integration_callback(ObjectID p_id, const StringName& p_method, const Variant &p_udata=Variant());
-
+	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
 
 	_FORCE_INLINE_ void add_area(Area2DSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
-		if( index > -1 ) {
+		if (index > -1) {
 			areas[index].refCount += 1;
 		} else {
 			areas.ordered_insert(AreaCMP(p_area));
@@ -156,62 +149,64 @@ public:
 
 	_FORCE_INLINE_ void remove_area(Area2DSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
-		if( index > -1 ) {
+		if (index > -1) {
 			areas[index].refCount -= 1;
-			if( areas[index].refCount < 1 )
+			if (areas[index].refCount < 1)
 				areas.remove(index);
 		}
 	}
 
-	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; if (mode==Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);}
+	_FORCE_INLINE_ void set_max_contacts_reported(int p_size) {
+		contacts.resize(p_size);
+		contact_count = 0;
+		if (mode == Physics2DServer::BODY_MODE_KINEMATIC && p_size) set_active(true);
+	}
 
 	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
 
 	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
-	_FORCE_INLINE_ void add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos);
+	_FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, float p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos);
 
-
-	_FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
-	_FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
-	_FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
-	_FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
+	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
+	_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
+	_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
+	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
 
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
-	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
-	_FORCE_INLINE_ Body2DSW* get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(Body2DSW* p_next) { island_next=p_next; }
+	_FORCE_INLINE_ Body2DSW *get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(Body2DSW *p_next) { island_next = p_next; }
 
-	_FORCE_INLINE_ Body2DSW* get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(Body2DSW* p_next) { island_list_next=p_next; }
+	_FORCE_INLINE_ Body2DSW *get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(Body2DSW *p_next) { island_list_next = p_next; }
 
-	_FORCE_INLINE_ void add_constraint(Constraint2DSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
-	_FORCE_INLINE_ void remove_constraint(Constraint2DSW* p_constraint) { constraint_map.erase(p_constraint); }
-	const Map<Constraint2DSW*,int>& get_constraint_map() const { return constraint_map; }
+	_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
+	_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint); }
+	const Map<Constraint2DSW *, int> &get_constraint_map() const { return constraint_map; }
 
-	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+	_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_ void set_linear_velocity(const Vector2& p_velocity) {linear_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_linear_velocity(const Vector2 &p_velocity) { linear_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector2 get_linear_velocity() const { return linear_velocity; }
 
-	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_angular_velocity(real_t p_velocity) { angular_velocity = p_velocity; }
 	_FORCE_INLINE_ real_t get_angular_velocity() const { return angular_velocity; }
 
-	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2& p_velocity) {biased_linear_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_biased_linear_velocity(const Vector2 &p_velocity) { biased_linear_velocity = p_velocity; }
 	_FORCE_INLINE_ Vector2 get_biased_linear_velocity() const { return biased_linear_velocity; }
 
-	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity=p_velocity; }
+	_FORCE_INLINE_ void set_biased_angular_velocity(real_t p_velocity) { biased_angular_velocity = p_velocity; }
 	_FORCE_INLINE_ real_t get_biased_angular_velocity() const { return biased_angular_velocity; }
 
-
-	_FORCE_INLINE_ void apply_impulse(const Vector2& p_offset, const Vector2& p_impulse) {
+	_FORCE_INLINE_ void apply_impulse(const Vector2 &p_offset, const Vector2 &p_impulse) {
 
 		linear_velocity += p_impulse * _inv_mass;
 		angular_velocity += _inv_inertia * p_offset.cross(p_impulse);
 	}
 
-	_FORCE_INLINE_ void apply_bias_impulse(const Vector2& p_pos, const Vector2& p_j) {
+	_FORCE_INLINE_ void apply_bias_impulse(const Vector2 &p_pos, const Vector2 &p_j) {
 
 		biased_linear_velocity += p_j * _inv_mass;
 		biased_angular_velocity += _inv_inertia * p_pos.cross(p_j);
@@ -221,45 +216,42 @@ public:
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
 	_FORCE_INLINE_ void wakeup() {
-		if ((!get_space()) || mode==Physics2DServer::BODY_MODE_STATIC || mode==Physics2DServer::BODY_MODE_KINEMATIC)
+		if ((!get_space()) || mode == Physics2DServer::BODY_MODE_STATIC || mode == Physics2DServer::BODY_MODE_KINEMATIC)
 			return;
 		set_active(true);
 	}
 
-
-
-
 	void set_param(Physics2DServer::BodyParameter p_param, float);
 	float get_param(Physics2DServer::BodyParameter p_param) const;
 
 	void set_mode(Physics2DServer::BodyMode p_mode);
 	Physics2DServer::BodyMode get_mode() const;
 
-	void set_state(Physics2DServer::BodyState p_state, const Variant& p_variant);
+	void set_state(Physics2DServer::BodyState p_state, const Variant &p_variant);
 	Variant get_state(Physics2DServer::BodyState p_state) const;
 
-	void set_applied_force(const Vector2& p_force) { applied_force=p_force; }
+	void set_applied_force(const Vector2 &p_force) { applied_force = p_force; }
 	Vector2 get_applied_force() const { return applied_force; }
 
-	void set_applied_torque(real_t p_torque) { applied_torque=p_torque; }
+	void set_applied_torque(real_t p_torque) { applied_torque = p_torque; }
 	real_t get_applied_torque() const { return applied_torque; }
 
-	_FORCE_INLINE_ void add_force(const Vector2& p_force, const Vector2& p_offset) {
+	_FORCE_INLINE_ void add_force(const Vector2 &p_force, const Vector2 &p_offset) {
 
 		applied_force += p_force;
 		applied_torque += p_offset.cross(p_force);
 	}
 
-	_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; }
+	_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode = p_mode; }
 	_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
 
-	void set_one_way_collision_direction(const Vector2& p_dir) {
-		one_way_collision_direction=p_dir;
-		using_one_way_cache=one_way_collision_direction!=Vector2();
+	void set_one_way_collision_direction(const Vector2 &p_dir) {
+		one_way_collision_direction = p_dir;
+		using_one_way_cache = one_way_collision_direction != Vector2();
 	}
 	Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; }
 
-	void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth=p_depth; }
+	void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth = p_depth; }
 	float get_one_way_collision_max_depth() const { return one_way_collision_max_depth; }
 
 	_FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; }
@@ -276,16 +268,15 @@ public:
 	_FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
 	_FORCE_INLINE_ real_t get_angular_damp() const { return angular_damp; }
 
-
 	void integrate_forces(real_t p_step);
 	void integrate_velocities(real_t p_step);
 
 	_FORCE_INLINE_ Vector2 get_motion() const {
 
-		if (mode>Physics2DServer::BODY_MODE_KINEMATIC) {
+		if (mode > Physics2DServer::BODY_MODE_KINEMATIC) {
 			return new_transform.get_origin() - get_transform().get_origin();
-		} else if (mode==Physics2DServer::BODY_MODE_KINEMATIC) {
-			return  get_transform().get_origin() -new_transform.get_origin(); //kinematic simulates forward
+		} else if (mode == Physics2DServer::BODY_MODE_KINEMATIC) {
+			return get_transform().get_origin() - new_transform.get_origin(); //kinematic simulates forward
 		}
 		return Vector2();
 	}
@@ -297,111 +288,127 @@ public:
 
 	Body2DSW();
 	~Body2DSW();
-
 };
 
-
 //add contact inline
 
-void Body2DSW::add_contact(const Vector2& p_local_pos,const Vector2& p_local_normal, float p_depth, int p_local_shape, const Vector2& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector2& p_collider_velocity_at_pos) {
+void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, float p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) {
 
-	int c_max=contacts.size();
+	int c_max = contacts.size();
 
-	if (c_max==0)
+	if (c_max == 0)
 		return;
 
 	Contact *c = &contacts[0];
 
+	int idx = -1;
 
-	int idx=-1;
-
-	if (contact_count<c_max) {
-		idx=contact_count++;
+	if (contact_count < c_max) {
+		idx = contact_count++;
 	} else {
 
-		float least_depth=1e20;
-		int least_deep=-1;
-		for(int i=0;i<c_max;i++) {
+		float least_depth = 1e20;
+		int least_deep = -1;
+		for (int i = 0; i < c_max; i++) {
 
-			if (i==0 || c[i].depth<least_depth) {
-				least_deep=i;
-				least_depth=c[i].depth;
+			if (i == 0 || c[i].depth < least_depth) {
+				least_deep = i;
+				least_depth = c[i].depth;
 			}
 		}
 
-		if (least_deep>=0 && least_depth<p_depth) {
+		if (least_deep >= 0 && least_depth < p_depth) {
 
-			idx=least_deep;
+			idx = least_deep;
 		}
-		if (idx==-1)
+		if (idx == -1)
 			return; //none least deepe than this
 	}
 
-	c[idx].local_pos=p_local_pos;
-	c[idx].local_normal=p_local_normal;
-	c[idx].depth=p_depth;
-	c[idx].local_shape=p_local_shape;
-	c[idx].collider_pos=p_collider_pos;
-	c[idx].collider_shape=p_collider_shape;
-	c[idx].collider_instance_id=p_collider_instance_id;
-	c[idx].collider=p_collider;
-	c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
-
+	c[idx].local_pos = p_local_pos;
+	c[idx].local_normal = p_local_normal;
+	c[idx].depth = p_depth;
+	c[idx].local_shape = p_local_shape;
+	c[idx].collider_pos = p_collider_pos;
+	c[idx].collider_shape = p_collider_shape;
+	c[idx].collider_instance_id = p_collider_instance_id;
+	c[idx].collider = p_collider;
+	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
 }
 
-
 class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
 
-	OBJ_TYPE( Physics2DDirectBodyStateSW, Physics2DDirectBodyState );
+	OBJ_TYPE(Physics2DDirectBodyStateSW, Physics2DDirectBodyState);
 
 public:
-
 	static Physics2DDirectBodyStateSW *singleton;
 	Body2DSW *body;
 	real_t step;
 
-	virtual Vector2 get_total_gravity() const {  return body->gravity;  } // get gravity vector working on this body space/area
-	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 Vector2 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area
+	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 float get_inverse_mass() const {  return body->get_inv_mass();  } // get the mass
-	virtual real_t get_inverse_inertia() const { return body->get_inv_inertia();   } // get density of this body space
+	virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
+	virtual real_t get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
 
-	virtual void set_linear_velocity(const Vector2& p_velocity) {  body->set_linear_velocity(p_velocity);  }
-	virtual Vector2 get_linear_velocity() const {  return body->get_linear_velocity();  }
+	virtual void set_linear_velocity(const Vector2 &p_velocity) { body->set_linear_velocity(p_velocity); }
+	virtual Vector2 get_linear_velocity() const { return body->get_linear_velocity(); }
 
-	virtual void set_angular_velocity(real_t p_velocity) {  body->set_angular_velocity(p_velocity);  }
-	virtual real_t get_angular_velocity() const {  return body->get_angular_velocity();  }
+	virtual void set_angular_velocity(real_t p_velocity) { body->set_angular_velocity(p_velocity); }
+	virtual real_t get_angular_velocity() const { return body->get_angular_velocity(); }
 
-	virtual void set_transform(const Matrix32& p_transform) {  body->set_state(Physics2DServer::BODY_STATE_TRANSFORM,p_transform);  }
-	virtual Matrix32 get_transform() const {  return body->get_transform();  }
+	virtual void set_transform(const Matrix32 &p_transform) { body->set_state(Physics2DServer::BODY_STATE_TRANSFORM, p_transform); }
+	virtual Matrix32 get_transform() const { return body->get_transform(); }
 
-	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
-	virtual bool is_sleeping() const {  return !body->is_active();  }
+	virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
+	virtual bool is_sleeping() const { return !body->is_active(); }
 
-	virtual int get_contact_count() const {  return body->contact_count;  }
+	virtual int get_contact_count() const { return body->contact_count; }
 
 	virtual Vector2 get_contact_local_pos(int p_contact_idx) const {
-		ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
 		return body->contacts[p_contact_idx].local_pos;
 	}
-	virtual Vector2 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].local_normal;   }
-	virtual int get_contact_local_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1);   return body->contacts[p_contact_idx].local_shape;   }
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+		return body->contacts[p_contact_idx].local_normal;
+	}
+	virtual int get_contact_local_shape(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
+		return body->contacts[p_contact_idx].local_shape;
+	}
 
-	virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID());   return body->contacts[p_contact_idx].collider;   }
-	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2());   return body->contacts[p_contact_idx].collider_pos;  }
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0);   return body->contacts[p_contact_idx].collider_instance_id;   }
-	virtual int get_contact_collider_shape(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape;  }
+	virtual RID get_contact_collider(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
+		return body->contacts[p_contact_idx].collider;
+	}
+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+		return body->contacts[p_contact_idx].collider_pos;
+	}
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+		return body->contacts[p_contact_idx].collider_instance_id;
+	}
+	virtual int get_contact_collider_shape(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+		return body->contacts[p_contact_idx].collider_shape;
+	}
 	virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const;
 
-	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const {  ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector2()); return body->contacts[p_contact_idx].collider_velocity_at_pos;  }
-
-	virtual Physics2DDirectSpaceState* get_space_state();
+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const {
+		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+		return body->contacts[p_contact_idx].collider_velocity_at_pos;
+	}
 
+	virtual Physics2DDirectSpaceState *get_space_state();
 
 	virtual real_t get_step() const { return step; }
-	Physics2DDirectBodyStateSW() { singleton=this; body=NULL; }
+	Physics2DDirectBodyStateSW() {
+		singleton = this;
+		body = NULL;
+	}
 };
 
-
 #endif // BODY_2D_SW_H
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 6fa4f154d..1f2cacd2b 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -30,54 +30,52 @@
 #include "collision_solver_2d_sw.h"
 #include "space_2d_sw.h"
 
-
 #define POSITION_CORRECTION
 #define ACCUMULATE_IMPULSES
 
-void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) {
+void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
 
 	BodyPair2DSW *self = (BodyPair2DSW *)p_self;
 
-	self->_contact_added_callback(p_point_A,p_point_B);
-
+	self->_contact_added_callback(p_point_A, p_point_B);
 }
 
-void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) {
+void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
 
 	// check if we already have the contact
 
 	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
-	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B);
+	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
 
 	int new_index = contact_count;
 
-	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+	ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
 
 	Contact contact;
 
-	contact.acc_normal_impulse=0;
-	contact.acc_bias_impulse=0;
-	contact.acc_tangent_impulse=0;
-	contact.local_A=local_A;
-	contact.local_B=local_B;
-	contact.reused=true;
-	contact.normal=(p_point_A-p_point_B).normalized();
+	contact.acc_normal_impulse = 0;
+	contact.acc_bias_impulse = 0;
+	contact.acc_tangent_impulse = 0;
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.reused = true;
+	contact.normal = (p_point_A - p_point_B).normalized();
 
 	// attempt to determine if the contact will be reused
 
 	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
 
-	for (int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 		if (
-		c.local_A.distance_squared_to( local_A ) < (recycle_radius_2) &&
-		c.local_B.distance_squared_to( local_B ) < (recycle_radius_2) ) {
+				c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
+				c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
 
-			contact.acc_normal_impulse=c.acc_normal_impulse;
-			contact.acc_tangent_impulse=c.acc_tangent_impulse;
-			contact.acc_bias_impulse=c.acc_bias_impulse;
-			new_index=i;
+			contact.acc_normal_impulse = c.acc_normal_impulse;
+			contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			contact.acc_bias_impulse = c.acc_bias_impulse;
+			new_index = i;
 			break;
 		}
 	}
@@ -88,44 +86,41 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
 
 		// remove the contact with the minimum depth
 
-		int least_deep=-1;
-		real_t min_depth=1e10;
+		int least_deep = -1;
+		real_t min_depth = 1e10;
 
+		for (int i = 0; i <= contact_count; i++) {
 
-		for (int i=0;i<=contact_count;i++) {
-
-			Contact& c = (i==contact_count)?contact:contacts[i];
+			Contact &c = (i == contact_count) ? contact : contacts[i];
 			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
 
 			Vector2 axis = global_A - global_B;
-			float depth = axis.dot( c.normal );
-
+			float depth = axis.dot(c.normal);
 
-			if (depth<min_depth) {
+			if (depth < min_depth) {
 
-				min_depth=depth;
-				least_deep=i;
+				min_depth = depth;
+				least_deep = i;
 			}
 		}
 
-		ERR_FAIL_COND(least_deep==-1);
+		ERR_FAIL_COND(least_deep == -1);
 
 		if (least_deep < contact_count) { //replace the last deep contact by the new one
 
-			contacts[least_deep]=contact;
+			contacts[least_deep] = contact;
 		}
 
 		return;
 	}
 
-	contacts[new_index]=contact;
+	contacts[new_index] = contact;
 
-	if (new_index==contact_count) {
+	if (new_index == contact_count) {
 
 		contact_count++;
 	}
-
 }
 
 void BodyPair2DSW::_validate_contacts() {
@@ -133,39 +128,35 @@ void BodyPair2DSW::_validate_contacts() {
 	//make sure to erase contacts that are no longer valid
 
 	real_t max_separation = space->get_contact_max_separation();
-	real_t max_separation2 = max_separation*max_separation;
+	real_t max_separation2 = max_separation * max_separation;
 
-	for (int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 
-		bool erase=false;
-		if (c.reused==false) {
+		bool erase = false;
+		if (c.reused == false) {
 			//was left behind in previous frame
-			erase=true;
+			erase = true;
 		} else {
-			c.reused=false;
+			c.reused = false;
 
 			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
 			Vector2 axis = global_A - global_B;
-			float depth = axis.dot( c.normal );
-
-
+			float depth = axis.dot(c.normal);
 
 			if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
-				erase=true;
+				erase = true;
 			}
 		}
 
 		if (erase) {
 			// contact no longer needed, remove
 
-
-			if ((i+1) < contact_count) {
+			if ((i + 1) < contact_count) {
 				// swap with the last one
-				SWAP( contacts[i], contacts[ contact_count-1 ] );
-
+				SWAP(contacts[i], contacts[contact_count - 1]);
 			}
 
 			i--;
@@ -174,21 +165,18 @@ void BodyPair2DSW::_validate_contacts() {
 	}
 }
 
+bool BodyPair2DSW::_test_ccd(float p_step, Body2DSW *p_A, int p_shape_A, const Matrix32 &p_xform_A, Body2DSW *p_B, int p_shape_B, const Matrix32 &p_xform_B, bool p_swap_result) {
 
-bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) {
-
-
-
-	Vector2 motion = p_A->get_linear_velocity()*p_step;
+	Vector2 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
-	if (mlen<CMP_EPSILON)
+	if (mlen < CMP_EPSILON)
 		return false;
 
 	Vector2 mnormal = motion / mlen;
 
-	real_t min,max;
-	p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
-	bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+	real_t min, max;
+	p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
+	bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
 
 	if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
 		return false;
@@ -198,22 +186,21 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
 	//support is the worst case collision point, so real collision happened before
 	int a;
 	Vector2 s[2];
-	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
 	Vector2 from = p_xform_A.xform(s[0]);
 	Vector2 to = from + motion;
 
 	Matrix32 from_inv = p_xform_B.affine_inverse();
 
-	Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+	Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
 	Vector2 local_to = from_inv.xform(to);
 
-	Vector2 rpos,rnorm;
-	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
+	Vector2 rpos, rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm))
 		return false;
 
 	//ray hit something
 
-
 	Vector2 hitpos = p_xform_B.xform(rpos);
 
 	Vector2 contact_A = to;
@@ -222,20 +209,18 @@ bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Mat
 	//create a contact
 
 	if (p_swap_result)
-		_contact_added_callback(contact_B,contact_A);
+		_contact_added_callback(contact_B, contact_A);
 	else
-		_contact_added_callback(contact_A,contact_B);
-
+		_contact_added_callback(contact_A, contact_B);
 
 	return true;
 }
 
 bool BodyPair2DSW::setup(float p_step) {
 
-
 	//cannot collide
-	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
-		collided=false;
+	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+		collided = false;
 		return false;
 	}
 
@@ -249,44 +234,43 @@ bool BodyPair2DSW::setup(float p_step) {
 	Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
 
 	Matrix32 xform_Bu = B->get_transform();
-	xform_Bu.elements[2]-=A->get_transform().get_origin();
+	xform_Bu.elements[2] -= A->get_transform().get_origin();
 	Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
-	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
-	Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
+	Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
+	Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
 
-	Vector2 motion_A,motion_B;
+	Vector2 motion_A, motion_B;
 
-	if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
-		motion_A=A->get_motion();
+	if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_A = A->get_motion();
 	}
-	if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
-		motion_B=B->get_motion();
+	if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_B = B->get_motion();
 	}
 	//faster to set than to check..
 
 	//bool prev_collided=collided;
 
-	collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis);
+	collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
 	if (!collided) {
 
 		//test ccd (currently just a raycast)
 
-		if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
-			if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B))
-				collided=true;
+		if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B))
+				collided = true;
 		}
 
-		if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
-			if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true))
-				collided=true;
+		if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true))
+				collided = true;
 		}
 
 		if (!collided) {
-			oneway_disabled=false;
+			oneway_disabled = false;
 			return false;
 		}
-
 	}
 
 	if (oneway_disabled)
@@ -297,45 +281,45 @@ bool BodyPair2DSW::setup(float p_step) {
 
 		if (A->is_using_one_way_collision()) {
 			Vector2 direction = A->get_one_way_collision_direction();
-			bool valid=false;
-			if (B->get_linear_velocity().dot(direction)>=0){
-				for(int i=0;i<contact_count;i++) {
-					Contact& c = contacts[i];
+			bool valid = false;
+			if (B->get_linear_velocity().dot(direction) >= 0) {
+				for (int i = 0; i < contact_count; i++) {
+					Contact &c = contacts[i];
 					if (!c.reused)
 						continue;
-					if (c.normal.dot(direction)<0)
+					if (c.normal.dot(direction) < 0)
 						continue;
 
-					valid=true;
+					valid = true;
 					break;
 				}
 			}
 
 			if (!valid) {
-				collided=false;
-				oneway_disabled=true;
+				collided = false;
+				oneway_disabled = true;
 				return false;
 			}
 		}
 
 		if (B->is_using_one_way_collision()) {
 			Vector2 direction = B->get_one_way_collision_direction();
-			bool valid=false;
-			if (A->get_linear_velocity().dot(direction)>=0){
-				for(int i=0;i<contact_count;i++) {
-					Contact& c = contacts[i];
+			bool valid = false;
+			if (A->get_linear_velocity().dot(direction) >= 0) {
+				for (int i = 0; i < contact_count; i++) {
+					Contact &c = contacts[i];
 					if (!c.reused)
 						continue;
-					if (c.normal.dot(direction)<0)
+					if (c.normal.dot(direction) < 0)
 						continue;
 
-					valid=true;
+					valid = true;
 					break;
 				}
 			}
 			if (!valid) {
-				collided=false;
-				oneway_disabled=true;
+				collided = false;
+				oneway_disabled = true;
 				return false;
 			}
 		}
@@ -346,72 +330,69 @@ bool BodyPair2DSW::setup(float p_step) {
 	float bias = 0.3f;
 	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
 
-		if (shape_A_ptr->get_custom_bias()==0)
-			bias=shape_B_ptr->get_custom_bias();
-		else if (shape_B_ptr->get_custom_bias()==0)
-			bias=shape_A_ptr->get_custom_bias();
+		if (shape_A_ptr->get_custom_bias() == 0)
+			bias = shape_B_ptr->get_custom_bias();
+		else if (shape_B_ptr->get_custom_bias() == 0)
+			bias = shape_A_ptr->get_custom_bias();
 		else
-			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
+			bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
 	}
 
+	cc = 0;
 
-	cc=0;
-
+	real_t inv_dt = 1.0 / p_step;
 
-	real_t inv_dt = 1.0/p_step;
-
-	bool do_process=false;
+	bool do_process = false;
 
 	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 
 		Vector2 global_A = xform_Au.xform(c.local_A);
 		Vector2 global_B = xform_Bu.xform(c.local_B);
 
 		real_t depth = c.normal.dot(global_A - global_B);
 
-		if (depth<=0 || !c.reused) {
-			c.active=false;
+		if (depth <= 0 || !c.reused) {
+			c.active = false;
 			continue;
 		}
 
-		c.active=true;
+		c.active = true;
 #ifdef DEBUG_ENABLED
 		if (space->is_debugging_contacts()) {
-			space->add_debug_contact(global_A+offset_A);
-			space->add_debug_contact(global_B+offset_A);
+			space->add_debug_contact(global_A + offset_A);
+			space->add_debug_contact(global_B + offset_A);
 		}
 #endif
 		int gather_A = A->can_report_contacts();
 		int gather_B = B->can_report_contacts();
 
 		c.rA = global_A;
-		c.rB = global_B-offset_B;
+		c.rB = global_B - offset_B;
 
 		if (gather_A | gather_B) {
 
 			//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
 
-			global_A+=offset_A;
-			global_B+=offset_A;
+			global_A += offset_A;
+			global_B += offset_A;
 
 			if (gather_A) {
-				Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
-				A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity());
+				Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
+				A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
 			}
 			if (gather_B) {
 
-				Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-				B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity());
+				Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+				B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
 			}
 		}
 
-		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) {
-			c.active=false;
-			collided=false;
+		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) {
+			c.active = false;
+			collided = false;
 			continue;
-
 		}
 
 		// Precompute normal mass, tangent mass, and bias.
@@ -426,39 +407,33 @@ bool BodyPair2DSW::setup(float p_step) {
 		real_t rtB = c.rB.dot(tangent);
 		real_t kTangent = A->get_inv_mass() + B->get_inv_mass();
 		kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB);
-		c.mass_tangent = 1.0f /  kTangent;
-
-
+		c.mass_tangent = 1.0f / kTangent;
 
 		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
-		c.depth=depth;
-		//c.acc_bias_impulse=0;
-
+		c.depth = depth;
+//c.acc_bias_impulse=0;
 
 #ifdef ACCUMULATE_IMPULSES
 		{
 			// Apply normal + friction impulse
 			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
 
-
-			A->apply_impulse(c.rA,-P);
+			A->apply_impulse(c.rA, -P);
 			B->apply_impulse(c.rB, P);
 		}
 
 #endif
 
-
-		c.bounce=MAX(A->get_bounce(),B->get_bounce());
+		c.bounce = MAX(A->get_bounce(), B->get_bounce());
 		if (c.bounce) {
 
-			Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-			Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
 			Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}
 
-		do_process=true;
-
+		do_process = true;
 	}
 
 	return do_process;
@@ -471,81 +446,71 @@ void BodyPair2DSW::solve(float p_step) {
 
 	for (int i = 0; i < contact_count; ++i) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 		cc++;
 
 		if (!c.active)
 			continue;
 
-
 		// Relative velocity at contact
 
-		Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-		Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+		Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+		Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
 		Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 
-		Vector2 crbA( -A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x );
-		Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x );
+		Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
+		Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
 		Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
 
-
 		real_t vn = dv.dot(c.normal);
 		real_t vbn = dbv.dot(c.normal);
 		Vector2 tangent = c.normal.tangent();
 		real_t vt = dv.dot(tangent);
 
-
-		real_t jbn = (c.bias - vbn)*c.mass_normal;
+		real_t jbn = (c.bias - vbn) * c.mass_normal;
 		real_t jbnOld = c.acc_bias_impulse;
 		c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
 
 		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-		A->apply_bias_impulse(c.rA,-jb);
+		A->apply_bias_impulse(c.rA, -jb);
 		B->apply_bias_impulse(c.rB, jb);
 
-		real_t jn = -(c.bounce + vn)*c.mass_normal;
+		real_t jn = -(c.bounce + vn) * c.mass_normal;
 		real_t jnOld = c.acc_normal_impulse;
 		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
 
-
 		real_t friction = A->get_friction() * B->get_friction();
 
-		real_t jtMax = friction*c.acc_normal_impulse;
-		real_t jt = -vt*c.mass_tangent;
+		real_t jtMax = friction * c.acc_normal_impulse;
+		real_t jt = -vt * c.mass_tangent;
 		real_t jtOld = c.acc_tangent_impulse;
 		c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
 
-		Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld );
+		Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
 
-		A->apply_impulse(c.rA,-j);
+		A->apply_impulse(c.rA, -j);
 		B->apply_impulse(c.rB, j);
-
-
 	}
 }
 
+BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B)
+	: Constraint2DSW(_arr, 2) {
 
-BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) {
-
-	A=p_A;
-	B=p_B;
-	shape_A=p_shape_A;
-	shape_B=p_shape_B;
-	space=A->get_space();
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
-	contact_count=0;
-	collided=false;
-	oneway_disabled=false;
-
+	A = p_A;
+	B = p_B;
+	shape_A = p_shape_A;
+	shape_B = p_shape_B;
+	space = A->get_space();
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+	contact_count = 0;
+	collided = false;
+	oneway_disabled = false;
 }
 
-
 BodyPair2DSW::~BodyPair2DSW() {
 
-
 	A->remove_constraint(this);
 	B->remove_constraint(this);
-
 }
diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h
index a219b99fd..e9e2bb477 100644
--- a/servers/physics_2d/body_pair_2d_sw.h
+++ b/servers/physics_2d/body_pair_2d_sw.h
@@ -35,7 +35,7 @@
 class BodyPair2DSW : public Constraint2DSW {
 
 	enum {
-		MAX_CONTACTS=2
+		MAX_CONTACTS = 2
 	};
 	union {
 		struct {
@@ -56,18 +56,17 @@ class BodyPair2DSW : public Constraint2DSW {
 		Vector2 position;
 		Vector2 normal;
 		Vector2 local_A, local_B;
-		real_t acc_normal_impulse;	// accumulated normal impulse (Pn)
-		real_t acc_tangent_impulse;	// accumulated tangent impulse (Pt)
-		real_t acc_bias_impulse;	// accumulated normal impulse for position bias (Pnb)
+		real_t acc_normal_impulse; // accumulated normal impulse (Pn)
+		real_t acc_tangent_impulse; // accumulated tangent impulse (Pt)
+		real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
 		real_t mass_normal, mass_tangent;
 		real_t bias;
 
 		real_t depth;
 		bool active;
-		Vector2 rA,rB;
+		Vector2 rA, rB;
 		bool reused;
 		float bounce;
-
 	};
 
 	Vector2 offset_B; //use local A coordinates to avoid numerical issues on collision detection
@@ -79,20 +78,17 @@ class BodyPair2DSW : public Constraint2DSW {
 	bool oneway_disabled;
 	int cc;
 
-
-	bool _test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result=false);
+	bool _test_ccd(float p_step, Body2DSW *p_A, int p_shape_A, const Matrix32 &p_xform_A, Body2DSW *p_B, int p_shape_B, const Matrix32 &p_xform_B, bool p_swap_result = false);
 	void _validate_contacts();
-	static void _add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self);
-	_FORCE_INLINE_ void _contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B);
+	static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self);
+	_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
 
 public:
-
 	bool setup(float p_step);
 	void solve(float p_step);
 
-	BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B);
+	BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B);
 	~BodyPair2DSW();
-
 };
 
 #endif // BODY_PAIR_2D_SW_H
diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp
index de2e89ce8..4fc7bcc19 100644
--- a/servers/physics_2d/broad_phase_2d_basic.cpp
+++ b/servers/physics_2d/broad_phase_2d_basic.cpp
@@ -30,94 +30,87 @@
 
 ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
 
-
 	current++;
 
 	Element e;
-	e.owner=p_object_;
-	e._static=false;
-	e.subindex=p_subindex;
+	e.owner = p_object_;
+	e._static = false;
+	e.subindex = p_subindex;
 
-	element_map[current]=e;
+	element_map[current] = e;
 	return current;
 }
 
-void BroadPhase2DBasic::move(ID p_id, const Rect2& p_aabb) {
+void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	E->get().aabb=p_aabb;
-
+	E->get().aabb = p_aabb;
 }
 void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
-	E->get()._static=p_static;
-
+	E->get()._static = p_static;
 }
 void BroadPhase2DBasic::remove(ID p_id) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 	element_map.erase(E);
-
 }
 
 CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,NULL);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, NULL);
 	return E->get().owner;
-
 }
 bool BroadPhase2DBasic::is_static(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,false);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, false);
 	return E->get()._static;
-
 }
 int BroadPhase2DBasic::get_subindex(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,-1);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, -1);
 	return E->get().subindex;
 }
 
-int BroadPhase2DBasic::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
 
-	int rc=0;
+	int rc = 0;
 
-	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
 
-		const Rect2 aabb=E->get().aabb;
-		if (aabb.intersects_segment(p_from,p_to)) {
+		const Rect2 aabb = E->get().aabb;
+		if (aabb.intersects_segment(p_from, p_to)) {
 
-			p_results[rc]=E->get().owner;
-			p_result_indices[rc]=E->get().subindex;
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
 			rc++;
-			if (rc>=p_max_results)
+			if (rc >= p_max_results)
 				break;
 		}
 	}
 
 	return rc;
-
 }
-int BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
 
-	int rc=0;
+	int rc = 0;
 
-	for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
+	for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
 
-		const Rect2 aabb=E->get().aabb;
+		const Rect2 aabb = E->get().aabb;
 		if (aabb.intersects(p_aabb)) {
 
-			p_results[rc]=E->get().owner;
-			p_result_indices[rc]=E->get().subindex;
+			p_results[rc] = E->get().owner;
+			p_result_indices[rc] = E->get().subindex;
 			rc++;
-			if (rc>=p_max_results)
+			if (rc >= p_max_results)
 				break;
 		}
 	}
@@ -125,68 +118,63 @@ int BroadPhase2DBasic::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_res
 	return rc;
 }
 
-void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
-
-	pair_userdata=p_userdata;
-	pair_callback=p_pair_callback;
+void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 
+	pair_userdata = p_userdata;
+	pair_callback = p_pair_callback;
 }
-void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) {
-
-	unpair_userdata=p_userdata;
-	unpair_callback=p_pair_callback;
+void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_pair_callback, void *p_userdata) {
 
+	unpair_userdata = p_userdata;
+	unpair_callback = p_pair_callback;
 }
 
 void BroadPhase2DBasic::update() {
 
 	// recompute pairs
-	for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) {
+	for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
 
-		for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) {
+		for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
 
-			Element *elem_A=&I->get();
-			Element *elem_B=&J->get();
+			Element *elem_A = &I->get();
+			Element *elem_B = &J->get();
 
 			if (elem_A->owner == elem_B->owner)
 				continue;
 
+			bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
 
-			bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static );
+			PairKey key(I->key(), J->key());
 
-			PairKey key(I->key(),J->key());
-
-			Map<PairKey,void*>::Element *E=pair_map.find(key);
+			Map<PairKey, void *>::Element *E = pair_map.find(key);
 
 			if (!pair_ok && E) {
 				if (unpair_callback)
-					unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata);
+					unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
 				pair_map.erase(key);
 			}
 
 			if (pair_ok && !E) {
 
-				void *data=NULL;
+				void *data = NULL;
 				if (pair_callback)
-					data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata);
-				pair_map.insert(key,data);
+					data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
+				pair_map.insert(key, data);
 			}
 		}
 	}
-
 }
 
 BroadPhase2DSW *BroadPhase2DBasic::_create() {
 
-	return memnew( BroadPhase2DBasic );
+	return memnew(BroadPhase2DBasic);
 }
 
 BroadPhase2DBasic::BroadPhase2DBasic() {
 
-	current=1;
-	unpair_callback=NULL;
-	unpair_userdata=NULL;
-	pair_callback=NULL;
-	pair_userdata=NULL;
-
+	current = 1;
+	unpair_callback = NULL;
+	unpair_userdata = NULL;
+	pair_callback = NULL;
+	pair_userdata = NULL;
 }
diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h
index 82e91118c..7c5d799f9 100644
--- a/servers/physics_2d/broad_phase_2d_basic.h
+++ b/servers/physics_2d/broad_phase_2d_basic.h
@@ -29,8 +29,8 @@
 #ifndef BROAD_PHASE_2D_BASIC_H
 #define BROAD_PHASE_2D_BASIC_H
 
-#include "space_2d_sw.h"
 #include "map.h"
+#include "space_2d_sw.h"
 class BroadPhase2DBasic : public BroadPhase2DSW {
 
 	struct Element {
@@ -41,8 +41,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW {
 		int subindex;
 	};
 
-
-	Map<ID,Element> element_map;
+	Map<ID, Element> element_map;
 
 	ID current;
 
@@ -56,17 +55,23 @@ class BroadPhase2DBasic : public BroadPhase2DSW {
 			uint64_t key;
 		};
 
-		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+		_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
 			return key < p_key.key;
 		}
 
-		PairKey() { key=0; }
-		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
-
+		PairKey() { key = 0; }
+		PairKey(ID p_a, ID p_b) {
+			if (p_a > p_b) {
+				a = p_b;
+				b = p_a;
+			} else {
+				a = p_a;
+				b = p_b;
+			}
+		}
 	};
 
-	Map<PairKey,void*> pair_map;
-
+	Map<PairKey, void *> pair_map;
 
 	PairCallback pair_callback;
 	void *pair_userdata;
@@ -74,10 +79,9 @@ class BroadPhase2DBasic : public BroadPhase2DSW {
 	void *unpair_userdata;
 
 public:
-
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
-	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const Rect2 &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 
@@ -85,11 +89,11 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
-	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
-	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
+	virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
 
 	virtual void update();
 
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 82d777357..a1996a5d9 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
@@ -31,110 +31,100 @@
 
 #define LARGE_ELEMENT_FI 1.01239812
 
-void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element* p_with) {
+void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {
 
-	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
 
 	ERR_FAIL_COND(p_elem->_static && p_with->_static);
 
 	if (!E) {
 
-		PairData *pd = memnew( PairData );
-		p_elem->paired[p_with]=pd;
-		p_with->paired[p_elem]=pd;
+		PairData *pd = memnew(PairData);
+		p_elem->paired[p_with] = pd;
+		p_with->paired[p_elem] = pd;
 	} else {
 		E->get()->rc++;
 	}
-
 }
 
-void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element* p_with) {
+void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {
 
-	Map<Element*,PairData*>::Element *E=p_elem->paired.find(p_with);
+	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
 
 	ERR_FAIL_COND(!E); //this should really be paired..
 
 	E->get()->rc--;
 
-	if (E->get()->rc==0) {
+	if (E->get()->rc == 0) {
 
 		if (E->get()->colliding) {
 			//uncollide
 			if (unpair_callback) {
-				unpair_callback(p_elem->owner,p_elem->subindex,p_with->owner,p_with->subindex,E->get()->ud,unpair_userdata);
+				unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata);
 			}
-
-
 		}
 
 		memdelete(E->get());
 		p_elem->paired.erase(E);
 		p_with->paired.erase(p_elem);
 	}
-
-
 }
 
 void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
 
-	for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
+	for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
 
-		bool pairing = p_elem->aabb.intersects( E->key()->aabb );
+		bool pairing = p_elem->aabb.intersects(E->key()->aabb);
 
-		if (pairing!=E->get()->colliding) {
+		if (pairing != E->get()->colliding) {
 
 			if (pairing) {
 
 				if (pair_callback) {
-					E->get()->ud=pair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,pair_userdata);
+					E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
 				}
 			} else {
 
 				if (unpair_callback) {
-					unpair_callback(p_elem->owner,p_elem->subindex,E->key()->owner,E->key()->subindex,E->get()->ud,unpair_userdata);
+					unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
 				}
-
 			}
 
-			E->get()->colliding=pairing;
+			E->get()->colliding = pairing;
 		}
 	}
 }
 
-void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
-
-
+void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
 
-	Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
-	if (sz.width*sz.height > large_object_min_surface) {
+	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
+	if (sz.width * sz.height > large_object_min_surface) {
 		//large object, do not use grid, must check against all elements
-		for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) {
-			if (E->key()==p_elem->self)
+		for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
+			if (E->key() == p_elem->self)
 				continue; // do not pair against itself
 			if (E->get().owner == p_elem->owner)
 				continue;
 			if (E->get()._static && p_static)
 				continue;
 
-			_pair_attempt(p_elem,&E->get());
+			_pair_attempt(p_elem, &E->get());
 		}
 
-
 		large_elements[p_elem].inc();
 		return;
 	}
 
-	Point2i from = (p_rect.pos/cell_size).floor();
-	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
-
-	for(int i=from.x;i<=to.x;i++) {
+	Point2i from = (p_rect.pos / cell_size).floor();
+	Point2i to = ((p_rect.pos + p_rect.size) / cell_size).floor();
 
+	for (int i = from.x; i <= to.x; i++) {
 
-		for(int j=from.y;j<=to.y;j++) {
+		for (int j = from.y; j <= to.y; j++) {
 
 			PosKey pk;
-			pk.x=i;
-			pk.y=j;
+			pk.x = i;
+			pk.y = j;
 
 			uint32_t idx = pk.hash() % hash_table_size;
 			PosBin *pb = hash_table[idx];
@@ -145,102 +135,94 @@ void BroadPhase2DHashGrid::_enter_grid( Element* p_elem, const Rect2& p_rect,boo
 					break;
 				}
 
-				pb=pb->next;
+				pb = pb->next;
 			}
 
-
-			bool entered=false;
+			bool entered = false;
 
 			if (!pb) {
 				//does not exist, create!
-				pb = memnew( PosBin );
-				pb->key=pk;
-				pb->next=hash_table[idx];
-				hash_table[idx]=pb;
+				pb = memnew(PosBin);
+				pb->key = pk;
+				pb->next = hash_table[idx];
+				hash_table[idx] = pb;
 			}
 
-
-
 			if (p_static) {
-				if (pb->static_object_set[p_elem].inc()==1) {
-					entered=true;
+				if (pb->static_object_set[p_elem].inc() == 1) {
+					entered = true;
 				}
 			} else {
-				if (pb->object_set[p_elem].inc()==1) {
+				if (pb->object_set[p_elem].inc() == 1) {
 
-					entered=true;
+					entered = true;
 				}
 			}
 
 			if (entered) {
 
-				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
 
-					if (E->key()->owner==p_elem->owner)
+					if (E->key()->owner == p_elem->owner)
 						continue;
-					_pair_attempt(p_elem,E->key());
+					_pair_attempt(p_elem, E->key());
 				}
 
 				if (!p_static) {
 
-					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
 
-						if (E->key()->owner==p_elem->owner)
+						if (E->key()->owner == p_elem->owner)
 							continue;
-						_pair_attempt(p_elem,E->key());
+						_pair_attempt(p_elem, E->key());
 					}
 				}
 			}
-
 		}
-
 	}
 
 	//pair separatedly with large elements
 
-	for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
 
-		if (E->key()==p_elem)
+		if (E->key() == p_elem)
 			continue; // do not pair against itself
 		if (E->key()->owner == p_elem->owner)
 			continue;
 		if (E->key()->_static && p_static)
 			continue;
 
-		_pair_attempt(E->key(),p_elem);
+		_pair_attempt(E->key(), p_elem);
 	}
-
 }
 
+void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {
 
-void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool p_static) {
-
-	Vector2 sz = (p_rect.size/cell_size*LARGE_ELEMENT_FI);
-	if (sz.width*sz.height > large_object_min_surface) {
+	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
+	if (sz.width * sz.height > large_object_min_surface) {
 
 		//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
-		for (Map<Element*,PairData*>::Element *E=p_elem->paired.front();E;E=E->next()) {
+		for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
 
-			_unpair_attempt(p_elem,E->key());
+			_unpair_attempt(p_elem, E->key());
 		}
 
-		if (large_elements[p_elem].dec()==0) {
+		if (large_elements[p_elem].dec() == 0) {
 			large_elements.erase(p_elem);
 		}
 		return;
 	}
 
+	Point2i from = (p_rect.pos / cell_size).floor();
+	Point2i to = ((p_rect.pos + p_rect.size) / cell_size).floor();
 
-	Point2i from = (p_rect.pos/cell_size).floor();
-	Point2i to = ((p_rect.pos+p_rect.size)/cell_size).floor();
-
-	for(int i=from.x;i<=to.x;i++) {
+	for (int i = from.x; i <= to.x; i++) {
 
-		for(int j=from.y;j<=to.y;j++) {
+		for (int j = from.y; j <= to.y; j++) {
 
 			PosKey pk;
-			pk.x=i;
-			pk.y=j;
+			pk.x = i;
+			pk.y = j;
 
 			uint32_t idx = pk.hash() % hash_table_size;
 			PosBin *pb = hash_table[idx];
@@ -251,82 +233,75 @@ void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool
 					break;
 				}
 
-				pb=pb->next;
+				pb = pb->next;
 			}
 
 			ERR_CONTINUE(!pb); //should exist!!
 
-			bool exited=false;
-
+			bool exited = false;
 
 			if (p_static) {
-				if (pb->static_object_set[p_elem].dec()==0) {
+				if (pb->static_object_set[p_elem].dec() == 0) {
 
 					pb->static_object_set.erase(p_elem);
-					exited=true;
-
+					exited = true;
 				}
 			} else {
-				if (pb->object_set[p_elem].dec()==0) {
+				if (pb->object_set[p_elem].dec() == 0) {
 
 					pb->object_set.erase(p_elem);
-					exited=true;
-
+					exited = true;
 				}
 			}
 
 			if (exited) {
 
-				for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
+				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
 
-					if (E->key()->owner==p_elem->owner)
+					if (E->key()->owner == p_elem->owner)
 						continue;
-					_unpair_attempt(p_elem,E->key());
-
+					_unpair_attempt(p_elem, E->key());
 				}
 
 				if (!p_static) {
 
-					for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
+					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
 
-						if (E->key()->owner==p_elem->owner)
+						if (E->key()->owner == p_elem->owner)
 							continue;
-						_unpair_attempt(p_elem,E->key());
+						_unpair_attempt(p_elem, E->key());
 					}
 				}
 			}
 
 			if (pb->object_set.empty() && pb->static_object_set.empty()) {
 
-				if (hash_table[idx]==pb) {
-					hash_table[idx]=pb->next;
+				if (hash_table[idx] == pb) {
+					hash_table[idx] = pb->next;
 				} else {
 
 					PosBin *px = hash_table[idx];
 
 					while (px) {
 
-						if (px->next==pb) {
-							px->next=pb->next;
+						if (px->next == pb) {
+							px->next = pb->next;
 							break;
 						}
 
-						px=px->next;
+						px = px->next;
 					}
 
 					ERR_CONTINUE(!px);
 				}
 
 				memdelete(pb);
-
 			}
 		}
-
 	}
 
-
-	for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
-		if (E->key()==p_elem)
+	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
+		if (E->key() == p_elem)
 			continue; // do not pair against itself
 		if (E->key()->owner == p_elem->owner)
 			continue;
@@ -334,121 +309,109 @@ void BroadPhase2DHashGrid::_exit_grid( Element* p_elem, const Rect2& p_rect,bool
 			continue;
 
 		//unpair from large elements
-		_unpair_attempt(p_elem,E->key());
+		_unpair_attempt(p_elem, E->key());
 	}
-
-
 }
 
-
 BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
 
 	current++;
 
 	Element e;
-	e.owner=p_object;
-	e._static=false;
-	e.subindex=p_subindex;
-	e.self=current;
-	e.pass=0;
+	e.owner = p_object;
+	e._static = false;
+	e.subindex = p_subindex;
+	e.self = current;
+	e.pass = 0;
 
-	element_map[current]=e;
+	element_map[current] = e;
 	return current;
-
 }
 
-void BroadPhase2DHashGrid::move(ID p_id, const Rect2& p_aabb) {
-
+void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 
-	Element &e=E->get();
+	Element &e = E->get();
 
-	if (p_aabb==e.aabb)
+	if (p_aabb == e.aabb)
 		return;
 
+	if (p_aabb != Rect2()) {
 
-	if (p_aabb!=Rect2()) {
-
-		_enter_grid(&e,p_aabb,e._static);
+		_enter_grid(&e, p_aabb, e._static);
 	}
 
-	if (e.aabb!=Rect2()) {
+	if (e.aabb != Rect2()) {
 
-		_exit_grid(&e,e.aabb,e._static);
+		_exit_grid(&e, e.aabb, e._static);
 	}
 
-	e.aabb=p_aabb;
+	e.aabb = p_aabb;
 
 	_check_motion(&e);
 
-	e.aabb=p_aabb;
-
+	e.aabb = p_aabb;
 }
 void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 
-	Element &e=E->get();
+	Element &e = E->get();
 
-	if (e._static==p_static)
+	if (e._static == p_static)
 		return;
 
-	if (e.aabb!=Rect2())
-		_exit_grid(&e,e.aabb,e._static);
+	if (e.aabb != Rect2())
+		_exit_grid(&e, e.aabb, e._static);
 
-	e._static=p_static;
+	e._static = p_static;
 
-	if (e.aabb!=Rect2()) {
-		_enter_grid(&e,e.aabb,e._static);
+	if (e.aabb != Rect2()) {
+		_enter_grid(&e, e.aabb, e._static);
 		_check_motion(&e);
 	}
-
 }
 void BroadPhase2DHashGrid::remove(ID p_id) {
 
-	Map<ID,Element>::Element *E=element_map.find(p_id);
+	Map<ID, Element>::Element *E = element_map.find(p_id);
 	ERR_FAIL_COND(!E);
 
-	Element &e=E->get();
+	Element &e = E->get();
 
-	if (e.aabb!=Rect2())
-		_exit_grid(&e,e.aabb,e._static);
+	if (e.aabb != Rect2())
+		_exit_grid(&e, e.aabb, e._static);
 
 	element_map.erase(p_id);
-
 }
 
 CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,NULL);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, NULL);
 	return E->get().owner;
-
 }
 bool BroadPhase2DHashGrid::is_static(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,false);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, false);
 	return E->get()._static;
-
 }
 int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
 
-	const Map<ID,Element>::Element *E=element_map.find(p_id);
-	ERR_FAIL_COND_V(!E,-1);
+	const Map<ID, Element>::Element *E = element_map.find(p_id);
+	ERR_FAIL_COND_V(!E, -1);
 	return E->get().subindex;
 }
 
-template<bool use_aabb,bool use_segment>
-void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index) {
-
+template <bool use_aabb, bool use_segment>
+void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) {
 
 	PosKey pk;
-	pk.x=p_cell.x;
-	pk.y=p_cell.y;
+	pk.x = p_cell.x;
+	pk.y = p_cell.y;
 
 	uint32_t idx = pk.hash() % hash_table_size;
 	PosBin *pb = hash_table[idx];
@@ -459,261 +422,237 @@ void BroadPhase2DHashGrid::_cull(const Point2i p_cell,const Rect2& p_aabb,const
 			break;
 		}
 
-		pb=pb->next;
+		pb = pb->next;
 	}
 
 	if (!pb)
 		return;
 
+	for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
 
-
-	for(Map<Element*,RC>::Element *E=pb->object_set.front();E;E=E->next()) {
-
-
-		if (index>=p_max_results)
+		if (index >= p_max_results)
 			break;
-		if (E->key()->pass==pass)
+		if (E->key()->pass == pass)
 			continue;
 
-		E->key()->pass=pass;
+		E->key()->pass = pass;
 
 		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
 			continue;
 
-		if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
+		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to))
 			continue;
 
-		p_results[index]=E->key()->owner;
-		p_result_indices[index]=E->key()->subindex;
+		p_results[index] = E->key()->owner;
+		p_result_indices[index] = E->key()->subindex;
 		index++;
-
-
 	}
 
-	for(Map<Element*,RC>::Element *E=pb->static_object_set.front();E;E=E->next()) {
-
+	for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
 
-		if (index>=p_max_results)
+		if (index >= p_max_results)
 			break;
-		if (E->key()->pass==pass)
+		if (E->key()->pass == pass)
 			continue;
 
 		if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
 			continue;
 		}
 
-		if (use_segment && !E->key()->aabb.intersects_segment(p_from,p_to))
+		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to))
 			continue;
 
-		E->key()->pass=pass;
-		p_results[index]=E->key()->owner;
-		p_result_indices[index]=E->key()->subindex;
+		E->key()->pass = pass;
+		p_results[index] = E->key()->owner;
+		p_result_indices[index] = E->key()->subindex;
 		index++;
-
 	}
 }
 
-int BroadPhase2DHashGrid::cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
 
 	pass++;
 
-	Vector2 dir = (p_to-p_from);
-	if (dir==Vector2())
+	Vector2 dir = (p_to - p_from);
+	if (dir == Vector2())
 		return 0;
 	//avoid divisions by zero
 	dir.normalize();
-	if (dir.x==0.0)
-		dir.x=0.000001;
-	if (dir.y==0.0)
-		dir.y=0.000001;
+	if (dir.x == 0.0)
+		dir.x = 0.000001;
+	if (dir.y == 0.0)
+		dir.y = 0.000001;
 	Vector2 delta = dir.abs();
 
-	delta.x=cell_size/delta.x;
-	delta.y=cell_size/delta.y;
+	delta.x = cell_size / delta.x;
+	delta.y = cell_size / delta.y;
 
-	Point2i pos = (p_from/cell_size).floor();
-	Point2i end = (p_to/cell_size).floor();
+	Point2i pos = (p_from / cell_size).floor();
+	Point2i end = (p_to / cell_size).floor();
 
-	Point2i step = Vector2( SGN(dir.x), SGN(dir.y) );
+	Point2i step = Vector2(SGN(dir.x), SGN(dir.y));
 
 	Vector2 max;
 
-	if (dir.x<0)
-		max.x= (Math::floor(pos.x)*cell_size - p_from.x) / dir.x;
+	if (dir.x < 0)
+		max.x = (Math::floor(pos.x) * cell_size - p_from.x) / dir.x;
 	else
-		max.x= (Math::floor(pos.x + 1)*cell_size - p_from.x) / dir.x;
+		max.x = (Math::floor(pos.x + 1) * cell_size - p_from.x) / dir.x;
 
-	if (dir.y<0)
-		max.y= (Math::floor(pos.y)*cell_size - p_from.y) / dir.y;
+	if (dir.y < 0)
+		max.y = (Math::floor(pos.y) * cell_size - p_from.y) / dir.y;
 	else
-		max.y= (Math::floor(pos.y + 1)*cell_size - p_from.y) / dir.y;
+		max.y = (Math::floor(pos.y + 1) * cell_size - p_from.y) / dir.y;
 
-	int cullcount=0;
-	_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
+	int cullcount = 0;
+	_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
 
-	bool reached_x=false;
-	bool reached_y=false;
+	bool reached_x = false;
+	bool reached_y = false;
 
-	while(true) {
+	while (true) {
 
 		if (max.x < max.y) {
 
-			max.x+=delta.x;
-			pos.x+=step.x;
+			max.x += delta.x;
+			pos.x += step.x;
 		} else {
 
-			max.y+=delta.y;
-			pos.y+=step.y;
-
+			max.y += delta.y;
+			pos.y += step.y;
 		}
 
-		if (step.x>0) {
-			if (pos.x>=end.x)
-				reached_x=true;
-		} else if (pos.x<=end.x) {
+		if (step.x > 0) {
+			if (pos.x >= end.x)
+				reached_x = true;
+		} else if (pos.x <= end.x) {
 
-			reached_x=true;
+			reached_x = true;
 		}
 
-		if (step.y>0) {
-			if (pos.y>=end.y)
-				reached_y=true;
-		} else if (pos.y<=end.y) {
+		if (step.y > 0) {
+			if (pos.y >= end.y)
+				reached_y = true;
+		} else if (pos.y <= end.y) {
 
-			reached_y=true;
+			reached_y = true;
 		}
 
-		_cull<false,true>(pos,Rect2(),p_from,p_to,p_results,p_max_results,p_result_indices,cullcount);
+		_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
 
 		if (reached_x && reached_y)
 			break;
-
 	}
 
-	for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
 
-		if (cullcount>=p_max_results)
+		if (cullcount >= p_max_results)
 			break;
-		if (E->key()->pass==pass)
+		if (E->key()->pass == pass)
 			continue;
 
-		E->key()->pass=pass;
+		E->key()->pass = pass;
 
-//		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
-//			continue;
+		//		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
+		//			continue;
 
-		if (!E->key()->aabb.intersects_segment(p_from,p_to))
+		if (!E->key()->aabb.intersects_segment(p_from, p_to))
 			continue;
 
-		p_results[cullcount]=E->key()->owner;
-		p_result_indices[cullcount]=E->key()->subindex;
+		p_results[cullcount] = E->key()->owner;
+		p_result_indices[cullcount] = E->key()->subindex;
 		cullcount++;
-
-
 	}
 
 	return cullcount;
 }
 
-
-int BroadPhase2DHashGrid::cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
 
 	pass++;
 
-	Point2i from = (p_aabb.pos/cell_size).floor();
-	Point2i to = ((p_aabb.pos+p_aabb.size)/cell_size).floor();
-	int cullcount=0;
+	Point2i from = (p_aabb.pos / cell_size).floor();
+	Point2i to = ((p_aabb.pos + p_aabb.size) / cell_size).floor();
+	int cullcount = 0;
 
-	for(int i=from.x;i<=to.x;i++) {
+	for (int i = from.x; i <= to.x; i++) {
 
-		for(int j=from.y;j<=to.y;j++) {
+		for (int j = from.y; j <= to.y; j++) {
 
-			_cull<true,false>(Point2i(i,j),p_aabb,Point2(),Point2(),p_results,p_max_results,p_result_indices,cullcount);
+			_cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount);
 		}
-
 	}
 
-	for (Map<Element*,RC>::Element *E=large_elements.front();E;E=E->next()) {
+	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
 
-		if (cullcount>=p_max_results)
+		if (cullcount >= p_max_results)
 			break;
-		if (E->key()->pass==pass)
+		if (E->key()->pass == pass)
 			continue;
 
-		E->key()->pass=pass;
+		E->key()->pass = pass;
 
 		if (!p_aabb.intersects(E->key()->aabb))
 			continue;
 
-//		if (!E->key()->aabb.intersects_segment(p_from,p_to))
-//			continue;
+		//		if (!E->key()->aabb.intersects_segment(p_from,p_to))
+		//			continue;
 
-		p_results[cullcount]=E->key()->owner;
-		p_result_indices[cullcount]=E->key()->subindex;
+		p_results[cullcount] = E->key()->owner;
+		p_result_indices[cullcount] = E->key()->subindex;
 		cullcount++;
-
-
 	}
 	return cullcount;
 }
 
-void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) {
-
-	pair_callback=p_pair_callback;
-	pair_userdata=p_userdata;
+void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 
+	pair_callback = p_pair_callback;
+	pair_userdata = p_userdata;
 }
-void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata) {
-
-	unpair_callback=p_unpair_callback;
-	unpair_userdata=p_userdata;
+void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
 
+	unpair_callback = p_unpair_callback;
+	unpair_userdata = p_userdata;
 }
 
 void BroadPhase2DHashGrid::update() {
-
-
 }
 
 BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
 
-	return memnew( BroadPhase2DHashGrid );
+	return memnew(BroadPhase2DHashGrid);
 }
 
-
 BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
 
-	hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size",4096);
+	hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size", 4096);
 	hash_table_size = Math::larger_prime(hash_table_size);
-	hash_table = memnew_arr( PosBin*, hash_table_size);
+	hash_table = memnew_arr(PosBin *, hash_table_size);
 
-	cell_size = GLOBAL_DEF("physics_2d/cell_size",128);
-	large_object_min_surface = GLOBAL_DEF("physics_2d/large_object_surface_treshold_in_cells",512);
+	cell_size = GLOBAL_DEF("physics_2d/cell_size", 128);
+	large_object_min_surface = GLOBAL_DEF("physics_2d/large_object_surface_treshold_in_cells", 512);
 
-	for(int i=0;i<hash_table_size;i++)
-		hash_table[i]=NULL;
-	pass=1;
+	for (int i = 0; i < hash_table_size; i++)
+		hash_table[i] = NULL;
+	pass = 1;
 
-	current=0;
+	current = 0;
 }
 
 BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
 
-	for(int i=0;i<hash_table_size;i++) {
-		while(hash_table[i]) {
-			PosBin *pb=hash_table[i];
-			hash_table[i]=pb->next;
+	for (int i = 0; i < hash_table_size; i++) {
+		while (hash_table[i]) {
+			PosBin *pb = hash_table[i];
+			hash_table[i] = pb->next;
 			memdelete(pb);
 		}
 	}
 
-	memdelete_arr( hash_table );
-
-
+	memdelete_arr(hash_table);
 }
 
-
-
 /* 3D version of voxel traversal:
 
 public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h
index 857053ccf..f30f72d6d 100644
--- a/servers/physics_2d/broad_phase_2d_hash_grid.h
+++ b/servers/physics_2d/broad_phase_2d_hash_grid.h
@@ -34,13 +34,16 @@
 
 class BroadPhase2DHashGrid : public BroadPhase2DSW {
 
-
 	struct PairData {
 
 		bool colliding;
 		int rc;
 		void *ud;
-		PairData() { colliding=false; rc=1; ud=NULL; }
+		PairData() {
+			colliding = false;
+			rc = 1;
+			ud = NULL;
+		}
 	};
 
 	struct Element {
@@ -51,8 +54,7 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 		Rect2 aabb;
 		int subindex;
 		uint64_t pass;
-		Map<Element*,PairData*> paired;
-
+		Map<Element *, PairData *> paired;
 	};
 
 	struct RC {
@@ -69,18 +71,17 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 		}
 
 		_FORCE_INLINE_ RC() {
-			ref=0;
+			ref = 0;
 		}
 	};
 
-	Map<ID,Element> element_map;
-	Map<Element*,RC> large_elements;
+	Map<ID, Element> element_map;
+	Map<Element *, RC> large_elements;
 
 	ID current;
 
 	uint64_t pass;
 
-
 	struct PairKey {
 
 		union {
@@ -91,17 +92,23 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 			uint64_t key;
 		};
 
-		_FORCE_INLINE_ bool operator<(const PairKey& p_key) const {
+		_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
 			return key < p_key.key;
 		}
 
-		PairKey() { key=0; }
-		PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }}
-
+		PairKey() { key = 0; }
+		PairKey(ID p_a, ID p_b) {
+			if (p_a > p_b) {
+				a = p_b;
+				b = p_a;
+			} else {
+				a = p_a;
+				b = p_b;
+			}
+		}
 	};
 
-
-	Map<PairKey,PairData> pair_map;
+	Map<PairKey, PairData> pair_map;
 
 	int cell_size;
 	int large_object_min_surface;
@@ -111,11 +118,10 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 	UnpairCallback unpair_callback;
 	void *unpair_userdata;
 
-	void _enter_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
-	void _exit_grid(Element* p_elem, const Rect2& p_rect,bool p_static);
-	template<bool use_aabb,bool use_segment>
-	_FORCE_INLINE_ void _cull(const Point2i p_cell,const Rect2& p_aabb,const Point2& p_from, const Point2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices,int &index);
-
+	void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static);
+	void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static);
+	template <bool use_aabb, bool use_segment>
+	_FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index);
 
 	struct PosKey {
 
@@ -127,9 +133,8 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 			uint64_t key;
 		};
 
-
 		_FORCE_INLINE_ uint32_t hash() const {
-			uint64_t k=key;
+			uint64_t k = key;
 			k = (~k) + (k << 18); // k = (k << 18) - k - 1;
 			k = k ^ (k >> 31);
 			k = k * 21; // k = (k + (k << 2)) + (k << 4);
@@ -139,36 +144,30 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
 			return k;
 		}
 
-		bool operator==(const PosKey& p_key) const { return key==p_key.key; }
-		_FORCE_INLINE_ bool operator<(const PosKey& p_key) const {
+		bool operator==(const PosKey &p_key) const { return key == p_key.key; }
+		_FORCE_INLINE_ bool operator<(const PosKey &p_key) const {
 			return key < p_key.key;
 		}
-
 	};
 
-
-
 	struct PosBin {
 
 		PosKey key;
-		Map<Element*,RC> object_set;
-		Map<Element*,RC> static_object_set;
+		Map<Element *, RC> object_set;
+		Map<Element *, RC> static_object_set;
 		PosBin *next;
 	};
 
-
 	uint32_t hash_table_size;
 	PosBin **hash_table;
 
-	void _pair_attempt(Element *p_elem, Element* p_with);
-	void _unpair_attempt(Element *p_elem, Element* p_with);
+	void _pair_attempt(Element *p_elem, Element *p_with);
+	void _unpair_attempt(Element *p_elem, Element *p_with);
 	void _check_motion(Element *p_elem);
 
-
 public:
-
-	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0);
-	virtual void move(ID p_id, const Rect2& p_aabb);
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const Rect2 &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 
@@ -176,21 +175,18 @@ public:
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
-	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
-	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL);
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
+	virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL);
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata);
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata);
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
 
 	virtual void update();
 
-
 	static BroadPhase2DSW *_create();
 
 	BroadPhase2DHashGrid();
 	~BroadPhase2DHashGrid();
-
-
 };
 
 #endif // BROAD_PHASE_2D_HASH_GRID_H
diff --git a/servers/physics_2d/broad_phase_2d_sw.cpp b/servers/physics_2d/broad_phase_2d_sw.cpp
index 4347155c2..8cde46217 100644
--- a/servers/physics_2d/broad_phase_2d_sw.cpp
+++ b/servers/physics_2d/broad_phase_2d_sw.cpp
@@ -28,8 +28,7 @@
 /*************************************************************************/
 #include "broad_phase_2d_sw.h"
 
-BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func=NULL;
+BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = NULL;
 
-BroadPhase2DSW::~BroadPhase2DSW()
-{
+BroadPhase2DSW::~BroadPhase2DSW() {
 }
diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h
index b9ec434ae..a255cc6ba 100644
--- a/servers/physics_2d/broad_phase_2d_sw.h
+++ b/servers/physics_2d/broad_phase_2d_sw.h
@@ -29,45 +29,42 @@
 #ifndef BROAD_PHASE_2D_SW_H
 #define BROAD_PHASE_2D_SW_H
 
-#include "math_funcs.h"
 #include "math_2d.h"
+#include "math_funcs.h"
 
 class CollisionObject2DSW;
 
-
 class BroadPhase2DSW {
 
 public:
-	typedef BroadPhase2DSW* (*CreateFunction)();
+	typedef BroadPhase2DSW *(*CreateFunction)();
 
 	static CreateFunction create_func;
 
 	typedef uint32_t ID;
 
-
-	typedef void* (*PairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_userdata);
-	typedef void (*UnpairCallback)(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_userdata);
+	typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata);
+	typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex=0)=0;
-	virtual void move(ID p_id, const Rect2& p_aabb)=0;
-	virtual void set_static(ID p_id, bool p_static)=0;
-	virtual void remove(ID p_id)=0;
+	virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0;
+	virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
+	virtual void set_static(ID p_id, bool p_static) = 0;
+	virtual void remove(ID p_id) = 0;
 
-	virtual CollisionObject2DSW *get_object(ID p_id) const=0;
-	virtual bool is_static(ID p_id) const=0;
-	virtual int get_subindex(ID p_id) const=0;
+	virtual CollisionObject2DSW *get_object(ID p_id) const = 0;
+	virtual bool is_static(ID p_id) const = 0;
+	virtual int get_subindex(ID p_id) const = 0;
 
-	virtual int cull_segment(const Vector2& p_from, const Vector2& p_to,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
-	virtual int cull_aabb(const Rect2& p_aabb,CollisionObject2DSW** p_results,int p_max_results,int *p_result_indices=NULL)=0;
+	virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
+	virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = NULL) = 0;
 
-	virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0;
-	virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0;
+	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0;
+	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
 
-	virtual void update()=0;
+	virtual void update() = 0;
 
 	virtual ~BroadPhase2DSW();
-
 };
 
 #endif // BROAD_PHASE_2D_SW_H
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 94e5d668b..8adb7ada1 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -29,46 +29,43 @@
 #include "collision_object_2d_sw.h"
 #include "space_2d_sw.h"
 
-void CollisionObject2DSW::add_shape(Shape2DSW *p_shape,const Matrix32& p_transform) {
+void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Matrix32 &p_transform) {
 
 	Shape s;
-	s.shape=p_shape;
-	s.xform=p_transform;
-	s.xform_inv=s.xform.affine_inverse();
-	s.bpid=0; //needs update
-	s.trigger=false;
+	s.shape = p_shape;
+	s.xform = p_transform;
+	s.xform_inv = s.xform.affine_inverse();
+	s.bpid = 0; //needs update
+	s.trigger = false;
 	shapes.push_back(s);
 	p_shape->add_owner(this);
 	_update_shapes();
 	_shapes_changed();
-
 }
 
-void CollisionObject2DSW::set_shape(int p_index,Shape2DSW *p_shape){
+void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
 
-	ERR_FAIL_INDEX(p_index,shapes.size());
+	ERR_FAIL_INDEX(p_index, shapes.size());
 	shapes[p_index].shape->remove_owner(this);
-	shapes[p_index].shape=p_shape;
+	shapes[p_index].shape = p_shape;
 
 	p_shape->add_owner(this);
 	_update_shapes();
 	_shapes_changed();
-
 }
 
-void CollisionObject2DSW::set_shape_metadata(int p_index,const Variant& p_metadata) {
-
-	ERR_FAIL_INDEX(p_index,shapes.size());
-	shapes[p_index].metadata=p_metadata;
+void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) {
 
+	ERR_FAIL_INDEX(p_index, shapes.size());
+	shapes[p_index].metadata = p_metadata;
 }
 
-void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_transform){
+void CollisionObject2DSW::set_shape_transform(int p_index, const Matrix32 &p_transform) {
 
-	ERR_FAIL_INDEX(p_index,shapes.size());
+	ERR_FAIL_INDEX(p_index, shapes.size());
 
-	shapes[p_index].xform=p_transform;
-	shapes[p_index].xform_inv=p_transform.affine_inverse();
+	shapes[p_index].xform = p_transform;
+	shapes[p_index].xform_inv = p_transform.affine_inverse();
 	_update_shapes();
 	_shapes_changed();
 }
@@ -76,61 +73,58 @@ void CollisionObject2DSW::set_shape_transform(int p_index,const Matrix32& p_tran
 void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) {
 
 	//remove a shape, all the times it appears
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		if (shapes[i].shape==p_shape) {
+		if (shapes[i].shape == p_shape) {
 			remove_shape(i);
 			i--;
 		}
 	}
 }
 
-void CollisionObject2DSW::remove_shape(int p_index){
+void CollisionObject2DSW::remove_shape(int p_index) {
 
 	//remove anything from shape to be erased to end, so subindices don't change
-	ERR_FAIL_INDEX(p_index,shapes.size());
-	for(int i=p_index;i<shapes.size();i++) {
+	ERR_FAIL_INDEX(p_index, shapes.size());
+	for (int i = p_index; i < shapes.size(); i++) {
 
-		if (shapes[i].bpid==0)
+		if (shapes[i].bpid == 0)
 			continue;
 		//should never get here with a null owner
 		space->get_broadphase()->remove(shapes[i].bpid);
-		shapes[i].bpid=0;
+		shapes[i].bpid = 0;
 	}
 	shapes[p_index].shape->remove_owner(this);
 	shapes.remove(p_index);
 
 	_shapes_changed();
-
 }
 
 void CollisionObject2DSW::_set_static(bool p_static) {
-	if (_static==p_static)
+	if (_static == p_static)
 		return;
-	_static=p_static;
+	_static = p_static;
 
 	if (!space)
 		return;
-	for(int i=0;i<get_shape_count();i++) {
-		Shape &s=shapes[i];
-		if (s.bpid>0) {
-			space->get_broadphase()->set_static(s.bpid,_static);
+	for (int i = 0; i < get_shape_count(); i++) {
+		Shape &s = shapes[i];
+		if (s.bpid > 0) {
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 	}
-
 }
 
 void CollisionObject2DSW::_unregister_shapes() {
 
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		Shape &s=shapes[i];
-		if (s.bpid>0) {
+		Shape &s = shapes[i];
+		if (s.bpid > 0) {
 			space->get_broadphase()->remove(s.bpid);
-			s.bpid=0;
+			s.bpid = 0;
 		}
 	}
-
 }
 
 void CollisionObject2DSW::_update_shapes() {
@@ -138,53 +132,47 @@ void CollisionObject2DSW::_update_shapes() {
 	if (!space)
 		return;
 
+	for (int i = 0; i < shapes.size(); i++) {
 
-	for(int i=0;i<shapes.size();i++) {
-
-		Shape &s=shapes[i];
-		if (s.bpid==0) {
-			s.bpid=space->get_broadphase()->create(this,i);
-			space->get_broadphase()->set_static(s.bpid,_static);
+		Shape &s = shapes[i];
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i);
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 
 		//not quite correct, should compute the next matrix..
-		Rect2 shape_aabb=s.shape->get_aabb();
+		Rect2 shape_aabb = s.shape->get_aabb();
 		Matrix32 xform = transform * s.xform;
-		shape_aabb=xform.xform(shape_aabb);
-		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 );
+		shape_aabb = xform.xform(shape_aabb);
+		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);
 
-
-		space->get_broadphase()->move(s.bpid,s.aabb_cache);
+		space->get_broadphase()->move(s.bpid, s.aabb_cache);
 	}
-
 }
 
-void CollisionObject2DSW::_update_shapes_with_motion(const Vector2& p_motion) {
-
+void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 
 	if (!space)
 		return;
 
-	for(int i=0;i<shapes.size();i++) {
+	for (int i = 0; i < shapes.size(); i++) {
 
-		Shape &s=shapes[i];
-		if (s.bpid==0) {
-			s.bpid=space->get_broadphase()->create(this,i);
-			space->get_broadphase()->set_static(s.bpid,_static);
+		Shape &s = shapes[i];
+		if (s.bpid == 0) {
+			s.bpid = space->get_broadphase()->create(this, i);
+			space->get_broadphase()->set_static(s.bpid, _static);
 		}
 
 		//not quite correct, should compute the next matrix..
-		Rect2 shape_aabb=s.shape->get_aabb();
+		Rect2 shape_aabb = s.shape->get_aabb();
 		Matrix32 xform = transform * s.xform;
-		shape_aabb=xform.xform(shape_aabb);
-		shape_aabb=shape_aabb.merge(Rect2( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion
-		s.aabb_cache=shape_aabb;
+		shape_aabb = xform.xform(shape_aabb);
+		shape_aabb = shape_aabb.merge(Rect2(shape_aabb.pos + p_motion, shape_aabb.size)); //use motion
+		s.aabb_cache = shape_aabb;
 
-		space->get_broadphase()->move(s.bpid,shape_aabb);
+		space->get_broadphase()->move(s.bpid, shape_aabb);
 	}
-
-
 }
 
 void CollisionObject2DSW::_set_space(Space2DSW *p_space) {
@@ -193,25 +181,23 @@ void CollisionObject2DSW::_set_space(Space2DSW *p_space) {
 
 		space->remove_object(this);
 
-		for(int i=0;i<shapes.size();i++) {
+		for (int i = 0; i < shapes.size(); i++) {
 
-			Shape &s=shapes[i];
+			Shape &s = shapes[i];
 			if (s.bpid) {
 				space->get_broadphase()->remove(s.bpid);
-				s.bpid=0;
+				s.bpid = 0;
 			}
 		}
-
 	}
 
-	space=p_space;
+	space = p_space;
 
 	if (space) {
 
 		space->add_object(this);
 		_update_shapes();
 	}
-
 }
 
 void CollisionObject2DSW::_shape_changed() {
@@ -222,11 +208,11 @@ void CollisionObject2DSW::_shape_changed() {
 
 CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
 
-	_static=true;
-	type=p_type;
-	space=NULL;
-	instance_id=0;
-	collision_mask=1;
-	layer_mask=1;
-	pickable=true;
+	_static = true;
+	type = p_type;
+	space = NULL;
+	instance_id = 0;
+	collision_mask = 1;
+	layer_mask = 1;
+	pickable = true;
 }
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index cf26a28ec..2c3acbee7 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -29,10 +29,10 @@
 #ifndef COLLISION_OBJECT_2D_SW_H
 #define COLLISION_OBJECT_2D_SW_H
 
-#include "shape_2d_sw.h"
-#include "servers/physics_2d_server.h"
-#include "self_list.h"
 #include "broad_phase_2d_sw.h"
+#include "self_list.h"
+#include "servers/physics_2d_server.h"
+#include "shape_2d_sw.h"
 
 class Space2DSW;
 
@@ -42,8 +42,8 @@ public:
 		TYPE_AREA,
 		TYPE_BODY
 	};
-private:
 
+private:
 	Type type;
 	RID self;
 	ObjectID instance_id;
@@ -58,7 +58,7 @@ private:
 		Shape2DSW *shape;
 		Variant metadata;
 		bool trigger;
-		Shape() { trigger=false; }
+		Shape() { trigger = false; }
 	};
 
 	Vector<Shape> shapes;
@@ -72,74 +72,74 @@ private:
 	void _update_shapes();
 
 protected:
-
-
-	void _update_shapes_with_motion(const Vector2& p_motion);
+	void _update_shapes_with_motion(const Vector2 &p_motion);
 	void _unregister_shapes();
 
-	_FORCE_INLINE_ void _set_transform(const Matrix32& p_transform, bool p_update_shapes=true) { transform=p_transform; if (p_update_shapes) {_update_shapes();} }
-	_FORCE_INLINE_ void _set_inv_transform(const Matrix32& p_transform) { inv_transform=p_transform; }
+	_FORCE_INLINE_ void _set_transform(const Matrix32 &p_transform, bool p_update_shapes = true) {
+		transform = p_transform;
+		if (p_update_shapes) {
+			_update_shapes();
+		}
+	}
+	_FORCE_INLINE_ void _set_inv_transform(const Matrix32 &p_transform) { inv_transform = p_transform; }
 	void _set_static(bool p_static);
 
-	virtual void _shapes_changed()=0;
+	virtual void _shapes_changed() = 0;
 	void _set_space(Space2DSW *space);
 
 	CollisionObject2DSW(Type p_type);
-public:
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	_FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; }
+	_FORCE_INLINE_ void set_instance_id(const ObjectID &p_instance_id) { instance_id = p_instance_id; }
 	_FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; }
 
 	void _shape_changed();
 
 	_FORCE_INLINE_ Type get_type() const { return type; }
-	void add_shape(Shape2DSW *p_shape,const Matrix32& p_transform=Matrix32());
-	void set_shape(int p_index,Shape2DSW *p_shape);
-	void set_shape_transform(int p_index,const Matrix32& p_transform);
-	void set_shape_metadata(int p_index,const Variant& p_metadata);
-
+	void add_shape(Shape2DSW *p_shape, const Matrix32 &p_transform = Matrix32());
+	void set_shape(int p_index, Shape2DSW *p_shape);
+	void set_shape_transform(int p_index, const Matrix32 &p_transform);
+	void set_shape_metadata(int p_index, const Variant &p_metadata);
 
 	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
 	_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { return shapes[p_index].shape; }
-	_FORCE_INLINE_ const Matrix32& get_shape_transform(int p_index) const { return shapes[p_index].xform; }
-	_FORCE_INLINE_ const Matrix32& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
-	_FORCE_INLINE_ const Rect2& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
-	_FORCE_INLINE_ const Variant& get_shape_metadata(int p_index) const { return shapes[p_index].metadata; }
+	_FORCE_INLINE_ const Matrix32 &get_shape_transform(int p_index) const { return shapes[p_index].xform; }
+	_FORCE_INLINE_ const Matrix32 &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
+	_FORCE_INLINE_ const Rect2 &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
+	_FORCE_INLINE_ const Variant &get_shape_metadata(int p_index) const { return shapes[p_index].metadata; }
 
 	_FORCE_INLINE_ Matrix32 get_transform() const { return transform; }
 	_FORCE_INLINE_ Matrix32 get_inv_transform() const { return inv_transform; }
-	_FORCE_INLINE_ Space2DSW* get_space() const { return space; }
+	_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
 
-	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx,bool p_enable) { shapes[p_idx].trigger=p_enable; }
+	_FORCE_INLINE_ void set_shape_as_trigger(int p_idx, bool p_enable) { shapes[p_idx].trigger = p_enable; }
 	_FORCE_INLINE_ bool is_shape_set_as_trigger(int p_idx) const { return shapes[p_idx].trigger; }
 
-
-	void set_collision_mask(uint32_t p_mask) {collision_mask=p_mask;}
+	void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	void set_layer_mask(uint32_t p_mask) {layer_mask=p_mask;}
+	void set_layer_mask(uint32_t p_mask) { layer_mask = p_mask; }
 	_FORCE_INLINE_ uint32_t get_layer_mask() const { return layer_mask; }
 
 	void remove_shape(Shape2DSW *p_shape);
 	void remove_shape(int p_index);
 
-	virtual void set_space(Space2DSW *p_space)=0;
+	virtual void set_space(Space2DSW *p_space) = 0;
 
-	_FORCE_INLINE_ bool is_static() const { return _static;  }
+	_FORCE_INLINE_ bool is_static() const { return _static; }
 
-	void set_pickable(bool p_pickable) { pickable=p_pickable; }
+	void set_pickable(bool p_pickable) { pickable = p_pickable; }
 	_FORCE_INLINE_ bool is_pickable() const { return pickable; }
 
-	_FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW* p_other) const {
+	_FORCE_INLINE_ bool test_collision_mask(CollisionObject2DSW *p_other) const {
 
-		return layer_mask&p_other->collision_mask || p_other->layer_mask&collision_mask;
+		return layer_mask & p_other->collision_mask || p_other->layer_mask & collision_mask;
 	}
 
 	virtual ~CollisionObject2DSW() {}
-
 };
 
 #endif // COLLISION_OBJECT_2D_SW_H
diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index d4ab2c3bc..3260b58e8 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -38,59 +38,55 @@ struct _CollectorCallback2D {
 	Vector2 normal;
 	Vector2 *sep_axis;
 
-	_FORCE_INLINE_ void call(const Vector2& p_point_A, const Vector2& p_point_B) {
+	_FORCE_INLINE_ void call(const Vector2 &p_point_A, const Vector2 &p_point_B) {
 
 		//if (normal.dot(p_point_A) >= normal.dot(p_point_B))
 		// return;
 		if (swap)
-			callback(p_point_B,p_point_A,userdata);
+			callback(p_point_B, p_point_A, userdata);
 		else
-			callback(p_point_A,p_point_B,userdata);
+			callback(p_point_A, p_point_B, userdata);
 	}
-
 };
 
-typedef void (*GenerateContactsFunc)(const Vector2 *,int, const Vector2 *,int ,_CollectorCallback2D *);
-
+typedef void (*GenerateContactsFunc)(const Vector2 *, int, const Vector2 *, int, _CollectorCallback2D *);
 
-_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+_FORCE_INLINE_ static void _generate_contacts_point_point(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 1 );
-	ERR_FAIL_COND( p_point_count_B != 1 );
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 1);
 #endif
 
-	p_collector->call(*p_points_A,*p_points_B);
+	p_collector->call(*p_points_A, *p_points_B);
 }
 
-_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+_FORCE_INLINE_ static void _generate_contacts_point_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 1 );
-	ERR_FAIL_COND( p_point_count_B != 2 );
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 2);
 #endif
 
-	Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B );
-	p_collector->call(*p_points_A,closest_B);
-
+	Vector2 closest_B = Geometry::get_closest_point_to_segment_uncapped_2d(*p_points_A, p_points_B);
+	p_collector->call(*p_points_A, closest_B);
 }
 
-
 struct _generate_contacts_Pair {
 	bool a;
 	int idx;
 	float d;
-	_FORCE_INLINE_ bool operator <(const _generate_contacts_Pair& l) const { return d< l.d; }
+	_FORCE_INLINE_ bool operator<(const _generate_contacts_Pair &l) const { return d < l.d; }
 };
 
-_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
+_FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A != 2 );
-	ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix
+	ERR_FAIL_COND(p_point_count_A != 2);
+	ERR_FAIL_COND(p_point_count_B != 2); // circle is actually a 4x3 matrix
 #endif
 
-# if 0
+#if 0
 	Vector2 rel_A=p_points_A[1]-p_points_A[0];
 	Vector2 rel_B=p_points_B[1]-p_points_B[0];
 
@@ -208,9 +204,6 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points
 
 #if 1
 
-
-
-
 	Vector2 n = p_collector->normal;
 	Vector2 t = n.tangent();
 	real_t dA = n.dot(p_points_A[0]);
@@ -218,90 +211,87 @@ _FORCE_INLINE_ static void _generate_contacts_edge_edge(const Vector2 * p_points
 
 	_generate_contacts_Pair dvec[4];
 
-	dvec[0].d=t.dot(p_points_A[0]);
-	dvec[0].a=true;
-	dvec[0].idx=0;
-	dvec[1].d=t.dot(p_points_A[1]);
-	dvec[1].a=true;
-	dvec[1].idx=1;
-	dvec[2].d=t.dot(p_points_B[0]);
-	dvec[2].a=false;
-	dvec[2].idx=0;
-	dvec[3].d=t.dot(p_points_B[1]);
-	dvec[3].a=false;
-	dvec[3].idx=1;
+	dvec[0].d = t.dot(p_points_A[0]);
+	dvec[0].a = true;
+	dvec[0].idx = 0;
+	dvec[1].d = t.dot(p_points_A[1]);
+	dvec[1].a = true;
+	dvec[1].idx = 1;
+	dvec[2].d = t.dot(p_points_B[0]);
+	dvec[2].a = false;
+	dvec[2].idx = 0;
+	dvec[3].d = t.dot(p_points_B[1]);
+	dvec[3].a = false;
+	dvec[3].idx = 1;
 
 	SortArray<_generate_contacts_Pair> sa;
-	sa.sort(dvec,4);
+	sa.sort(dvec, 4);
 
-	for(int i=1;i<=2;i++) {
+	for (int i = 1; i <= 2; i++) {
 
 		if (dvec[i].a) {
 			Vector2 a = p_points_A[dvec[i].idx];
-			Vector2 b = n.plane_project(dB,a);
-			if (n.dot(a) > n.dot(b)-CMP_EPSILON)
+			Vector2 b = n.plane_project(dB, a);
+			if (n.dot(a) > n.dot(b) - CMP_EPSILON)
 				continue;
-			p_collector->call(a,b);
+			p_collector->call(a, b);
 		} else {
 			Vector2 b = p_points_B[dvec[i].idx];
-			Vector2 a = n.plane_project(dA,b);
-			if (n.dot(a) > n.dot(b)-CMP_EPSILON)
+			Vector2 a = n.plane_project(dA, b);
+			if (n.dot(a) > n.dot(b) - CMP_EPSILON)
 				continue;
-			p_collector->call(a,b);
+			p_collector->call(a, b);
 		}
 	}
 
-
-
 #elif 0
 	Vector2 axis = rel_A.normalized(); //make an axis
 	Vector2 axis_B = rel_B.normalized();
-	if (axis.dot(axis_B)<0)
-		axis_B=-axis_B;
-	axis=(axis+axis_B)*0.5;
+	if (axis.dot(axis_B) < 0)
+		axis_B = -axis_B;
+	axis = (axis + axis_B) * 0.5;
 	Vector2 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]);
 	Vector2 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]);
 
 	//sort all 4 points in axis
-	float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
+	float dvec[4] = { axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) };
 
 	//todo , find max/min and then use 2 central points
 	SortArray<float> sa;
-	sa.sort(dvec,4);
+	sa.sort(dvec, 4);
 
 	//use the middle ones as contacts
-	for (int i=1;i<=2;i++) {
+	for (int i = 1; i <= 2; i++) {
 
-		Vector2 a = base_A+axis*dvec[i];
-		Vector2 b = base_B+axis*dvec[i];
-		if (p_collector->normal.dot(a) > p_collector->normal.dot(b)-0.01) {
-			print_line("fail a: "+a);
-			print_line("fail b: "+b);
+		Vector2 a = base_A + axis * dvec[i];
+		Vector2 b = base_B + axis * dvec[i];
+		if (p_collector->normal.dot(a) > p_collector->normal.dot(b) - 0.01) {
+			print_line("fail a: " + a);
+			print_line("fail b: " + b);
 			continue;
 		}
-		print_line("res a: "+a);
-		print_line("res b: "+b);
-		p_collector->call(a,b);
+		print_line("res a: " + a);
+		print_line("res b: " + b);
+		p_collector->call(a, b);
 	}
 #endif
 }
 
-static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_point_count_A, const Vector2 * p_points_B,int p_point_count_B,_CollectorCallback2D *p_collector) {
-
+static void _generate_contacts_from_supports(const Vector2 *p_points_A, int p_point_count_A, const Vector2 *p_points_B, int p_point_count_B, _CollectorCallback2D *p_collector) {
 
 #ifdef DEBUG_ENABLED
-	ERR_FAIL_COND( p_point_count_A <1 );
-	ERR_FAIL_COND( p_point_count_B <1 );
+	ERR_FAIL_COND(p_point_count_A < 1);
+	ERR_FAIL_COND(p_point_count_B < 1);
 #endif
 
-
-	static const GenerateContactsFunc generate_contacts_func_table[2][2]={
+	static const GenerateContactsFunc generate_contacts_func_table[2][2] = {
 		{
-			_generate_contacts_point_point,
-			_generate_contacts_point_edge,
-		},{
-			0,
-			_generate_contacts_edge_edge,
+				_generate_contacts_point_point,
+				_generate_contacts_point_edge,
+		},
+		{
+				0,
+				_generate_contacts_edge_edge,
 		}
 	};
 
@@ -317,28 +307,25 @@ static void _generate_contacts_from_supports(const Vector2 * p_points_A,int p_po
 
 		pointcount_B = p_point_count_A;
 		pointcount_A = p_point_count_B;
-		points_A=p_points_B;
-		points_B=p_points_A;
+		points_A = p_points_B;
+		points_B = p_points_A;
 	} else {
 
 		pointcount_B = p_point_count_B;
 		pointcount_A = p_point_count_A;
-		points_A=p_points_A;
-		points_B=p_points_B;
+		points_A = p_points_A;
+		points_B = p_points_B;
 	}
 
-	int version_A = (pointcount_A > 3 ?  3 : pointcount_A) -1;
-	int version_B = (pointcount_B > 3 ?  3 : pointcount_B) -1;
+	int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1;
+	int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1;
 
 	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
 	ERR_FAIL_COND(!contacts_func);
-	contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_collector);
-
+	contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_collector);
 }
 
-
-
-template<class ShapeA, class ShapeB,bool castA=false,bool castB=false, bool withMargin=false>
+template <class ShapeA, class ShapeB, bool castA = false, bool castB = false, bool withMargin = false>
 class SeparatorAxisTest2D {
 
 	const ShapeA *shape_A;
@@ -356,16 +343,14 @@ class SeparatorAxisTest2D {
 	_CollectorCallback2D *callback;
 
 public:
-
 	_FORCE_INLINE_ bool test_previous_axis() {
 
-		if (callback && callback->sep_axis && *callback->sep_axis!=Vector2()) {
+		if (callback && callback->sep_axis && *callback->sep_axis != Vector2()) {
 			return test_axis(*callback->sep_axis);
 		} else {
 #ifdef DEBUG_ENABLED
 			best_axis_count++;
 #endif
-
 		}
 		return true;
 	}
@@ -393,45 +378,44 @@ public:
 		return true;
 	}
 
-	_FORCE_INLINE_ bool test_axis(const Vector2& p_axis) {
+	_FORCE_INLINE_ bool test_axis(const Vector2 &p_axis) {
 
-		Vector2 axis=p_axis;
+		Vector2 axis = p_axis;
 
-
-		if (	Math::abs(axis.x)<CMP_EPSILON &&
-			Math::abs(axis.y)<CMP_EPSILON) {
+		if (Math::abs(axis.x) < CMP_EPSILON &&
+				Math::abs(axis.y) < CMP_EPSILON) {
 			// strange case, try an upwards separator
-			axis=Vector2(0.0,1.0);
+			axis = Vector2(0.0, 1.0);
 		}
 
-		real_t min_A,max_A,min_B,max_B;
+		real_t min_A, max_A, min_B, max_B;
 
 		if (castA)
-			shape_A->project_range_cast(motion_A,axis,*transform_A,min_A,max_A);
+			shape_A->project_range_cast(motion_A, axis, *transform_A, min_A, max_A);
 		else
-			shape_A->project_range(axis,*transform_A,min_A,max_A);
+			shape_A->project_range(axis, *transform_A, min_A, max_A);
 
 		if (castB)
-			shape_B->project_range_cast(motion_B,axis,*transform_B,min_B,max_B);
+			shape_B->project_range_cast(motion_B, axis, *transform_B, min_B, max_B);
 		else
-			shape_B->project_range(axis,*transform_B,min_B,max_B);
+			shape_B->project_range(axis, *transform_B, min_B, max_B);
 
 		if (withMargin) {
-			min_A-=margin_A;
-			max_A+=margin_A;
-			min_B-=margin_B;
-			max_B+=margin_B;
+			min_A -= margin_A;
+			max_A += margin_A;
+			min_B -= margin_B;
+			max_B += margin_B;
 		}
 
-		min_B -= ( max_A - min_A ) * 0.5;
-		max_B += ( max_A - min_A ) * 0.5;
+		min_B -= (max_A - min_A) * 0.5;
+		max_B += (max_A - min_A) * 0.5;
 
-		real_t dmin = min_B - ( min_A + max_A ) * 0.5;
-		real_t dmax = max_B - ( min_A + max_A ) * 0.5;
+		real_t dmin = min_B - (min_A + max_A) * 0.5;
+		real_t dmax = max_B - (min_A + max_A) * 0.5;
 
 		if (dmin > 0.0 || dmax < 0.0) {
 			if (callback && callback->sep_axis)
-				*callback->sep_axis=axis;
+				*callback->sep_axis = axis;
 #ifdef DEBUG_ENABLED
 			best_axis_count++;
 #endif
@@ -443,26 +427,25 @@ public:
 
 		dmin = Math::abs(dmin);
 
-		if ( dmax < dmin ) {
-			if ( dmax < best_depth ) {
-				best_depth=dmax;
-				best_axis=axis;
+		if (dmax < dmin) {
+			if (dmax < best_depth) {
+				best_depth = dmax;
+				best_axis = axis;
 #ifdef DEBUG_ENABLED
-				best_axis_index=best_axis_count;
+				best_axis_index = best_axis_count;
 #endif
-
 			}
 		} else {
-			if ( dmin < best_depth ) {
-				best_depth=dmin;
-				best_axis=-axis; // keep it as A axis
+			if (dmin < best_depth) {
+				best_depth = dmin;
+				best_axis = -axis; // keep it as A axis
 #ifdef DEBUG_ENABLED
-				best_axis_index=best_axis_count;
+				best_axis_index = best_axis_count;
 #endif
 			}
 		}
 
-	//	print_line("test axis: "+p_axis+" depth: "+rtos(best_depth));
+//	print_line("test axis: "+p_axis+" depth: "+rtos(best_depth));
 #ifdef DEBUG_ENABLED
 		best_axis_count++;
 #endif
@@ -470,60 +453,55 @@ public:
 		return true;
 	}
 
-
 	_FORCE_INLINE_ void generate_contacts() {
 
 		// nothing to do, don't generate
-		if (best_axis==Vector2(0.0,0.0))
+		if (best_axis == Vector2(0.0, 0.0))
 			return;
 
-		callback->collided=true;
+		callback->collided = true;
 
 		if (!callback->callback)
 			return; //only collide, no callback
-		static const int max_supports=2;
+		static const int max_supports = 2;
 
 		Vector2 supports_A[max_supports];
 		int support_count_A;
 		if (castA) {
-			shape_A->get_supports_transformed_cast(motion_A,-best_axis,*transform_A,supports_A,support_count_A);
+			shape_A->get_supports_transformed_cast(motion_A, -best_axis, *transform_A, supports_A, support_count_A);
 		} else {
-			shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(),supports_A,support_count_A);
-			for(int i=0;i<support_count_A;i++) {
+			shape_A->get_supports(transform_A->basis_xform_inv(-best_axis).normalized(), supports_A, support_count_A);
+			for (int i = 0; i < support_count_A; i++) {
 				supports_A[i] = transform_A->xform(supports_A[i]);
 			}
 		}
 
 		if (withMargin) {
 
-			for(int i=0;i<support_count_A;i++) {
-				supports_A[i]+=-best_axis*margin_A;
+			for (int i = 0; i < support_count_A; i++) {
+				supports_A[i] += -best_axis * margin_A;
 			}
-
 		}
 
-
-
 		Vector2 supports_B[max_supports];
 		int support_count_B;
 		if (castB) {
-			shape_B->get_supports_transformed_cast(motion_B,best_axis,*transform_B,supports_B,support_count_B);
+			shape_B->get_supports_transformed_cast(motion_B, best_axis, *transform_B, supports_B, support_count_B);
 		} else {
-			shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(),supports_B,support_count_B);
-			for(int i=0;i<support_count_B;i++) {
+			shape_B->get_supports(transform_B->basis_xform_inv(best_axis).normalized(), supports_B, support_count_B);
+			for (int i = 0; i < support_count_B; i++) {
 				supports_B[i] = transform_B->xform(supports_B[i]);
 			}
 		}
 
 		if (withMargin) {
 
-			for(int i=0;i<support_count_B;i++) {
-				supports_B[i]+=best_axis*margin_B;
+			for (int i = 0; i < support_count_B; i++) {
+				supports_B[i] += best_axis * margin_B;
 			}
-
 		}
 
-/*
+		/*
 
 
 		print_line("**************************");
@@ -547,38 +525,33 @@ public:
 		}
 //*/
 
+		callback->normal = best_axis;
+		_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
 
-
-
-		callback->normal=best_axis;
-		_generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback);
-
-		if (callback && callback->sep_axis && *callback->sep_axis!=Vector2())
-			*callback->sep_axis=Vector2(); //invalidate previous axis (no test)
+		if (callback && callback->sep_axis && *callback->sep_axis != Vector2())
+			*callback->sep_axis = Vector2(); //invalidate previous axis (no test)
 		//CollisionSolver2DSW::CallbackResult cbk=NULL;
 		//cbk(Vector2(),Vector2(),NULL);
-
 	}
 
-	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A,const Matrix32& p_transform_a, const ShapeB *p_shape_B,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_A=Vector2(), const Vector2& p_motion_B=Vector2(),real_t p_margin_A=0,real_t p_margin_B=0) {
+	_FORCE_INLINE_ SeparatorAxisTest2D(const ShapeA *p_shape_A, const Matrix32 &p_transform_a, const ShapeB *p_shape_B, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_A = Vector2(), const Vector2 &p_motion_B = Vector2(), real_t p_margin_A = 0, real_t p_margin_B = 0) {
 
-		margin_A=p_margin_A;
-		margin_B=p_margin_B;
-		best_depth=1e15;
-		shape_A=p_shape_A;
-		shape_B=p_shape_B;
-		transform_A=&p_transform_a;
-		transform_B=&p_transform_b;
-		motion_A=p_motion_A;
-		motion_B=p_motion_B;
+		margin_A = p_margin_A;
+		margin_B = p_margin_B;
+		best_depth = 1e15;
+		shape_A = p_shape_A;
+		shape_B = p_shape_B;
+		transform_A = &p_transform_a;
+		transform_B = &p_transform_b;
+		motion_A = p_motion_A;
+		motion_B = p_motion_B;
 
-		callback=p_collector;
+		callback = p_collector;
 #ifdef DEBUG_ENABLED
-		best_axis_count=0;
-		best_axis_index=-1;
+		best_axis_count = 0;
+		best_axis_index = -1;
 #endif
 	}
-
 };
 
 /****** SAT TESTS *******/
@@ -586,37 +559,32 @@ public:
 /****** SAT TESTS *******/
 /****** SAT TESTS *******/
 
+#define TEST_POINT(m_a, m_b)                                                                \
+	((!separator.test_axis(((m_a) - (m_b)).normalized())) ||                                \
+			(castA && !separator.test_axis(((m_a) + p_motion_a - (m_b)).normalized())) ||   \
+			(castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \
+			(castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized())))
 
-#define TEST_POINT(m_a,m_b) \
-	 (	(!separator.test_axis(((m_a)-(m_b)).normalized())) ||\
-		(castA && !separator.test_axis(((m_a)+p_motion_a-(m_b)).normalized())) ||\
-		(castB && !separator.test_axis(((m_a)-((m_b)+p_motion_b)).normalized())) ||\
-		(castA && castB && !separator.test_axis(((m_a)+p_motion_a-((m_b)+p_motion_b)).normalized())) )
-
-
-typedef void (*CollisionFunc)(const Shape2DSW*,const Matrix32&,const Shape2DSW*,const Matrix32&,_CollectorCallback2D *p_collector,const Vector2&,const Vector2&,float,float);
+typedef void (*CollisionFunc)(const Shape2DSW *, const Matrix32 &, const Shape2DSW *, const Matrix32 &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, float, float);
 
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_segment(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a);
+	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW *>(p_b);
 
-	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
-	const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW*>(p_b);
-
-	SeparatorAxisTest2D<SegmentShape2DSW,SegmentShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,segment_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<SegmentShape2DSW, SegmentShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
 	//this collision is kind of pointless
 
-
 	//if (!separator.test_previous_axis())
 	//	return;
 
 	if (!separator.test_cast())
 		return;
 
-
 	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 	if (!separator.test_axis(segment_B->get_xformed_normal(p_transform_b)))
@@ -625,30 +593,26 @@ static void _collision_segment_segment(const Shape2DSW* p_a,const Matrix32& p_tr
 	if (withMargin) {
 		//points grow to circles
 
-
-		if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_a())) )
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_a())))
 			return;
-		if (TEST_POINT( p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(segment_B->get_b())) )
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(segment_B->get_b())))
 			return;
-		if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_a())) )
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_a())))
 			return;
-		if (TEST_POINT( p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(segment_B->get_b())) )
+		if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(segment_B->get_b())))
 			return;
 	}
 
 	separator.generate_contacts();
-
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_circle(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a);
+	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b);
 
-	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
-	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
-
-
-	SeparatorAxisTest2D<SegmentShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<SegmentShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -658,28 +622,26 @@ static void _collision_segment_circle(const Shape2DSW* p_a,const Matrix32& p_tra
 
 	//segment normal
 	if (!separator.test_axis(
-				(p_transform_a.xform(segment_A->get_b())-p_transform_a.xform(segment_A->get_a())).normalized().tangent()
-				))
+				(p_transform_a.xform(segment_A->get_b()) - p_transform_a.xform(segment_A->get_a())).normalized().tangent()))
 		return;
 
 	//endpoint a vs circle
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.get_origin()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.get_origin()))
 		return;
 	//endpoint b vs circle
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.get_origin()))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.get_origin()))
 		return;
 
-
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_rectangle(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
-	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<SegmentShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -703,48 +665,46 @@ static void _collision_segment_rectangle(const Shape2DSW* p_a,const Matrix32& p_
 		Vector2 a = p_transform_a.xform(segment_A->get_a());
 		Vector2 b = p_transform_a.xform(segment_A->get_b());
 
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a)))
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a)))
 			return;
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b)))
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b)))
 			return;
 
 		if (castA) {
 
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a+p_motion_a)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a + p_motion_a)))
 				return;
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b+p_motion_a)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b + p_motion_a)))
 				return;
 		}
 
 		if (castB) {
 
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b)))
 				return;
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b)))
 				return;
 		}
 
 		if (castA && castB) {
 
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,a-p_motion_b+p_motion_a)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, a - p_motion_b + p_motion_a)))
 				return;
-			if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,inv,b-p_motion_b+p_motion_a)))
+			if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, inv, b - p_motion_b + p_motion_a)))
 				return;
 		}
-
 	}
 
 	separator.generate_contacts();
-
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_capsule(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
-	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<SegmentShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -758,25 +718,25 @@ static void _collision_segment_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
 		return;
 
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5)))
 		return;
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5)))
 		return;
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5)))
 		return;
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5)))
 		return;
 
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW*>(p_a);
-	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+	const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<SegmentShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(segment_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<SegmentShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -787,37 +747,32 @@ static void _collision_segment_convex_polygon(const Shape2DSW* p_a,const Matrix3
 	if (!separator.test_axis(segment_A->get_xformed_normal(p_transform_a)))
 		return;
 
-	for(int i=0;i<convex_B->get_point_count();i++) {
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
 
-		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i)))
 			return;
 
 		if (withMargin) {
 
-			if (TEST_POINT(p_transform_a.xform(segment_A->get_a()),p_transform_b.xform(convex_B->get_point(i) )))
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), p_transform_b.xform(convex_B->get_point(i))))
 				return;
-			if (TEST_POINT(p_transform_a.xform(segment_A->get_b()),p_transform_b.xform(convex_B->get_point(i) )))
+			if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), p_transform_b.xform(convex_B->get_point(i))))
 				return;
-
 		}
-
 	}
 
 	separator.generate_contacts();
-
 }
 
-
 /////////
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
-	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_circle(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a);
+	const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<CircleShape2DSW,CircleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,circle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CircleShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -825,22 +780,19 @@ static void _collision_circle_circle(const Shape2DSW* p_a,const Matrix32& p_tran
 	if (!separator.test_cast())
 		return;
 
-	if (TEST_POINT(p_transform_a.get_origin(),p_transform_b.get_origin()))
+	if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.get_origin()))
 		return;
 
-
 	separator.generate_contacts();
-
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
-	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_rectangle(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<CircleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CircleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -848,9 +800,9 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
 	if (!separator.test_cast())
 		return;
 
-	const Vector2 &sphere=p_transform_a.elements[2];
-	const Vector2 *axis=&p_transform_b.elements[0];
-//	const Vector2& half_extents = rectangle_B->get_half_extents();
+	const Vector2 &sphere = p_transform_a.elements[2];
+	const Vector2 *axis = &p_transform_b.elements[0];
+	//	const Vector2& half_extents = rectangle_B->get_half_extents();
 
 	if (!separator.test_axis(axis[0].normalized()))
 		return;
@@ -861,42 +813,41 @@ static void _collision_circle_rectangle(const Shape2DSW* p_a,const Matrix32& p_t
 	Matrix32 binv = p_transform_b.affine_inverse();
 	{
 
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv,sphere ) ) )
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphere)))
 			return;
 	}
 
 	if (castA) {
 
 		Vector2 sphereofs = sphere + p_motion_a;
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs)))
 			return;
 	}
 
 	if (castB) {
 
 		Vector2 sphereofs = sphere - p_motion_b;
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs)))
 			return;
 	}
 
 	if (castA && castB) {
 
 		Vector2 sphereofs = sphere - p_motion_b + p_motion_a;
-		if (!separator.test_axis( rectangle_B->get_circle_axis(p_transform_b,binv, sphereofs) ) )
+		if (!separator.test_axis(rectangle_B->get_circle_axis(p_transform_b, binv, sphereofs)))
 			return;
 	}
 
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
-	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_capsule(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<CircleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CircleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -909,24 +860,21 @@ static void _collision_circle_capsule(const Shape2DSW* p_a,const Matrix32& p_tra
 		return;
 
 	//capsule endpoints
-	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*0.5)))
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5)))
 		return;
-	if (TEST_POINT(p_transform_a.get_origin(),(p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*-0.5)))
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5)))
 		return;
 
 	separator.generate_contacts();
-
-
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW*>(p_a);
-	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<CircleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(circle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CircleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -934,31 +882,28 @@ static void _collision_circle_convex_polygon(const Shape2DSW* p_a,const Matrix32
 	if (!separator.test_cast())
 		return;
 
-
 	//poly faces and poly points vs circle
-	for(int i=0;i<convex_B->get_point_count();i++) {
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
 
-		if (TEST_POINT( p_transform_a.get_origin(),p_transform_b.xform(convex_B->get_point(i)) ))
+		if (TEST_POINT(p_transform_a.get_origin(), p_transform_b.xform(convex_B->get_point(i))))
 			return;
 
-		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i)))
 			return;
 	}
 
 	separator.generate_contacts();
 }
 
-
 /////////
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
-	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW*>(p_b);
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a);
+	const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b);
 
-
-	SeparatorAxisTest2D<RectangleShape2DSW,RectangleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,rectangle_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<RectangleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -982,38 +927,38 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32&
 
 	if (withMargin) {
 
-		Matrix32 invA=p_transform_a.affine_inverse();
-		Matrix32 invB=p_transform_b.affine_inverse();
+		Matrix32 invA = p_transform_a.affine_inverse();
+		Matrix32 invB = p_transform_b.affine_inverse();
 
-		if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,p_transform_b,invB) ) )
+		if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, p_transform_b, invB)))
 			return;
 
 		if (castA || castB) {
 
 			Matrix32 aofs = p_transform_a;
-			aofs.elements[2]+=p_motion_a;
+			aofs.elements[2] += p_motion_a;
 
 			Matrix32 bofs = p_transform_b;
-			bofs.elements[2]+=p_motion_b;
+			bofs.elements[2] += p_motion_b;
 
 			Matrix32 aofsinv = aofs.affine_inverse();
 			Matrix32 bofsinv = bofs.affine_inverse();
 
 			if (castA) {
 
-				if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,p_transform_b,invB) ) )
+				if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, p_transform_b, invB)))
 					return;
 			}
 
 			if (castB) {
 
-				if (!separator.test_axis( rectangle_A->get_box_axis(p_transform_a,invA,rectangle_B,bofs,bofsinv) ) )
+				if (!separator.test_axis(rectangle_A->get_box_axis(p_transform_a, invA, rectangle_B, bofs, bofsinv)))
 					return;
 			}
 
 			if (castA && castB) {
 
-				if (!separator.test_axis( rectangle_A->get_box_axis(aofs,aofsinv,rectangle_B,bofs,bofsinv) ) )
+				if (!separator.test_axis(rectangle_A->get_box_axis(aofs, aofsinv, rectangle_B, bofs, bofsinv)))
 					return;
 			}
 		}
@@ -1022,14 +967,13 @@ static void _collision_rectangle_rectangle(const Shape2DSW* p_a,const Matrix32&
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
-	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<RectangleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<RectangleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1048,61 +992,55 @@ static void _collision_rectangle_capsule(const Shape2DSW* p_a,const Matrix32& p_
 	if (!separator.test_axis(p_transform_b.elements[0].normalized()))
 		return;
 
-
 	//box endpoints to capsule circles
 
 	Matrix32 boxinv = p_transform_a.affine_inverse();
 
-	for(int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
 		{
-			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
 
-			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint)))
 				return;
 		}
 
-
 		if (castA) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
-			capsule_endpoint-=p_motion_a;
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			capsule_endpoint -= p_motion_a;
 
-			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint)))
 				return;
 		}
 
 		if (castB) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
-			capsule_endpoint+=p_motion_b;
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			capsule_endpoint += p_motion_b;
 
-			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint)))
 				return;
 		}
 
 		if (castA && castB) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(i==0?0.5:-0.5);
-			capsule_endpoint-=p_motion_a;
-			capsule_endpoint+=p_motion_b;
-
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			capsule_endpoint -= p_motion_a;
+			capsule_endpoint += p_motion_b;
 
-			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,capsule_endpoint)))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint)))
 				return;
 		}
-
 	}
 
-
 	separator.generate_contacts();
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW*>(p_a);
-	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+template <bool castA, bool castB, bool withMargin>
+static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	SeparatorAxisTest2D<RectangleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(rectangle_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b);
 
+	SeparatorAxisTest2D<RectangleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1120,51 +1058,47 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW* p_a,const Matri
 	//convex faces
 	Matrix32 boxinv;
 	if (withMargin) {
-		boxinv=p_transform_a.affine_inverse();
+		boxinv = p_transform_a.affine_inverse();
 	}
-	for(int i=0;i<convex_B->get_point_count();i++) {
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
 
-		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i)))
 			return;
 
 		if (withMargin) {
 			//all points vs all points need to be tested if margin exist
-			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i)))))
+			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)))))
 				return;
 			if (castA) {
 
-				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))-p_motion_a)))
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) - p_motion_a)))
 					return;
 			}
 			if (castB) {
 
-				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b)))
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b)))
 					return;
 			}
 			if (castA && castB) {
 
-				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a,boxinv,p_transform_b.xform(convex_B->get_point(i))+p_motion_b-p_motion_a)))
+				if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, p_transform_b.xform(convex_B->get_point(i)) + p_motion_b - p_motion_a)))
 					return;
 			}
-
 		}
 	}
 
 	separator.generate_contacts();
-
 }
 
-
 /////////
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_capsule_capsule(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
-	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW*>(p_b);
+	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a);
+	const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b);
 
-
-	SeparatorAxisTest2D<CapsuleShape2DSW,CapsuleShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CapsuleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1182,32 +1116,29 @@ static void _collision_capsule_capsule(const Shape2DSW* p_a,const Matrix32& p_tr
 
 	//capsule endpoints
 
-	for(int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
-		Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(i==0?0.5:-0.5);
+		Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (i == 0 ? 0.5 : -0.5);
 
-		for(int j=0;j<2;j++) {
+		for (int j = 0; j < 2; j++) {
 
-			Vector2 capsule_endpoint_B = p_transform_b.get_origin()+p_transform_b.elements[1]*capsule_B->get_height()*(j==0?0.5:-0.5);
+			Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (j == 0 ? 0.5 : -0.5);
 
-			if (TEST_POINT(capsule_endpoint_A,capsule_endpoint_B) )
+			if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B))
 				return;
-
 		}
 	}
 
 	separator.generate_contacts();
-
 }
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
+template <bool castA, bool castB, bool withMargin>
+static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW*>(p_a);
-	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+	const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b);
 
-
-	SeparatorAxisTest2D<CapsuleShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(capsule_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<CapsuleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1220,40 +1151,35 @@ static void _collision_capsule_convex_polygon(const Shape2DSW* p_a,const Matrix3
 	if (!separator.test_axis(p_transform_a.elements[0].normalized()))
 		return;
 
-
 	//poly vs capsule
-	for(int i=0;i<convex_B->get_point_count();i++) {
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
 
 		Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
 
-		for(int j=0;j<2;j++) {
+		for (int j = 0; j < 2; j++) {
 
-			Vector2 capsule_endpoint_A = p_transform_a.get_origin()+p_transform_a.elements[1]*capsule_A->get_height()*(j==0?0.5:-0.5);
+			Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (j == 0 ? 0.5 : -0.5);
 
-			if (TEST_POINT(capsule_endpoint_A,cpoint ))
+			if (TEST_POINT(capsule_endpoint_A, cpoint))
 				return;
-
 		}
 
-		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i)))
 			return;
 	}
 
 	separator.generate_contacts();
 }
 
-
 /////////
 
+template <bool castA, bool castB, bool withMargin>
+static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Matrix32 &p_transform_a, const Shape2DSW *p_b, const Matrix32 &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, float p_margin_A, float p_margin_B) {
 
-template<bool castA, bool castB,bool withMargin>
-static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const Matrix32& p_transform_a,const Shape2DSW* p_b,const Matrix32& p_transform_b,_CollectorCallback2D *p_collector,const Vector2& p_motion_a,const Vector2& p_motion_b,float p_margin_A,float p_margin_B) {
-
-
-	const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW*>(p_a);
-	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW*>(p_b);
+	const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW *>(p_a);
+	const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b);
 
-	SeparatorAxisTest2D<ConvexPolygonShape2DSW,ConvexPolygonShape2DSW,castA,castB,withMargin> separator(convex_A,p_transform_a,convex_B,p_transform_b,p_collector,p_motion_a,p_motion_b,p_margin_A,p_margin_B);
+	SeparatorAxisTest2D<ConvexPolygonShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -1261,345 +1187,332 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW* p_a,const
 	if (!separator.test_cast())
 		return;
 
+	for (int i = 0; i < convex_A->get_point_count(); i++) {
 
-	for(int i=0;i<convex_A->get_point_count();i++) {
-
-		if (!separator.test_axis( convex_A->get_xformed_segment_normal(p_transform_a,i)))
+		if (!separator.test_axis(convex_A->get_xformed_segment_normal(p_transform_a, i)))
 			return;
 	}
 
-	for(int i=0;i<convex_B->get_point_count();i++) {
+	for (int i = 0; i < convex_B->get_point_count(); i++) {
 
-		if (!separator.test_axis( convex_B->get_xformed_segment_normal(p_transform_b,i)))
+		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i)))
 			return;
-
 	}
 
 	if (withMargin) {
 
-		for(int i=0;i<convex_A->get_point_count();i++) {
-			for(int j=0;j<convex_B->get_point_count();j++) {
+		for (int i = 0; i < convex_A->get_point_count(); i++) {
+			for (int j = 0; j < convex_B->get_point_count(); j++) {
 
-				if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)) , p_transform_b.xform(convex_B->get_point(j))))
+				if (TEST_POINT(p_transform_a.xform(convex_A->get_point(i)), p_transform_b.xform(convex_B->get_point(j))))
 					return;
 			}
 		}
-
 	}
 
 	separator.generate_contacts();
-
 }
 
-
 ////////
 
-bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A, const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, float p_margin_A, float p_margin_B) {
 
-	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
+	Physics2DServer::ShapeType type_A = p_shape_A->get_type();
 
-	ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_LINE,false);
+	ERR_FAIL_COND_V(type_A == Physics2DServer::SHAPE_LINE, false);
 	//ERR_FAIL_COND_V(type_A==Physics2DServer::SHAPE_RAY,false);
-	ERR_FAIL_COND_V(p_shape_A->is_concave(),false);
+	ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
 
-	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
+	Physics2DServer::ShapeType type_B = p_shape_B->get_type();
 
-	ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_LINE,false);
+	ERR_FAIL_COND_V(type_B == Physics2DServer::SHAPE_LINE, false);
 	//ERR_FAIL_COND_V(type_B==Physics2DServer::SHAPE_RAY,false);
-	ERR_FAIL_COND_V(p_shape_B->is_concave(),false);
+	ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
 
-
-	static const CollisionFunc collision_table[5][5]={
-		{_collision_segment_segment<false,false,false>,
-		 _collision_segment_circle<false,false,false>,
-		 _collision_segment_rectangle<false,false,false>,
-		 _collision_segment_capsule<false,false,false>,
-		 _collision_segment_convex_polygon<false,false,false>},
-		{0,
-		 _collision_circle_circle<false,false,false>,
-		 _collision_circle_rectangle<false,false,false>,
-		 _collision_circle_capsule<false,false,false>,
-		 _collision_circle_convex_polygon<false,false,false>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<false,false,false>,
-		 _collision_rectangle_capsule<false,false,false>,
-		 _collision_rectangle_convex_polygon<false,false,false>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<false,false,false>,
-		 _collision_capsule_convex_polygon<false,false,false>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<false,false,false>}
+	static const CollisionFunc collision_table[5][5] = {
+		{ _collision_segment_segment<false, false, false>,
+				_collision_segment_circle<false, false, false>,
+				_collision_segment_rectangle<false, false, false>,
+				_collision_segment_capsule<false, false, false>,
+				_collision_segment_convex_polygon<false, false, false> },
+		{ 0,
+				_collision_circle_circle<false, false, false>,
+				_collision_circle_rectangle<false, false, false>,
+				_collision_circle_capsule<false, false, false>,
+				_collision_circle_convex_polygon<false, false, false> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<false, false, false>,
+				_collision_rectangle_capsule<false, false, false>,
+				_collision_rectangle_convex_polygon<false, false, false> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<false, false, false>,
+				_collision_capsule_convex_polygon<false, false, false> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<false, false, false> }
 
 	};
 
-	static const CollisionFunc collision_table_castA[5][5]={
-		{_collision_segment_segment<true,false,false>,
-		 _collision_segment_circle<true,false,false>,
-		 _collision_segment_rectangle<true,false,false>,
-		 _collision_segment_capsule<true,false,false>,
-		 _collision_segment_convex_polygon<true,false,false>},
-		{0,
-		 _collision_circle_circle<true,false,false>,
-		 _collision_circle_rectangle<true,false,false>,
-		 _collision_circle_capsule<true,false,false>,
-		 _collision_circle_convex_polygon<true,false,false>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<true,false,false>,
-		 _collision_rectangle_capsule<true,false,false>,
-		 _collision_rectangle_convex_polygon<true,false,false>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<true,false,false>,
-		 _collision_capsule_convex_polygon<true,false,false>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<true,false,false>}
+	static const CollisionFunc collision_table_castA[5][5] = {
+		{ _collision_segment_segment<true, false, false>,
+				_collision_segment_circle<true, false, false>,
+				_collision_segment_rectangle<true, false, false>,
+				_collision_segment_capsule<true, false, false>,
+				_collision_segment_convex_polygon<true, false, false> },
+		{ 0,
+				_collision_circle_circle<true, false, false>,
+				_collision_circle_rectangle<true, false, false>,
+				_collision_circle_capsule<true, false, false>,
+				_collision_circle_convex_polygon<true, false, false> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<true, false, false>,
+				_collision_rectangle_capsule<true, false, false>,
+				_collision_rectangle_convex_polygon<true, false, false> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<true, false, false>,
+				_collision_capsule_convex_polygon<true, false, false> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<true, false, false> }
 
 	};
 
-	static const CollisionFunc collision_table_castB[5][5]={
-		{_collision_segment_segment<false,true,false>,
-		 _collision_segment_circle<false,true,false>,
-		 _collision_segment_rectangle<false,true,false>,
-		 _collision_segment_capsule<false,true,false>,
-		 _collision_segment_convex_polygon<false,true,false>},
-		{0,
-		 _collision_circle_circle<false,true,false>,
-		 _collision_circle_rectangle<false,true,false>,
-		 _collision_circle_capsule<false,true,false>,
-		 _collision_circle_convex_polygon<false,true,false>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<false,true,false>,
-		 _collision_rectangle_capsule<false,true,false>,
-		 _collision_rectangle_convex_polygon<false,true,false>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<false,true,false>,
-		 _collision_capsule_convex_polygon<false,true,false>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<false,true,false>}
+	static const CollisionFunc collision_table_castB[5][5] = {
+		{ _collision_segment_segment<false, true, false>,
+				_collision_segment_circle<false, true, false>,
+				_collision_segment_rectangle<false, true, false>,
+				_collision_segment_capsule<false, true, false>,
+				_collision_segment_convex_polygon<false, true, false> },
+		{ 0,
+				_collision_circle_circle<false, true, false>,
+				_collision_circle_rectangle<false, true, false>,
+				_collision_circle_capsule<false, true, false>,
+				_collision_circle_convex_polygon<false, true, false> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<false, true, false>,
+				_collision_rectangle_capsule<false, true, false>,
+				_collision_rectangle_convex_polygon<false, true, false> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<false, true, false>,
+				_collision_capsule_convex_polygon<false, true, false> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<false, true, false> }
 
 	};
 
-	static const CollisionFunc collision_table_castA_castB[5][5]={
-		{_collision_segment_segment<true,true,false>,
-		 _collision_segment_circle<true,true,false>,
-		 _collision_segment_rectangle<true,true,false>,
-		 _collision_segment_capsule<true,true,false>,
-		 _collision_segment_convex_polygon<true,true,false>},
-		{0,
-		 _collision_circle_circle<true,true,false>,
-		 _collision_circle_rectangle<true,true,false>,
-		 _collision_circle_capsule<true,true,false>,
-		 _collision_circle_convex_polygon<true,true,false>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<true,true,false>,
-		 _collision_rectangle_capsule<true,true,false>,
-		 _collision_rectangle_convex_polygon<true,true,false>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<true,true,false>,
-		 _collision_capsule_convex_polygon<true,true,false>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<true,true,false>}
+	static const CollisionFunc collision_table_castA_castB[5][5] = {
+		{ _collision_segment_segment<true, true, false>,
+				_collision_segment_circle<true, true, false>,
+				_collision_segment_rectangle<true, true, false>,
+				_collision_segment_capsule<true, true, false>,
+				_collision_segment_convex_polygon<true, true, false> },
+		{ 0,
+				_collision_circle_circle<true, true, false>,
+				_collision_circle_rectangle<true, true, false>,
+				_collision_circle_capsule<true, true, false>,
+				_collision_circle_convex_polygon<true, true, false> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<true, true, false>,
+				_collision_rectangle_capsule<true, true, false>,
+				_collision_rectangle_convex_polygon<true, true, false> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<true, true, false>,
+				_collision_capsule_convex_polygon<true, true, false> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<true, true, false> }
 
 	};
 
-	static const CollisionFunc collision_table_margin[5][5]={
-		{_collision_segment_segment<false,false,true>,
-		 _collision_segment_circle<false,false,true>,
-		 _collision_segment_rectangle<false,false,true>,
-		 _collision_segment_capsule<false,false,true>,
-		 _collision_segment_convex_polygon<false,false,true>},
-		{0,
-		 _collision_circle_circle<false,false,true>,
-		 _collision_circle_rectangle<false,false,true>,
-		 _collision_circle_capsule<false,false,true>,
-		 _collision_circle_convex_polygon<false,false,true>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<false,false,true>,
-		 _collision_rectangle_capsule<false,false,true>,
-		 _collision_rectangle_convex_polygon<false,false,true>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<false,false,true>,
-		 _collision_capsule_convex_polygon<false,false,true>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<false,false,true>}
+	static const CollisionFunc collision_table_margin[5][5] = {
+		{ _collision_segment_segment<false, false, true>,
+				_collision_segment_circle<false, false, true>,
+				_collision_segment_rectangle<false, false, true>,
+				_collision_segment_capsule<false, false, true>,
+				_collision_segment_convex_polygon<false, false, true> },
+		{ 0,
+				_collision_circle_circle<false, false, true>,
+				_collision_circle_rectangle<false, false, true>,
+				_collision_circle_capsule<false, false, true>,
+				_collision_circle_convex_polygon<false, false, true> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<false, false, true>,
+				_collision_rectangle_capsule<false, false, true>,
+				_collision_rectangle_convex_polygon<false, false, true> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<false, false, true>,
+				_collision_capsule_convex_polygon<false, false, true> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<false, false, true> }
 
 	};
 
-	static const CollisionFunc collision_table_castA_margin[5][5]={
-		{_collision_segment_segment<true,false,true>,
-		 _collision_segment_circle<true,false,true>,
-		 _collision_segment_rectangle<true,false,true>,
-		 _collision_segment_capsule<true,false,true>,
-		 _collision_segment_convex_polygon<true,false,true>},
-		{0,
-		 _collision_circle_circle<true,false,true>,
-		 _collision_circle_rectangle<true,false,true>,
-		 _collision_circle_capsule<true,false,true>,
-		 _collision_circle_convex_polygon<true,false,true>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<true,false,true>,
-		 _collision_rectangle_capsule<true,false,true>,
-		 _collision_rectangle_convex_polygon<true,false,true>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<true,false,true>,
-		 _collision_capsule_convex_polygon<true,false,true>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<true,false,true>}
+	static const CollisionFunc collision_table_castA_margin[5][5] = {
+		{ _collision_segment_segment<true, false, true>,
+				_collision_segment_circle<true, false, true>,
+				_collision_segment_rectangle<true, false, true>,
+				_collision_segment_capsule<true, false, true>,
+				_collision_segment_convex_polygon<true, false, true> },
+		{ 0,
+				_collision_circle_circle<true, false, true>,
+				_collision_circle_rectangle<true, false, true>,
+				_collision_circle_capsule<true, false, true>,
+				_collision_circle_convex_polygon<true, false, true> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<true, false, true>,
+				_collision_rectangle_capsule<true, false, true>,
+				_collision_rectangle_convex_polygon<true, false, true> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<true, false, true>,
+				_collision_capsule_convex_polygon<true, false, true> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<true, false, true> }
 
 	};
 
-	static const CollisionFunc collision_table_castB_margin[5][5]={
-		{_collision_segment_segment<false,true,true>,
-		 _collision_segment_circle<false,true,true>,
-		 _collision_segment_rectangle<false,true,true>,
-		 _collision_segment_capsule<false,true,true>,
-		 _collision_segment_convex_polygon<false,true,true>},
-		{0,
-		 _collision_circle_circle<false,true,true>,
-		 _collision_circle_rectangle<false,true,true>,
-		 _collision_circle_capsule<false,true,true>,
-		 _collision_circle_convex_polygon<false,true,true>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<false,true,true>,
-		 _collision_rectangle_capsule<false,true,true>,
-		 _collision_rectangle_convex_polygon<false,true,true>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<false,true,true>,
-		 _collision_capsule_convex_polygon<false,true,true>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<false,true,true>}
+	static const CollisionFunc collision_table_castB_margin[5][5] = {
+		{ _collision_segment_segment<false, true, true>,
+				_collision_segment_circle<false, true, true>,
+				_collision_segment_rectangle<false, true, true>,
+				_collision_segment_capsule<false, true, true>,
+				_collision_segment_convex_polygon<false, true, true> },
+		{ 0,
+				_collision_circle_circle<false, true, true>,
+				_collision_circle_rectangle<false, true, true>,
+				_collision_circle_capsule<false, true, true>,
+				_collision_circle_convex_polygon<false, true, true> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<false, true, true>,
+				_collision_rectangle_capsule<false, true, true>,
+				_collision_rectangle_convex_polygon<false, true, true> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<false, true, true>,
+				_collision_capsule_convex_polygon<false, true, true> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<false, true, true> }
 
 	};
 
-	static const CollisionFunc collision_table_castA_castB_margin[5][5]={
-		{_collision_segment_segment<true,true,true>,
-		 _collision_segment_circle<true,true,true>,
-		 _collision_segment_rectangle<true,true,true>,
-		 _collision_segment_capsule<true,true,true>,
-		 _collision_segment_convex_polygon<true,true,true>},
-		{0,
-		 _collision_circle_circle<true,true,true>,
-		 _collision_circle_rectangle<true,true,true>,
-		 _collision_circle_capsule<true,true,true>,
-		 _collision_circle_convex_polygon<true,true,true>},
-		{0,
-		 0,
-		 _collision_rectangle_rectangle<true,true,true>,
-		 _collision_rectangle_capsule<true,true,true>,
-		 _collision_rectangle_convex_polygon<true,true,true>},
-		{0,
-		 0,
-		 0,
-		 _collision_capsule_capsule<true,true,true>,
-		 _collision_capsule_convex_polygon<true,true,true>},
-		{0,
-		 0,
-		 0,
-		 0,
-		 _collision_convex_polygon_convex_polygon<true,true,true>}
+	static const CollisionFunc collision_table_castA_castB_margin[5][5] = {
+		{ _collision_segment_segment<true, true, true>,
+				_collision_segment_circle<true, true, true>,
+				_collision_segment_rectangle<true, true, true>,
+				_collision_segment_capsule<true, true, true>,
+				_collision_segment_convex_polygon<true, true, true> },
+		{ 0,
+				_collision_circle_circle<true, true, true>,
+				_collision_circle_rectangle<true, true, true>,
+				_collision_circle_capsule<true, true, true>,
+				_collision_circle_convex_polygon<true, true, true> },
+		{ 0,
+				0,
+				_collision_rectangle_rectangle<true, true, true>,
+				_collision_rectangle_capsule<true, true, true>,
+				_collision_rectangle_convex_polygon<true, true, true> },
+		{ 0,
+				0,
+				0,
+				_collision_capsule_capsule<true, true, true>,
+				_collision_capsule_convex_polygon<true, true, true> },
+		{ 0,
+				0,
+				0,
+				0,
+				_collision_convex_polygon_convex_polygon<true, true, true> }
 
 	};
 
-
 	_CollectorCallback2D callback;
-	callback.callback=p_result_callback;
-	callback.swap=p_swap;
-	callback.userdata=p_userdata;
-	callback.collided=false;
-	callback.sep_axis=sep_axis;
+	callback.callback = p_result_callback;
+	callback.swap = p_swap;
+	callback.userdata = p_userdata;
+	callback.collided = false;
+	callback.sep_axis = sep_axis;
 
-	const Shape2DSW *A=p_shape_A;
-	const Shape2DSW *B=p_shape_B;
-	const Matrix32 *transform_A=&p_transform_A;
-	const Matrix32 *transform_B=&p_transform_B;
-	const Vector2 *motion_A=&p_motion_A;
-	const Vector2 *motion_B=&p_motion_B;
-	real_t margin_A=p_margin_A,margin_B=p_margin_B;
+	const Shape2DSW *A = p_shape_A;
+	const Shape2DSW *B = p_shape_B;
+	const Matrix32 *transform_A = &p_transform_A;
+	const Matrix32 *transform_B = &p_transform_B;
+	const Vector2 *motion_A = &p_motion_A;
+	const Vector2 *motion_B = &p_motion_B;
+	real_t margin_A = p_margin_A, margin_B = p_margin_B;
 
 	if (type_A > type_B) {
-		SWAP(A,B);
-		SWAP(transform_A,transform_B);
-		SWAP(type_A,type_B);
-		SWAP(motion_A,motion_B);
-		SWAP(margin_A,margin_B);
+		SWAP(A, B);
+		SWAP(transform_A, transform_B);
+		SWAP(type_A, type_B);
+		SWAP(motion_A, motion_B);
+		SWAP(margin_A, margin_B);
 		callback.swap = !callback.swap;
 	}
 
-
 	CollisionFunc collision_func;
 
 	if (p_margin_A || p_margin_B) {
-		if (*motion_A==Vector2() && *motion_B==Vector2()) {
-			collision_func = collision_table_margin[type_A-2][type_B-2];
-		} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
-			collision_func = collision_table_castA_margin[type_A-2][type_B-2];
-		} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
-			collision_func = collision_table_castB_margin[type_A-2][type_B-2];
+		if (*motion_A == Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_margin[type_A - 2][type_B - 2];
+		} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_castA_margin[type_A - 2][type_B - 2];
+		} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
+			collision_func = collision_table_castB_margin[type_A - 2][type_B - 2];
 		} else {
-			collision_func = collision_table_castA_castB_margin[type_A-2][type_B-2];
+			collision_func = collision_table_castA_castB_margin[type_A - 2][type_B - 2];
 		}
 	} else {
 
-		if (*motion_A==Vector2() && *motion_B==Vector2()) {
-			collision_func = collision_table[type_A-2][type_B-2];
-		} else if (*motion_A!=Vector2() &&  *motion_B==Vector2()) {
-			collision_func = collision_table_castA[type_A-2][type_B-2];
-		} else if (*motion_A==Vector2() && *motion_B!=Vector2()) {
-			collision_func = collision_table_castB[type_A-2][type_B-2];
+		if (*motion_A == Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table[type_A - 2][type_B - 2];
+		} else if (*motion_A != Vector2() && *motion_B == Vector2()) {
+			collision_func = collision_table_castA[type_A - 2][type_B - 2];
+		} else if (*motion_A == Vector2() && *motion_B != Vector2()) {
+			collision_func = collision_table_castB[type_A - 2][type_B - 2];
 		} else {
-			collision_func = collision_table_castA_castB[type_A-2][type_B-2];
+			collision_func = collision_table_castA_castB[type_A - 2][type_B - 2];
 		}
-
 	}
 
+	ERR_FAIL_COND_V(!collision_func, false);
 
-
-	ERR_FAIL_COND_V(!collision_func,false);
-
-	collision_func(A,*transform_A,B,*transform_B,&callback,*motion_A,*motion_B,margin_A,margin_B);
+	collision_func(A, *transform_A, B, *transform_B, &callback, *motion_A, *motion_B, margin_A, margin_B);
 
 	return callback.collided;
-
-
 }
diff --git a/servers/physics_2d/collision_solver_2d_sat.h b/servers/physics_2d/collision_solver_2d_sat.h
index b885dba91..489e59147 100644
--- a/servers/physics_2d/collision_solver_2d_sat.h
+++ b/servers/physics_2d/collision_solver_2d_sat.h
@@ -31,7 +31,6 @@
 
 #include "collision_solver_2d_sw.h"
 
-
-bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32& p_transform_A, const Vector2& p_motion_A,const Shape2DSW *p_shape_B, const Matrix32& p_transform_B,const Vector2& p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
+bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector2 *sep_axis = NULL, float p_margin_A = 0, float p_margin_B = 0);
 
 #endif // COLLISION_SOLVER_2D_SAT_H
diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp
index 0ad519c9d..594e6a9ee 100644
--- a/servers/physics_2d/collision_solver_2d_sw.cpp
+++ b/servers/physics_2d/collision_solver_2d_sw.cpp
@@ -29,89 +29,78 @@
 #include "collision_solver_2d_sw.h"
 #include "collision_solver_2d_sat.h"
 
-
 #define collision_solver sat_2d_calculate_penetration
 //#define collision_solver gjk_epa_calculate_penetration
 
+bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
 
-bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) {
-
-
-	const LineShape2DSW *line = static_cast<const LineShape2DSW*>(p_shape_A);
-	if (p_shape_B->get_type()==Physics2DServer::SHAPE_LINE)
+	const LineShape2DSW *line = static_cast<const LineShape2DSW *>(p_shape_A);
+	if (p_shape_B->get_type() == Physics2DServer::SHAPE_LINE)
 		return false;
 
-
 	Vector2 n = p_transform_A.basis_xform(line->get_normal()).normalized();
-	Vector2 p = p_transform_A.xform(line->get_normal()*line->get_d());
+	Vector2 p = p_transform_A.xform(line->get_normal() * line->get_d());
 	real_t d = n.dot(p);
 
 	Vector2 supports[2];
 	int support_count;
 
-	p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(),supports,support_count);
-
-	bool found=false;
+	p_shape_B->get_supports(p_transform_A.affine_inverse().basis_xform(-n).normalized(), supports, support_count);
 
+	bool found = false;
 
-	for(int i=0;i<support_count;i++) {
+	for (int i = 0; i < support_count; i++) {
 
-		supports[i] = p_transform_B.xform( supports[i] );
+		supports[i] = p_transform_B.xform(supports[i]);
 		real_t pd = n.dot(supports[i]);
-		if (pd>=d)
+		if (pd >= d)
 			continue;
-		found=true;
+		found = true;
 
-		Vector2 support_A = supports[i] - n*(pd-d);
+		Vector2 support_A = supports[i] - n * (pd - d);
 
 		if (p_result_callback) {
 			if (p_swap_result)
-				p_result_callback(supports[i],support_A,p_userdata);
+				p_result_callback(supports[i], support_A, p_userdata);
 			else
-				p_result_callback(support_A,supports[i],p_userdata);
+				p_result_callback(support_A, supports[i], p_userdata);
 		}
-
 	}
 
-
 	return found;
 }
 
-bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis) {
-
-
+bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) {
 
-	const RayShape2DSW *ray = static_cast<const RayShape2DSW*>(p_shape_A);
-	if (p_shape_B->get_type()==Physics2DServer::SHAPE_RAY)
+	const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A);
+	if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY)
 		return false;
 
 	Vector2 from = p_transform_A.get_origin();
-	Vector2 to = from+p_transform_A[1]*ray->get_length();
-	Vector2 support_A=to;
+	Vector2 to = from + p_transform_A[1] * ray->get_length();
+	Vector2 support_A = to;
 
 	Matrix32 invb = p_transform_B.affine_inverse();
 	from = invb.xform(from);
 	to = invb.xform(to);
 
-	Vector2 p,n;
-	if (!p_shape_B->intersect_segment(from,to,p,n)) {
+	Vector2 p, n;
+	if (!p_shape_B->intersect_segment(from, to, p, n)) {
 
 		if (sep_axis)
-			*sep_axis=p_transform_A[1].normalized();
+			*sep_axis = p_transform_A[1].normalized();
 		return false;
 	}
 
-
-	Vector2 support_B=p_transform_B.xform(p);
+	Vector2 support_B = p_transform_B.xform(p);
 
 	if (p_result_callback) {
 		if (p_swap_result)
-			p_result_callback(support_B,support_A,p_userdata);
+			p_result_callback(support_B, support_A, p_userdata);
 		else
-			p_result_callback(support_A,support_B,p_userdata);
+			p_result_callback(support_A, support_B, p_userdata);
 	}
 	return true;
-
 }
 
 /*
@@ -159,113 +148,102 @@ struct _ConcaveCollisionInfo2D {
 	int aabb_tests;
 	int collisions;
 	Vector2 *sep_axis;
-
 };
 
 void CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) {
 
-
-
-	_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D*)(p_userdata);
+	_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata);
 	cinfo.aabb_tests++;
 	if (!cinfo.result_callback && cinfo.collided)
 		return; //already collided and no contacts requested, don't test anymore
 
-	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex,*cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result,cinfo.sep_axis,cinfo.margin_A,cinfo.margin_B );
+	bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, cinfo.motion_A, p_convex, *cinfo.transform_B, cinfo.motion_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result, cinfo.sep_axis, cinfo.margin_A, cinfo.margin_B);
 	if (!collided)
 		return;
 
-
-	cinfo.collided=true;
+	cinfo.collided = true;
 	cinfo.collisions++;
-
 }
 
-bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
-
+bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, float p_margin_A, float p_margin_B) {
 
-	const ConcaveShape2DSW *concave_B=static_cast<const ConcaveShape2DSW*>(p_shape_B);
+	const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B);
 
 	_ConcaveCollisionInfo2D cinfo;
-	cinfo.transform_A=&p_transform_A;
-	cinfo.shape_A=p_shape_A;
-	cinfo.transform_B=&p_transform_B;
-	cinfo.motion_A=p_motion_A;
-	cinfo.result_callback=p_result_callback;
-	cinfo.userdata=p_userdata;
-	cinfo.swap_result=p_swap_result;
-	cinfo.collided=false;
-	cinfo.collisions=0;
-	cinfo.sep_axis=sep_axis;
-	cinfo.margin_A=p_margin_A;
-	cinfo.margin_B=p_margin_B;
+	cinfo.transform_A = &p_transform_A;
+	cinfo.shape_A = p_shape_A;
+	cinfo.transform_B = &p_transform_B;
+	cinfo.motion_A = p_motion_A;
+	cinfo.result_callback = p_result_callback;
+	cinfo.userdata = p_userdata;
+	cinfo.swap_result = p_swap_result;
+	cinfo.collided = false;
+	cinfo.collisions = 0;
+	cinfo.sep_axis = sep_axis;
+	cinfo.margin_A = p_margin_A;
+	cinfo.margin_B = p_margin_B;
 
-	cinfo.aabb_tests=0;
+	cinfo.aabb_tests = 0;
 
 	Matrix32 rel_transform = p_transform_A;
-	rel_transform.elements[2]-=p_transform_B.elements[2];
+	rel_transform.elements[2] -= p_transform_B.elements[2];
 
 	//quickly compute a local Rect2
 
 	Rect2 local_aabb;
-	for(int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
-	     Vector2 axis( p_transform_B.elements[i] );
-	     float axis_scale = 1.0/axis.length();
-	     axis*=axis_scale;
+		Vector2 axis(p_transform_B.elements[i]);
+		float axis_scale = 1.0 / axis.length();
+		axis *= axis_scale;
 
-	     float smin,smax;
-	     p_shape_A->project_rangev(axis,rel_transform,smin,smax);
-	     smin*=axis_scale;
-	     smax*=axis_scale;
+		float smin, smax;
+		p_shape_A->project_rangev(axis, rel_transform, smin, smax);
+		smin *= axis_scale;
+		smax *= axis_scale;
 
-	     local_aabb.pos[i]=smin;
-	     local_aabb.size[i]=smax-smin;
+		local_aabb.pos[i] = smin;
+		local_aabb.size[i] = smax - smin;
 	}
 
-	concave_B->cull(local_aabb,concave_callback,&cinfo);
+	concave_B->cull(local_aabb, concave_callback, &cinfo);
 
-
-//	print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));
+	//	print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));
 	return cinfo.collided;
 }
 
+bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis, float p_margin_A, float p_margin_B) {
 
-bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis,float p_margin_A,float p_margin_B) {
-
-
-
-
-	Physics2DServer::ShapeType type_A=p_shape_A->get_type();
-	Physics2DServer::ShapeType type_B=p_shape_B->get_type();
-	bool concave_A=p_shape_A->is_concave();
-	bool concave_B=p_shape_B->is_concave();
-	real_t margin_A=p_margin_A,margin_B=p_margin_B;
+	Physics2DServer::ShapeType type_A = p_shape_A->get_type();
+	Physics2DServer::ShapeType type_B = p_shape_B->get_type();
+	bool concave_A = p_shape_A->is_concave();
+	bool concave_B = p_shape_B->is_concave();
+	real_t margin_A = p_margin_A, margin_B = p_margin_B;
 
 	bool swap = false;
 
-	if (type_A>type_B) {
-		SWAP(type_A,type_B);
-		SWAP(concave_A,concave_B);
-		SWAP(margin_A,margin_B);
-		swap=true;
+	if (type_A > type_B) {
+		SWAP(type_A, type_B);
+		SWAP(concave_A, concave_B);
+		SWAP(margin_A, margin_B);
+		swap = true;
 	}
 
-	if (type_A==Physics2DServer::SHAPE_LINE) {
+	if (type_A == Physics2DServer::SHAPE_LINE) {
 
-		if (type_B==Physics2DServer::SHAPE_LINE || type_B==Physics2DServer::SHAPE_RAY) {
+		if (type_B == Physics2DServer::SHAPE_LINE || type_B == Physics2DServer::SHAPE_RAY) {
 			return false;
-		//if (type_B==Physics2DServer::SHAPE_RAY) {
-		//	return false;
+			//if (type_B==Physics2DServer::SHAPE_RAY) {
+			//	return false;
 		}
 
 		if (swap) {
-			return solve_static_line(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true);
+			return solve_static_line(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
 		} else {
-			return solve_static_line(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false);
+			return solve_static_line(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}
 
-	/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
+		/*} else if (type_A==Physics2DServer::SHAPE_RAY) {
 
 		if (type_B==Physics2DServer::SHAPE_RAY)
 			return false;
@@ -276,41 +254,33 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A,const Matrix32& p_tra
 			return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_inverse_B,p_result_callback,p_userdata,false);
 		}
 */
-	} else if (type_A==Physics2DServer::SHAPE_RAY) {
+	} else if (type_A == Physics2DServer::SHAPE_RAY) {
 
-		if (type_B==Physics2DServer::SHAPE_RAY) {
+		if (type_B == Physics2DServer::SHAPE_RAY) {
 
 			return false; //no ray-ray
 		}
 
-
 		if (swap) {
-			return solve_raycast(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true,sep_axis);
+			return solve_raycast(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis);
 		} else {
-			return solve_raycast(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false,sep_axis);
+			return solve_raycast(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis);
 		}
 
-
 	} else if (concave_B) {
 
-
 		if (concave_A)
 			return false;
 
 		if (!swap)
-			return solve_concave(p_shape_A,p_transform_A,p_motion_A,p_shape_B,p_transform_B,p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
+			return solve_concave(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B);
 		else
-			return solve_concave(p_shape_B,p_transform_B,p_motion_B,p_shape_A,p_transform_A,p_motion_A,p_result_callback,p_userdata,true,sep_axis,margin_A,margin_B);
-
-
+			return solve_concave(p_shape_B, p_transform_B, p_motion_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, sep_axis, margin_A, margin_B);
 
 	} else {
 
-
-		return collision_solver(p_shape_A, p_transform_A,p_motion_A, p_shape_B, p_transform_B, p_motion_B,p_result_callback,p_userdata,false,sep_axis,margin_A,margin_B);
+		return collision_solver(p_shape_A, p_transform_A, p_motion_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, sep_axis, margin_A, margin_B);
 	}
 
-
 	return false;
 }
-
diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h
index 8e421dec7..ce30d9351 100644
--- a/servers/physics_2d/collision_solver_2d_sw.h
+++ b/servers/physics_2d/collision_solver_2d_sw.h
@@ -33,20 +33,16 @@
 
 class CollisionSolver2DSW {
 public:
-	typedef void (*CallbackResult)(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
+	typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
+
 private:
-	static bool solve_static_line(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result);
+	static bool solve_static_line(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
 	static void concave_callback(void *p_userdata, Shape2DSW *p_convex);
-	static bool solve_concave(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
-	static bool solve_raycast(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result,Vector2 *sep_axis=NULL);
-
-
+	static bool solve_concave(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, float p_margin_A = 0, float p_margin_B = 0);
+	static bool solve_raycast(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL);
 
 public:
-
-	static bool solve(const Shape2DSW *p_shape_A,const Matrix32& p_transform_A,const Vector2& p_motion_A,const Shape2DSW *p_shape_B,const Matrix32& p_transform_B,const Vector2& p_motion_B,CallbackResult p_result_callback,void *p_userdata,Vector2 *sep_axis=NULL,float p_margin_A=0,float p_margin_B=0);
-
-
+	static bool solve(const Shape2DSW *p_shape_A, const Matrix32 &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Matrix32 &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = NULL, float p_margin_A = 0, float p_margin_B = 0);
 };
 
 #endif // COLLISION_SOLVER_2D_SW_H
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
index 9750b8724..f54170e81 100644
--- a/servers/physics_2d/constraint_2d_sw.h
+++ b/servers/physics_2d/constraint_2d_sw.h
@@ -39,32 +39,33 @@ class Constraint2DSW {
 	Constraint2DSW *island_next;
 	Constraint2DSW *island_list_next;
 
-
 	RID self;
 
 protected:
-	Constraint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; }
-public:
+	Constraint2DSW(Body2DSW **p_body_ptr = NULL, int p_body_count = 0) {
+		_body_ptr = p_body_ptr;
+		_body_count = p_body_count;
+		island_step = 0;
+	}
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
-	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
+	_FORCE_INLINE_ Constraint2DSW *get_island_next() const { return island_next; }
+	_FORCE_INLINE_ void set_island_next(Constraint2DSW *p_next) { island_next = p_next; }
 
-	_FORCE_INLINE_ Constraint2DSW* get_island_next() const { return island_next; }
-	_FORCE_INLINE_ void set_island_next(Constraint2DSW* p_next) { island_next=p_next; }
-
-	_FORCE_INLINE_ Constraint2DSW* get_island_list_next() const { return island_list_next; }
-	_FORCE_INLINE_ void set_island_list_next(Constraint2DSW* p_next) { island_list_next=p_next; }
+	_FORCE_INLINE_ Constraint2DSW *get_island_list_next() const { return island_list_next; }
+	_FORCE_INLINE_ void set_island_list_next(Constraint2DSW *p_next) { island_list_next = p_next; }
 
 	_FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
-
-	virtual bool setup(float p_step)=0;
-	virtual void solve(float p_step)=0;
+	virtual bool setup(float p_step) = 0;
+	virtual void solve(float p_step) = 0;
 
 	virtual ~Constraint2DSW() {}
 };
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 531821212..e1b8413ab 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -52,40 +52,37 @@
  * SOFTWARE.
  */
 
-static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) {
-
-
-	real_t value=0;
+static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
 
+	real_t value = 0;
 
 	{
-		value+=a->get_inv_mass();
+		value += a->get_inv_mass();
 		real_t rcn = rA.cross(n);
-		value+=a->get_inv_inertia() * rcn * rcn;
+		value += a->get_inv_inertia() * rcn * rcn;
 	}
 
 	if (b) {
 
-		value+=b->get_inv_mass();
+		value += b->get_inv_mass();
 		real_t rcn = rB.cross(n);
-		value+=b->get_inv_inertia() * rcn * rcn;
+		value += b->get_inv_inertia() * rcn * rcn;
 	}
 
 	return value;
-
 }
 
 static inline Vector2
-relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){
-	Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity();
+relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) {
+	Vector2 sum = a->get_linear_velocity() - rA.tangent() * a->get_angular_velocity();
 	if (b)
-		return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum;
+		return (b->get_linear_velocity() - rB.tangent() * b->get_angular_velocity()) - sum;
 	else
 		return -sum;
 }
 
 static inline real_t
-normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
+normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n) {
 	return relative_velocity(a, b, rA, rB).dot(n);
 }
 
@@ -188,13 +185,12 @@ PinJoint2DSW::~PinJoint2DSW() {
 
 #else
 
-
 bool PinJoint2DSW::setup(float p_step) {
 
 	Space2DSW *space = A->get_space();
-	ERR_FAIL_COND_V(!space,false;)
+	ERR_FAIL_COND_V(!space, false;)
 	rA = A->get_transform().basis_xform(anchor_A);
-	rB = B?B->get_transform().basis_xform(anchor_B):anchor_B;
+	rB = B ? B->get_transform().basis_xform(anchor_B) : anchor_B;
 #if 0
 	Vector2 gA = rA+A->get_transform().get_origin();
 	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
@@ -212,29 +208,34 @@ bool PinJoint2DSW::setup(float p_step) {
 	//      = [1/m1+1/m2     0    ] + invI1 * [rA.y*rA.y -rA.x*rA.y] + invI2 * [rA.y*rA.y -rA.x*rA.y]
 	//        [    0     1/m1+1/m2]           [-rA.x*rA.y rA.x*rA.x]           [-rA.x*rA.y rA.x*rA.x]
 
-	real_t B_inv_mass = B?B->get_inv_mass():0.0;
-
+	real_t B_inv_mass = B ? B->get_inv_mass() : 0.0;
 
 	Matrix32 K1;
-	K1[0].x = A->get_inv_mass() + B_inv_mass;	K1[1].x = 0.0f;
-	K1[0].y = 0.0f;					K1[1].y = A->get_inv_mass() + B_inv_mass;
+	K1[0].x = A->get_inv_mass() + B_inv_mass;
+	K1[1].x = 0.0f;
+	K1[0].y = 0.0f;
+	K1[1].y = A->get_inv_mass() + B_inv_mass;
 
 	Matrix32 K2;
-	K2[0].x =  A->get_inv_inertia() * rA.y * rA.y;		K2[1].x = -A->get_inv_inertia() * rA.x * rA.y;
-	K2[0].y = -A->get_inv_inertia() * rA.x * rA.y;		K2[1].y =  A->get_inv_inertia() * rA.x * rA.x;
+	K2[0].x = A->get_inv_inertia() * rA.y * rA.y;
+	K2[1].x = -A->get_inv_inertia() * rA.x * rA.y;
+	K2[0].y = -A->get_inv_inertia() * rA.x * rA.y;
+	K2[1].y = A->get_inv_inertia() * rA.x * rA.x;
 
 	Matrix32 K;
-	K[0]= K1[0] + K2[0];
-	K[1]= K1[1] + K2[1];
+	K[0] = K1[0] + K2[0];
+	K[1] = K1[1] + K2[1];
 
 	if (B) {
 
 		Matrix32 K3;
-		K3[0].x =  B->get_inv_inertia() * rB.y * rB.y;		K3[1].x = -B->get_inv_inertia() * rB.x * rB.y;
-		K3[0].y = -B->get_inv_inertia() * rB.x * rB.y;		K3[1].y =  B->get_inv_inertia() * rB.x * rB.x;
+		K3[0].x = B->get_inv_inertia() * rB.y * rB.y;
+		K3[1].x = -B->get_inv_inertia() * rB.x * rB.y;
+		K3[0].y = -B->get_inv_inertia() * rB.x * rB.y;
+		K3[1].y = B->get_inv_inertia() * rB.x * rB.x;
 
-		K[0]+=K3[0];
-		K[1]+=K3[1];
+		K[0] += K3[0];
+		K[1] += K3[1];
 	}
 
 	K[0].x += softness;
@@ -242,23 +243,22 @@ bool PinJoint2DSW::setup(float p_step) {
 
 	M = K.affine_inverse();
 
-	Vector2 gA = rA+A->get_transform().get_origin();
-	Vector2 gB = B?rB+B->get_transform().get_origin():rB;
+	Vector2 gA = rA + A->get_transform().get_origin();
+	Vector2 gB = B ? rB + B->get_transform().get_origin() : rB;
 
 	Vector2 delta = gB - gA;
 
-	bias = delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step);
+	bias = delta * -(get_bias() == 0 ? space->get_constraint_bias() : get_bias()) * (1.0 / p_step);
 
 	// apply accumulated impulse
-	A->apply_impulse(rA,-P);
+	A->apply_impulse(rA, -P);
 	if (B)
-		B->apply_impulse(rB,P);
+		B->apply_impulse(rB, P);
 
 	return true;
 }
 
-void PinJoint2DSW::solve(float p_step){
-
+void PinJoint2DSW::solve(float p_step) {
 
 	// compute relative velocity
 	Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity());
@@ -269,114 +269,110 @@ void PinJoint2DSW::solve(float p_step){
 	else
 		rel_vel = -vA;
 
-	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness,softness) * P);
+	Vector2 impulse = M.basis_xform(bias - rel_vel - Vector2(softness, softness) * P);
 
-	A->apply_impulse(rA,-impulse);
+	A->apply_impulse(rA, -impulse);
 	if (B)
-		B->apply_impulse(rB,impulse);
-
+		B->apply_impulse(rB, impulse);
 
 	P += impulse;
 }
 
 void PinJoint2DSW::set_param(Physics2DServer::PinJointParam p_param, real_t p_value) {
 
-	if(p_param == Physics2DServer::PIN_JOINT_SOFTNESS)
+	if (p_param == Physics2DServer::PIN_JOINT_SOFTNESS)
 		softness = p_value;
 }
 
 real_t PinJoint2DSW::get_param(Physics2DServer::PinJointParam p_param) const {
 
-	if(p_param == Physics2DServer::PIN_JOINT_SOFTNESS)
+	if (p_param == Physics2DServer::PIN_JOINT_SOFTNESS)
 		return softness;
 	ERR_FAIL_V(0);
 }
 
-PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) {
+PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b)
+	: Joint2DSW(_arr, p_body_b ? 2 : 1) {
 
-	A=p_body_a;
-	B=p_body_b;
+	A = p_body_a;
+	B = p_body_b;
 	anchor_A = p_body_a->get_inv_transform().xform(p_pos);
-	anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos;
+	anchor_B = p_body_b ? p_body_b->get_inv_transform().xform(p_pos) : p_pos;
 
-	softness=0;
+	softness = 0;
 
-	p_body_a->add_constraint(this,0);
+	p_body_a->add_constraint(this, 0);
 	if (p_body_b)
-		p_body_b->add_constraint(this,1);
-
+		p_body_b->add_constraint(this, 1);
 }
 
-
-
 PinJoint2DSW::~PinJoint2DSW() {
 
 	if (A)
 		A->remove_constraint(this);
 	if (B)
 		B->remove_constraint(this);
-
 }
 
-
-
 #endif
 
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 
-
 static inline void
-k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2)
-{
+k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) {
 	// calculate mass matrix
 	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
 	real_t k11, k12, k21, k22;
 	real_t m_sum = a->get_inv_mass() + b->get_inv_mass();
 
 	// start with I*m_sum
-	k11 = m_sum; k12 = 0.0f;
-	k21 = 0.0f;  k22 = m_sum;
+	k11 = m_sum;
+	k12 = 0.0f;
+	k21 = 0.0f;
+	k22 = m_sum;
 
 	// add the influence from r1
 	real_t a_i_inv = a->get_inv_inertia();
-	real_t r1xsq =  r1.x * r1.x * a_i_inv;
-	real_t r1ysq =  r1.y * r1.y * a_i_inv;
+	real_t r1xsq = r1.x * r1.x * a_i_inv;
+	real_t r1ysq = r1.y * r1.y * a_i_inv;
 	real_t r1nxy = -r1.x * r1.y * a_i_inv;
-	k11 += r1ysq; k12 += r1nxy;
-	k21 += r1nxy; k22 += r1xsq;
+	k11 += r1ysq;
+	k12 += r1nxy;
+	k21 += r1nxy;
+	k22 += r1xsq;
 
 	// add the influnce from r2
 	real_t b_i_inv = b->get_inv_inertia();
-	real_t r2xsq =  r2.x * r2.x * b_i_inv;
-	real_t r2ysq =  r2.y * r2.y * b_i_inv;
+	real_t r2xsq = r2.x * r2.x * b_i_inv;
+	real_t r2ysq = r2.y * r2.y * b_i_inv;
 	real_t r2nxy = -r2.x * r2.y * b_i_inv;
-	k11 += r2ysq; k12 += r2nxy;
-	k21 += r2nxy; k22 += r2xsq;
+	k11 += r2ysq;
+	k12 += r2nxy;
+	k21 += r2nxy;
+	k22 += r2xsq;
 
 	// invert
-	real_t determinant = k11*k22 - k12*k21;
-	ERR_FAIL_COND(determinant== 0.0);
+	real_t determinant = k11 * k22 - k12 * k21;
+	ERR_FAIL_COND(determinant == 0.0);
 
-	real_t det_inv = 1.0f/determinant;
-	*k1 = Vector2( k22*det_inv, -k12*det_inv);
-	*k2 = Vector2(-k21*det_inv,  k11*det_inv);
+	real_t det_inv = 1.0f / determinant;
+	*k1 = Vector2(k22 * det_inv, -k12 * det_inv);
+	*k2 = Vector2(-k21 * det_inv, k11 * det_inv);
 }
 
 static _FORCE_INLINE_ Vector2
-mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2)
-{
+mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
 	return Vector2(vr.dot(k1), vr.dot(k2));
 }
 
 bool GrooveJoint2DSW::setup(float p_step) {
 
-
 	// calculate endpoints in worldspace
 	Vector2 ta = A->get_transform().xform(A_groove_1);
 	Vector2 tb = A->get_transform().xform(A_groove_2);
-	Space2DSW *space=A->get_space();
+	Space2DSW *space = A->get_space();
 
 	// calculate axis
 	Vector2 n = -(tb - ta).tangent().normalized();
@@ -388,16 +384,16 @@ bool GrooveJoint2DSW::setup(float p_step) {
 	// calculate tangential distance along the axis of rB
 	real_t td = (B->get_transform().get_origin() + rB).cross(n);
 	// calculate clamping factor and rB
-	if(td <= ta.cross(n)){
+	if (td <= ta.cross(n)) {
 		clamp = 1.0f;
 		rA = ta - A->get_transform().get_origin();
-	} else if(td >= tb.cross(n)){
+	} else if (td >= tb.cross(n)) {
 		clamp = -1.0f;
 		rA = tb - A->get_transform().get_origin();
 	} else {
 		clamp = 0.0f;
 		//joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
-		rA =  ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin();
+		rA = ((-n.tangent() * -td) + n * d) - A->get_transform().get_origin();
 	}
 
 	// Calculate mass tensor
@@ -407,55 +403,52 @@ bool GrooveJoint2DSW::setup(float p_step) {
 	jn_max = get_max_force() * p_step;
 
 	// calculate bias velocity
-//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
-//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
-
+	//	cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
+	//	joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
 
-	Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA);
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
 	float _b = get_bias();
-	_b=0.001;
-	gbias=(delta*-(_b==0?space->get_constraint_bias():_b)*(1.0/p_step)).clamped(get_max_bias());
+	_b = 0.001;
+	gbias = (delta * -(_b == 0 ? space->get_constraint_bias() : _b) * (1.0 / p_step)).clamped(get_max_bias());
 
 	// apply accumulated impulse
-	A->apply_impulse(rA,-jn_acc);
-	B->apply_impulse(rB,jn_acc);
+	A->apply_impulse(rA, -jn_acc);
+	B->apply_impulse(rB, jn_acc);
 
-	correct=true;
+	correct = true;
 	return true;
 }
 
-void GrooveJoint2DSW::solve(float p_step){
-
+void GrooveJoint2DSW::solve(float p_step) {
 
 	// compute impulse
-	Vector2 vr = relative_velocity(A, B, rA,rB);
+	Vector2 vr = relative_velocity(A, B, rA, rB);
 
-	Vector2 j = mult_k(gbias-vr, k1, k2);
+	Vector2 j = mult_k(gbias - vr, k1, k2);
 	Vector2 jOld = jn_acc;
-	j+=jOld;
+	j += jOld;
 
 	jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max);
 
 	j = jn_acc - jOld;
 
-	A->apply_impulse(rA,-j);
-	B->apply_impulse(rB,j);
+	A->apply_impulse(rA, -j);
+	B->apply_impulse(rB, j);
 }
 
+GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b)
+	: Joint2DSW(_arr, 2) {
 
-GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
-
-	A=p_body_a;
-	B=p_body_b;
+	A = p_body_a;
+	B = p_body_b;
 
 	A_groove_1 = A->get_inv_transform().xform(p_a_groove1);
 	A_groove_2 = A->get_inv_transform().xform(p_a_groove2);
-	B_anchor=B->get_inv_transform().xform(p_b_anchor);
+	B_anchor = B->get_inv_transform().xform(p_b_anchor);
 	A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent();
 
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
-
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
 GrooveJoint2DSW::~GrooveJoint2DSW() {
@@ -464,38 +457,35 @@ GrooveJoint2DSW::~GrooveJoint2DSW() {
 	B->remove_constraint(this);
 }
 
-
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 
-
 bool DampedSpringJoint2DSW::setup(float p_step) {
 
 	rA = A->get_transform().basis_xform(anchor_A);
 	rB = B->get_transform().basis_xform(anchor_B);
 
-	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ;
+	Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA);
 	real_t dist = delta.length();
 
 	if (dist)
-		n=delta/dist;
+		n = delta / dist;
 	else
-		n=Vector2();
+		n = Vector2();
 
 	real_t k = k_scalar(A, B, rA, rB, n);
-	n_mass = 1.0f/k;
+	n_mass = 1.0f / k;
 
 	target_vrn = 0.0f;
-	v_coef = 1.0f - Math::exp(-damping*(p_step)*k);
+	v_coef = 1.0f - Math::exp(-damping * (p_step)*k);
 
 	// apply spring force
 	real_t f_spring = (rest_length - dist) * stiffness;
-	Vector2 j = n * f_spring*(p_step);
-
-	A->apply_impulse(rA,-j);
-	B->apply_impulse(rB,j);
+	Vector2 j = n * f_spring * (p_step);
 
+	A->apply_impulse(rA, -j);
+	B->apply_impulse(rB, j);
 
 	return true;
 }
@@ -507,38 +497,36 @@ void DampedSpringJoint2DSW::solve(float p_step) {
 
 	// compute velocity loss from drag
 	// not 100% certain this is derived correctly, though it makes sense
-	real_t v_damp = -vrn*v_coef;
+	real_t v_damp = -vrn * v_coef;
 	target_vrn = vrn + v_damp;
-	Vector2 j=n*v_damp*n_mass;
-
-	A->apply_impulse(rA,-j);
-	B->apply_impulse(rB,j);
+	Vector2 j = n * v_damp * n_mass;
 
+	A->apply_impulse(rA, -j);
+	B->apply_impulse(rB, j);
 }
 
 void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 
 		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
 
-			rest_length=p_value;
+			rest_length = p_value;
 		} break;
 		case Physics2DServer::DAMPED_STRING_DAMPING: {
 
-			damping=p_value;
+			damping = p_value;
 		} break;
 		case Physics2DServer::DAMPED_STRING_STIFFNESS: {
 
-			stiffness=p_value;
+			stiffness = p_value;
 		} break;
 	}
-
 }
 
-real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{
+real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 
 		case Physics2DServer::DAMPED_STRING_REST_LENGTH: {
 
@@ -557,30 +545,24 @@ real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_par
 	ERR_FAIL_V(0);
 }
 
+DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b)
+	: Joint2DSW(_arr, 2) {
 
-DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) {
-
-
-	A=p_body_a;
-	B=p_body_b;
+	A = p_body_a;
+	B = p_body_b;
 	anchor_A = A->get_inv_transform().xform(p_anchor_a);
 	anchor_B = B->get_inv_transform().xform(p_anchor_b);
 
-	rest_length=p_anchor_a.distance_to(p_anchor_b);
-	stiffness=20;
-	damping=1.5;
-
-
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
+	rest_length = p_anchor_a.distance_to(p_anchor_b);
+	stiffness = 20;
+	damping = 1.5;
 
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
 }
 
 DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
 
 	A->remove_constraint(this);
 	B->remove_constraint(this);
-
 }
-
-
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index cbbb6e6db..9feb496f4 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -29,30 +29,31 @@
 #ifndef JOINTS_2D_SW_H
 #define JOINTS_2D_SW_H
 
-#include "constraint_2d_sw.h"
 #include "body_2d_sw.h"
-
-
+#include "constraint_2d_sw.h"
 
 class Joint2DSW : public Constraint2DSW {
 
 	real_t max_force;
 	real_t bias;
 	real_t max_bias;
-public:
 
-	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
+public:
+	_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force = p_force; }
 	_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
 
-	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
+	_FORCE_INLINE_ void set_bias(real_t p_bias) { bias = p_bias; }
 	_FORCE_INLINE_ real_t get_bias() const { return bias; }
 
-	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
+	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
 
-	virtual Physics2DServer::JointType get_type() const=0;
-	Joint2DSW(Body2DSW **p_body_ptr=NULL,int p_body_count=0) : Constraint2DSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
-
+	virtual Physics2DServer::JointType get_type() const = 0;
+	Joint2DSW(Body2DSW **p_body_ptr = NULL, int p_body_count = 0)
+		: Constraint2DSW(p_body_ptr, p_body_count) {
+		bias = 0;
+		max_force = max_bias = 3.40282e+38;
+	};
 };
 #if 0
 
@@ -107,7 +108,7 @@ class PinJoint2DSW : public Joint2DSW {
 	};
 
 	Matrix32 M;
-	Vector2 rA,rB;
+	Vector2 rA, rB;
 	Vector2 anchor_A;
 	Vector2 anchor_B;
 	Vector2 bias;
@@ -115,7 +116,6 @@ class PinJoint2DSW : public Joint2DSW {
 	real_t softness;
 
 public:
-
 	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_PIN; }
 
 	virtual bool setup(float p_step);
@@ -124,11 +124,10 @@ public:
 	void set_param(Physics2DServer::PinJointParam p_param, real_t p_value);
 	real_t get_param(Physics2DServer::PinJointParam p_param) const;
 
-	PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b=NULL);
+	PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = NULL);
 	~PinJoint2DSW();
 };
 
-
 #endif
 class GrooveJoint2DSW : public Joint2DSW {
 
@@ -150,25 +149,21 @@ class GrooveJoint2DSW : public Joint2DSW {
 	real_t jn_max;
 	real_t clamp;
 	Vector2 xf_normal;
-	Vector2 rA,rB;
-	Vector2 k1,k2;
-
+	Vector2 rA, rB;
+	Vector2 k1, k2;
 
 	bool correct;
 
 public:
-
 	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_GROOVE; }
 
 	virtual bool setup(float p_step);
 	virtual void solve(float p_step);
 
-
-	GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b);
 	~GrooveJoint2DSW();
 };
 
-
 class DampedSpringJoint2DSW : public Joint2DSW {
 
 	union {
@@ -180,7 +175,6 @@ class DampedSpringJoint2DSW : public Joint2DSW {
 		Body2DSW *_arr[2];
 	};
 
-
 	Vector2 anchor_A;
 	Vector2 anchor_B;
 
@@ -188,14 +182,13 @@ class DampedSpringJoint2DSW : public Joint2DSW {
 	real_t damping;
 	real_t stiffness;
 
-	Vector2 rA,rB;
+	Vector2 rA, rB;
 	Vector2 n;
 	real_t n_mass;
 	real_t target_vrn;
 	real_t v_coef;
 
 public:
-
 	virtual Physics2DServer::JointType get_type() const { return Physics2DServer::JOINT_DAMPED_SPRING; }
 
 	virtual bool setup(float p_step);
@@ -204,9 +197,8 @@ public:
 	void set_param(Physics2DServer::DampedStringParam p_param, real_t p_value);
 	real_t get_param(Physics2DServer::DampedStringParam p_param) const;
 
-	DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b);
+	DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b);
 	~DampedSpringJoint2DSW();
 };
 
-
 #endif // JOINTS_2D_SW_H
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index baa238156..c2fd1ec4c 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -31,52 +31,51 @@
 #include "broad_phase_2d_hash_grid.h"
 #include "collision_solver_2d_sw.h"
 #include "globals.h"
-#include "script_language.h"
 #include "os/os.h"
+#include "script_language.h"
 
 RID Physics2DServerSW::shape_create(ShapeType p_shape) {
 
-	Shape2DSW *shape=NULL;
-	switch(p_shape) {
+	Shape2DSW *shape = NULL;
+	switch (p_shape) {
 
 		case SHAPE_LINE: {
 
-			shape=memnew( LineShape2DSW );
+			shape = memnew(LineShape2DSW);
 		} break;
 		case SHAPE_RAY: {
 
-			shape=memnew( RayShape2DSW );
+			shape = memnew(RayShape2DSW);
 		} break;
 		case SHAPE_SEGMENT: {
 
-			shape=memnew( SegmentShape2DSW);
+			shape = memnew(SegmentShape2DSW);
 		} break;
 		case SHAPE_CIRCLE: {
 
-			shape=memnew( CircleShape2DSW);
+			shape = memnew(CircleShape2DSW);
 		} break;
 		case SHAPE_RECTANGLE: {
 
-			shape=memnew( RectangleShape2DSW);
+			shape = memnew(RectangleShape2DSW);
 		} break;
 		case SHAPE_CAPSULE: {
 
-			shape=memnew( CapsuleShape2DSW );
+			shape = memnew(CapsuleShape2DSW);
 		} break;
 		case SHAPE_CONVEX_POLYGON: {
 
-			shape=memnew( ConvexPolygonShape2DSW );
+			shape = memnew(ConvexPolygonShape2DSW);
 		} break;
 		case SHAPE_CONCAVE_POLYGON: {
 
-			shape=memnew( ConcavePolygonShape2DSW );
+			shape = memnew(ConcavePolygonShape2DSW);
 		} break;
 		case SHAPE_CUSTOM: {
 
 			ERR_FAIL_V(RID());
 
 		} break;
-
 	}
 
 	RID id = shape_owner.make_rid(shape);
@@ -85,68 +84,55 @@ RID Physics2DServerSW::shape_create(ShapeType p_shape) {
 	return id;
 };
 
-
-
-
-void Physics2DServerSW::shape_set_data(RID p_shape, const Variant& p_data) {
+void Physics2DServerSW::shape_set_data(RID p_shape, const Variant &p_data) {
 
 	Shape2DSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 	shape->set_data(p_data);
-
-
 };
 
-
 void Physics2DServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
 
 	Shape2DSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 	shape->set_custom_bias(p_bias);
-
 }
 
-
 Physics2DServer::ShapeType Physics2DServerSW::shape_get_type(RID p_shape) const {
 
 	const Shape2DSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM);
+	ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM);
 	return shape->get_type();
-
 };
 
 Variant Physics2DServerSW::shape_get_data(RID p_shape) const {
 
 	const Shape2DSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,Variant());
-	ERR_FAIL_COND_V(!shape->is_configured(),Variant());
+	ERR_FAIL_COND_V(!shape, Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(), Variant());
 	return shape->get_data();
-
 };
 
 real_t Physics2DServerSW::shape_get_custom_solver_bias(RID p_shape) const {
 
 	const Shape2DSW *shape = shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ERR_FAIL_COND_V(!shape, 0);
 	return shape->get_custom_bias();
-
 }
 
+void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
 
-void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
-
+	CollCbkData *cbk = (CollCbkData *)p_userdata;
 
-	CollCbkData *cbk=(CollCbkData *)p_userdata;
-
-	if (cbk->max==0)
+	if (cbk->max == 0)
 		return;
 
-	if (cbk->valid_dir!=Vector2()) {
-		if (p_point_A.distance_squared_to(p_point_B)>cbk->valid_depth*cbk->valid_depth) {
+	if (cbk->valid_dir != Vector2()) {
+		if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) {
 			return;
 		}
-		if (cbk->valid_dir.dot((p_point_A-p_point_B).normalized())<0.7071) {
-/*			print_line("A: "+p_point_A);
+		if (cbk->valid_dir.dot((p_point_A - p_point_B).normalized()) < 0.7071) {
+			/*			print_line("A: "+p_point_A);
 			print_line("B: "+p_point_B);
 			print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B))));
 			print_line("resnorm: "+(p_point_A-p_point_B).normalized());
@@ -158,66 +144,61 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2& p_point_A,const Vector2& p
 
 	if (cbk->amount == cbk->max) {
 		//find least deep
-		float min_depth=1e20;
-		int min_depth_idx=0;
-		for(int i=0;i<cbk->amount;i++) {
+		float min_depth = 1e20;
+		int min_depth_idx = 0;
+		for (int i = 0; i < cbk->amount; i++) {
 
-			float d = cbk->ptr[i*2+0].distance_squared_to(cbk->ptr[i*2+1]);
-			if (d<min_depth) {
-				min_depth=d;
-				min_depth_idx=i;
+			float d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]);
+			if (d < min_depth) {
+				min_depth = d;
+				min_depth_idx = i;
 			}
-
 		}
 
 		float d = p_point_A.distance_squared_to(p_point_B);
-		if (d<min_depth)
+		if (d < min_depth)
 			return;
-		cbk->ptr[min_depth_idx*2+0]=p_point_A;
-		cbk->ptr[min_depth_idx*2+1]=p_point_B;
-
+		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
+		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
 
 	} else {
 
-		cbk->ptr[cbk->amount*2+0]=p_point_A;
-		cbk->ptr[cbk->amount*2+1]=p_point_B;
+		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
+		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
 		cbk->amount++;
 	}
 }
 
-bool Physics2DServerSW::shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) {
-
+bool Physics2DServerSW::shape_collide(RID p_shape_A, const Matrix32 &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Matrix32 &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) {
 
 	Shape2DSW *shape_A = shape_owner.get(p_shape_A);
-	ERR_FAIL_COND_V(!shape_A,false);
+	ERR_FAIL_COND_V(!shape_A, false);
 	Shape2DSW *shape_B = shape_owner.get(p_shape_B);
-	ERR_FAIL_COND_V(!shape_B,false);
+	ERR_FAIL_COND_V(!shape_B, false);
 
-	if (p_result_max==0) {
+	if (p_result_max == 0) {
 
-		return CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,NULL,NULL);
+		return CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, NULL, NULL);
 	}
 
 	CollCbkData cbk;
-	cbk.max=p_result_max;
-	cbk.amount=0;
-	cbk.ptr=r_results;
+	cbk.max = p_result_max;
+	cbk.amount = 0;
+	cbk.ptr = r_results;
 
-	bool res= CollisionSolver2DSW::solve(shape_A,p_xform_A,p_motion_A,shape_B,p_xform_B,p_motion_B,_shape_col_cbk,&cbk);
-	r_result_count=cbk.amount;
+	bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk);
+	r_result_count = cbk.amount;
 	return res;
-
 }
 
-
 RID Physics2DServerSW::space_create() {
 
-	Space2DSW *space = memnew( Space2DSW );
+	Space2DSW *space = memnew(Space2DSW);
 	RID id = space_owner.make_rid(space);
 	space->set_self(id);
 	RID area_id = area_create();
 	Area2DSW *area = area_owner.get(area_id);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 	space->set_default_area(area);
 	area->set_space(space);
 	area->set_priority(-1);
@@ -225,7 +206,7 @@ RID Physics2DServerSW::space_create() {
 	return id;
 };
 
-void Physics2DServerSW::space_set_active(RID p_space,bool p_active) {
+void Physics2DServerSW::space_set_active(RID p_space, bool p_active) {
 
 	Space2DSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
@@ -238,56 +219,51 @@ void Physics2DServerSW::space_set_active(RID p_space,bool p_active) {
 bool Physics2DServerSW::space_is_active(RID p_space) const {
 
 	const Space2DSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,false);
+	ERR_FAIL_COND_V(!space, false);
 
 	return active_spaces.has(space);
-
 }
 
-void Physics2DServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) {
+void Physics2DServerSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
 
 	Space2DSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
 
-	space->set_param(p_param,p_value);
-
+	space->set_param(p_param, p_value);
 }
 
-real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) const {
+real_t Physics2DServerSW::space_get_param(RID p_space, SpaceParameter p_param) const {
 
 	const Space2DSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,0);
+	ERR_FAIL_COND_V(!space, 0);
 	return space->get_param(p_param);
 }
 
-void Physics2DServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) {
+void Physics2DServerSW::space_set_debug_contacts(RID p_space, int p_max_contacts) {
 
 	Space2DSW *space = space_owner.get(p_space);
 	ERR_FAIL_COND(!space);
 	space->set_debug_contacts(p_max_contacts);
-
 }
 
 Vector<Vector2> Physics2DServerSW::space_get_contacts(RID p_space) const {
 
 	Space2DSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,Vector<Vector2>());
+	ERR_FAIL_COND_V(!space, Vector<Vector2>());
 	return space->get_debug_contacts();
-
 }
 
 int Physics2DServerSW::space_get_contact_count(RID p_space) const {
 
 	Space2DSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,0);
+	ERR_FAIL_COND_V(!space, 0);
 	return space->get_debug_contact_count();
-
 }
 
-Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) {
+Physics2DDirectSpaceState *Physics2DServerSW::space_get_direct_state(RID p_space) {
 
 	Space2DSW *space = space_owner.get(p_space);
-	ERR_FAIL_COND_V(!space,NULL);
+	ERR_FAIL_COND_V(!space, NULL);
 	if ((using_threads && !doing_sync) || space->is_locked()) {
 
 		ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification.");
@@ -299,7 +275,7 @@ Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space
 
 RID Physics2DServerSW::area_create() {
 
-	Area2DSW *area = memnew( Area2DSW );
+	Area2DSW *area = memnew(Area2DSW);
 	RID rid = area_owner.make_rid(area);
 	area->set_self(rid);
 	return rid;
@@ -309,20 +285,19 @@ void Physics2DServerSW::area_set_space(RID p_area, RID p_space) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	Space2DSW *space=NULL;
+	Space2DSW *space = NULL;
 	if (p_space.is_valid()) {
 		space = space_owner.get(p_space);
 		ERR_FAIL_COND(!space);
 	}
 
 	area->set_space(space);
-
 };
 
 RID Physics2DServerSW::area_get_space(RID p_area) const {
 
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 
 	Space2DSW *space = area->get_space();
 	if (!space)
@@ -332,7 +307,6 @@ RID Physics2DServerSW::area_get_space(RID p_area) const {
 
 void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
 
-
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
@@ -342,13 +316,12 @@ void Physics2DServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverri
 Physics2DServer::AreaSpaceOverrideMode Physics2DServerSW::area_get_space_override_mode(RID p_area) const {
 
 	const Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED);
+	ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED);
 
 	return area->get_space_override_mode();
 }
 
-
-void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform) {
+void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Matrix32 &p_transform) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -356,11 +329,10 @@ void Physics2DServerSW::area_add_shape(RID p_area, RID p_shape, const Matrix32&
 	Shape2DSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 
-	area->add_shape(shape,p_transform);
-
+	area->add_shape(shape, p_transform);
 }
 
-void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) {
+void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -369,39 +341,37 @@ void Physics2DServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape)
 	ERR_FAIL_COND(!shape);
 	ERR_FAIL_COND(!shape->is_configured());
 
-	area->set_shape(p_shape_idx,shape);
-
+	area->set_shape(p_shape_idx, shape);
 }
-void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform) {
+void Physics2DServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32 &p_transform) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_shape_transform(p_shape_idx,p_transform);
+	area->set_shape_transform(p_shape_idx, p_transform);
 }
 
 int Physics2DServerSW::area_get_shape_count(RID p_area) const {
 
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,-1);
+	ERR_FAIL_COND_V(!area, -1);
 
 	return area->get_shape_count();
-
 }
 RID Physics2DServerSW::area_get_shape(RID p_area, int p_shape_idx) const {
 
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,RID());
+	ERR_FAIL_COND_V(!area, RID());
 
 	Shape2DSW *shape = area->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape,RID());
+	ERR_FAIL_COND_V(!shape, RID());
 
 	return shape->get_self();
 }
 Matrix32 Physics2DServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const {
 
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Matrix32());
+	ERR_FAIL_COND_V(!area, Matrix32());
 
 	return area->get_shape_transform(p_shape_idx);
 }
@@ -419,65 +389,57 @@ void Physics2DServerSW::area_clear_shapes(RID p_area) {
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	while(area->get_shape_count())
+	while (area->get_shape_count())
 		area->remove_shape(0);
-
 }
 
-void Physics2DServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) {
+void Physics2DServerSW::area_attach_object_instance_ID(RID p_area, ObjectID p_ID) {
 
 	if (space_owner.owns(p_area)) {
-		Space2DSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		Space2DSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 	area->set_instance_id(p_ID);
-
 }
 ObjectID Physics2DServerSW::area_get_object_instance_ID(RID p_area) const {
 
 	if (space_owner.owns(p_area)) {
-		Space2DSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		Space2DSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,0);
+	ERR_FAIL_COND_V(!area, 0);
 	return area->get_instance_id();
-
-
 }
 
-
-void Physics2DServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) {
+void Physics2DServerSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
 
 	if (space_owner.owns(p_area)) {
-		Space2DSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		Space2DSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	area->set_param(p_param,p_value);
-
+	area->set_param(p_param, p_value);
 };
 
-
-void Physics2DServerSW::area_set_transform(RID p_area, const Matrix32& p_transform) {
+void Physics2DServerSW::area_set_transform(RID p_area, const Matrix32 &p_transform) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 	area->set_transform(p_transform);
-
 };
 
-Variant Physics2DServerSW::area_get_param(RID p_area,AreaParameter p_param) const {
+Variant Physics2DServerSW::area_get_param(RID p_area, AreaParameter p_param) const {
 
 	if (space_owner.owns(p_area)) {
-		Space2DSW *space=space_owner.get(p_area);
-		p_area=space->get_default_area()->get_self();
+		Space2DSW *space = space_owner.get(p_area);
+		p_area = space->get_default_area()->get_self();
 	}
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Variant());
+	ERR_FAIL_COND_V(!area, Variant());
 
 	return area->get_param(p_param);
 };
@@ -485,29 +447,27 @@ Variant Physics2DServerSW::area_get_param(RID p_area,AreaParameter p_param) cons
 Matrix32 Physics2DServerSW::area_get_transform(RID p_area) const {
 
 	Area2DSW *area = area_owner.get(p_area);
-	ERR_FAIL_COND_V(!area,Matrix32());
+	ERR_FAIL_COND_V(!area, Matrix32());
 
 	return area->get_transform();
 };
 
-void Physics2DServerSW::area_set_pickable(RID p_area,bool p_pickable) {
+void Physics2DServerSW::area_set_pickable(RID p_area, bool p_pickable) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 	area->set_pickable(p_pickable);
-
 }
 
-void Physics2DServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
+void Physics2DServerSW::area_set_monitorable(RID p_area, bool p_monitorable) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
 	area->set_monitorable(p_monitorable);
-
 }
 
-void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) {
+void Physics2DServerSW::area_set_collision_mask(RID p_area, uint32_t p_mask) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -515,7 +475,7 @@ void Physics2DServerSW::area_set_collision_mask(RID p_area,uint32_t p_mask) {
 	area->set_collision_mask(p_mask);
 }
 
-void Physics2DServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) {
+void Physics2DServerSW::area_set_layer_mask(RID p_area, uint32_t p_mask) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
@@ -523,59 +483,53 @@ void Physics2DServerSW::area_set_layer_mask(RID p_area,uint32_t p_mask) {
 	area->set_layer_mask(p_mask);
 }
 
-
-void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+void Physics2DServerSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
-
-
+	area->set_monitor_callback(p_receiver ? p_receiver->get_instance_ID() : 0, p_method);
 }
 
-void Physics2DServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
-
+void Physics2DServerSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
 
 	Area2DSW *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
 
-	area->set_area_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
+	area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_ID() : 0, p_method);
 }
 
 /* BODY API */
 
-RID Physics2DServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) {
+RID Physics2DServerSW::body_create(BodyMode p_mode, bool p_init_sleeping) {
 
-	Body2DSW *body = memnew( Body2DSW );
-	if (p_mode!=BODY_MODE_RIGID)
+	Body2DSW *body = memnew(Body2DSW);
+	if (p_mode != BODY_MODE_RIGID)
 		body->set_mode(p_mode);
 	if (p_init_sleeping)
-		body->set_state(BODY_STATE_SLEEPING,p_init_sleeping);
+		body->set_state(BODY_STATE_SLEEPING, p_init_sleeping);
 	RID rid = body_owner.make_rid(body);
 	body->set_self(rid);
 	return rid;
 };
 
-
 void Physics2DServerSW::body_set_space(RID p_body, RID p_space) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	Space2DSW *space=NULL;
+	Space2DSW *space = NULL;
 	if (p_space.is_valid()) {
 		space = space_owner.get(p_space);
 		ERR_FAIL_COND(!space);
 	}
 
 	body->set_space(space);
-
 };
 
 RID Physics2DServerSW::body_get_space(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,RID());
+	ERR_FAIL_COND_V(!body, RID());
 
 	Space2DSW *space = body->get_space();
 	if (!space)
@@ -583,7 +537,6 @@ RID Physics2DServerSW::body_get_space(RID p_body) const {
 	return space->get_self();
 };
 
-
 void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
 
 	Body2DSW *body = body_owner.get(p_body);
@@ -595,12 +548,12 @@ void Physics2DServerSW::body_set_mode(RID p_body, BodyMode p_mode) {
 Physics2DServer::BodyMode Physics2DServerSW::body_get_mode(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,BODY_MODE_STATIC);
+	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
 
 	return body->get_mode();
 };
 
-void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform) {
+void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Matrix32 &p_transform) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -608,11 +561,10 @@ void Physics2DServerSW::body_add_shape(RID p_body, RID p_shape, const Matrix32&
 	Shape2DSW *shape = shape_owner.get(p_shape);
 	ERR_FAIL_COND(!shape);
 
-	body->add_shape(shape,p_transform);
-
+	body->add_shape(shape, p_transform);
 }
 
-void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) {
+void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -621,55 +573,51 @@ void Physics2DServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape)
 	ERR_FAIL_COND(!shape);
 	ERR_FAIL_COND(!shape->is_configured());
 
-	body->set_shape(p_shape_idx,shape);
-
+	body->set_shape(p_shape_idx, shape);
 }
-void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform) {
+void Physics2DServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32 &p_transform) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_shape_transform(p_shape_idx,p_transform);
+	body->set_shape_transform(p_shape_idx, p_transform);
 }
 
-void Physics2DServerSW::body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata) {
+void Physics2DServerSW::body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	body->set_shape_metadata(p_shape_idx,p_metadata);
+	body->set_shape_metadata(p_shape_idx, p_metadata);
 }
 
-
 Variant Physics2DServerSW::body_get_shape_metadata(RID p_body, int p_shape_idx) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Variant());
+	ERR_FAIL_COND_V(!body, Variant());
 	return body->get_shape_metadata(p_shape_idx);
 }
 
-
 int Physics2DServerSW::body_get_shape_count(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,-1);
+	ERR_FAIL_COND_V(!body, -1);
 
 	return body->get_shape_count();
-
 }
 RID Physics2DServerSW::body_get_shape(RID p_body, int p_shape_idx) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,RID());
+	ERR_FAIL_COND_V(!body, RID());
 
 	Shape2DSW *shape = body->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape,RID());
+	ERR_FAIL_COND_V(!shape, RID());
 
 	return shape->get_self();
 }
 Matrix32 Physics2DServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Matrix32());
+	ERR_FAIL_COND_V(!body, Matrix32());
 
 	return body->get_shape_transform(p_shape_idx);
 }
@@ -687,67 +635,57 @@ void Physics2DServerSW::body_clear_shapes(RID p_body) {
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	while(body->get_shape_count())
+	while (body->get_shape_count())
 		body->remove_shape(0);
-
 }
 
-
-void Physics2DServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) {
+void Physics2DServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	ERR_FAIL_INDEX(p_shape_idx,body->get_shape_count());
-
-	body->set_shape_as_trigger(p_shape_idx,p_enable);
+	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
 
+	body->set_shape_as_trigger(p_shape_idx, p_enable);
 }
 
 bool Physics2DServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const {
 
 	const Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body, false);
 
-	ERR_FAIL_INDEX_V(p_shape_idx,body->get_shape_count(),false);
+	ERR_FAIL_INDEX_V(p_shape_idx, body->get_shape_count(), false);
 
 	return body->is_shape_set_as_trigger(p_shape_idx);
-
 }
 
-
-void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode) {
+void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_continuous_collision_detection_mode(p_mode);
-
 }
 
-Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const{
+Physics2DServerSW::CCDMode Physics2DServerSW::body_get_continuous_collision_detection_mode(RID p_body) const {
 
 	const Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,CCD_MODE_DISABLED);
+	ERR_FAIL_COND_V(!body, CCD_MODE_DISABLED);
 
 	return body->get_continuous_collision_detection_mode();
-
 }
 
-
-
-void Physics2DServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) {
+void Physics2DServerSW::body_attach_object_instance_ID(RID p_body, uint32_t p_ID) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_instance_id(p_ID);
-
 };
 
 uint32_t Physics2DServerSW::body_get_object_instance_ID(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_instance_id();
 };
@@ -757,30 +695,27 @@ void Physics2DServerSW::body_set_layer_mask(RID p_body, uint32_t p_flags) {
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_layer_mask(p_flags);
-
 };
 
 uint32_t Physics2DServerSW::body_get_layer_mask(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_layer_mask();
 };
 
-
 void Physics2DServerSW::body_set_collision_mask(RID p_body, uint32_t p_flags) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_collision_mask(p_flags);
-
 };
 
 uint32_t Physics2DServerSW::body_get_collision_mask(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_collision_mask();
 };
@@ -790,50 +725,46 @@ void Physics2DServerSW::body_set_param(RID p_body, BodyParameter p_param, float
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_param(p_param,p_value);
+	body->set_param(p_param, p_value);
 };
 
 float Physics2DServerSW::body_get_param(RID p_body, BodyParameter p_param) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
 };
 
-
-
-void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) {
+void Physics2DServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_state(p_state,p_variant);
+	body->set_state(p_state, p_variant);
 };
 
 Variant Physics2DServerSW::body_get_state(RID p_body, BodyState p_state) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Variant());
+	ERR_FAIL_COND_V(!body, Variant());
 
 	return body->get_state(p_state);
 };
 
-
-void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2& p_force) {
+void Physics2DServerSW::body_set_applied_force(RID p_body, const Vector2 &p_force) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_applied_force(p_force);
 	body->wakeup();
-
 };
 
 Vector2 Physics2DServerSW::body_get_applied_force(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Vector2());
+	ERR_FAIL_COND_V(!body, Vector2());
 	return body->get_applied_force();
 };
 
@@ -849,38 +780,38 @@ void Physics2DServerSW::body_set_applied_torque(RID p_body, float p_torque) {
 float Physics2DServerSW::body_get_applied_torque(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_applied_torque();
 };
 
-void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse) {
+void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, const Vector2 &p_impulse) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->apply_impulse(p_pos,p_impulse);
+	body->apply_impulse(p_pos, p_impulse);
 	body->wakeup();
 };
 
-void Physics2DServerSW::body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force) {
+void Physics2DServerSW::body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->add_force(p_force,p_offset);
+	body->add_force(p_force, p_offset);
 	body->wakeup();
 };
 
-void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity) {
+void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	Vector2 v = body->get_linear_velocity();
 	Vector2 axis = p_axis_velocity.normalized();
-	v-=axis*axis.dot(v);
-	v+=p_axis_velocity;
+	v -= axis * axis.dot(v);
+	v += p_axis_velocity;
 	body->set_linear_velocity(v);
 	body->wakeup();
 };
@@ -901,7 +832,6 @@ void Physics2DServerSW::body_remove_collision_exception(RID p_body, RID p_body_b
 
 	body->remove_exception(p_body_b);
 	body->wakeup();
-
 };
 
 void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
@@ -909,27 +839,25 @@ void Physics2DServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_e
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	for(int i=0;i<body->get_exceptions().size();i++) {
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
 		p_exceptions->push_back(body->get_exceptions()[i]);
 	}
-
 };
 
 void Physics2DServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-
 };
 
 float Physics2DServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 	return 0;
 };
 
-void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) {
+void Physics2DServerSW::body_set_omit_force_integration(RID p_body, bool p_omit) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -940,7 +868,7 @@ void Physics2DServerSW::body_set_omit_force_integration(RID p_body,bool p_omit)
 bool Physics2DServerSW::body_is_omitting_force_integration(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
+	ERR_FAIL_COND_V(!body, false);
 	return body->get_omit_force_integration();
 };
 
@@ -954,93 +882,83 @@ void Physics2DServerSW::body_set_max_contacts_reported(RID p_body, int p_contact
 int Physics2DServerSW::body_get_max_contacts_reported(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,-1);
+	ERR_FAIL_COND_V(!body, -1);
 	return body->get_max_contacts_reported();
 }
 
-void Physics2DServerSW::body_set_one_way_collision_direction(RID p_body,const Vector2& p_direction) {
+void Physics2DServerSW::body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_one_way_collision_direction(p_direction);
 }
 
-Vector2 Physics2DServerSW::body_get_one_way_collision_direction(RID p_body) const{
+Vector2 Physics2DServerSW::body_get_one_way_collision_direction(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,Vector2());
+	ERR_FAIL_COND_V(!body, Vector2());
 	return body->get_one_way_collision_direction();
-
 }
 
-void Physics2DServerSW::body_set_one_way_collision_max_depth(RID p_body,float p_max_depth) {
+void Physics2DServerSW::body_set_one_way_collision_max_depth(RID p_body, float p_max_depth) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_one_way_collision_max_depth(p_max_depth);
-
 }
 
 float Physics2DServerSW::body_get_one_way_collision_max_depth(RID p_body) const {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,0);
+	ERR_FAIL_COND_V(!body, 0);
 	return body->get_one_way_collision_max_depth();
-
 }
 
-void Physics2DServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) {
-
+void Physics2DServerSW::body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-	body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata);
-
+	body->set_force_integration_callback(p_receiver ? p_receiver->get_instance_ID() : ObjectID(0), p_method, p_udata);
 }
 
-bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) {
+bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
-	ERR_FAIL_INDEX_V(p_body_shape,body->get_shape_count(),false);
-
-	return shape_collide(body->get_shape(p_body_shape)->get_self(),body->get_transform() * body->get_shape_transform(p_body_shape),Vector2(),p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_INDEX_V(p_body_shape, body->get_shape_count(), false);
 
+	return shape_collide(body->get_shape(p_body_shape)->get_self(), body->get_transform() * body->get_shape_transform(p_body_shape), Vector2(), p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count);
 }
 
-void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) {
+void Physics2DServerSW::body_set_pickable(RID p_body, bool p_pickable) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 	body->set_pickable(p_pickable);
-
 }
 
-bool Physics2DServerSW::body_test_motion(RID p_body, const Vector2& p_motion, float p_margin, MotionResult *r_result) {
+bool Physics2DServerSW::body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin, MotionResult *r_result) {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
-	ERR_FAIL_COND_V(!body->get_space(),false);
-	ERR_FAIL_COND_V(body->get_space()->is_locked(),false);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
 
 	// Since this is the old-style, broken version, the transform to be used
 	// is that the physics server is aware of
-	return body->get_space()->test_body_motion(body,body->get_transform(),p_motion,p_margin,r_result);
-
+	return body->get_space()->test_body_motion(body, body->get_transform(), p_motion, p_margin, r_result);
 }
 
-bool Physics2DServerSW::body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin, MotionResult *r_result) {
+bool Physics2DServerSW::body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin, MotionResult *r_result) {
 
 	Body2DSW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body,false);
-	ERR_FAIL_COND_V(!body->get_space(),false);
-	ERR_FAIL_COND_V(body->get_space()->is_locked(),false);
-
-	return body->get_space()->test_body_motion(body,p_from,p_motion,p_margin,r_result);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
 
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
 }
 
-
 /* JOINT API */
 
 void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
@@ -1048,21 +966,19 @@ void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t
 	Joint2DSW *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!joint);
 
-	switch(p_param) {
+	switch (p_param) {
 		case JOINT_PARAM_BIAS: joint->set_bias(p_value); break;
 		case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break;
 		case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break;
 	}
-
-
 }
 
-real_t Physics2DServerSW::joint_get_param(RID p_joint,JointParam p_param) const {
+real_t Physics2DServerSW::joint_get_param(RID p_joint, JointParam p_param) const {
 
 	const Joint2DSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,-1);
+	ERR_FAIL_COND_V(!joint, -1);
 
-	switch(p_param) {
+	switch (p_param) {
 		case JOINT_PARAM_BIAS: return joint->get_bias(); break;
 		case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break;
 		case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break;
@@ -1071,115 +987,106 @@ real_t Physics2DServerSW::joint_get_param(RID p_joint,JointParam p_param) const
 	return 0;
 }
 
+RID Physics2DServerSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b) {
 
-RID Physics2DServerSW::pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b) {
-
-	Body2DSW *A=body_owner.get(p_body_a);
-	ERR_FAIL_COND_V(!A,RID());
-	Body2DSW *B=NULL;
+	Body2DSW *A = body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A, RID());
+	Body2DSW *B = NULL;
 	if (body_owner.owns(p_body_b)) {
-		B=body_owner.get(p_body_b);
-		ERR_FAIL_COND_V(!B,RID());
+		B = body_owner.get(p_body_b);
+		ERR_FAIL_COND_V(!B, RID());
 	}
 
-	Joint2DSW *joint = memnew( PinJoint2DSW(p_pos,A,B) );
+	Joint2DSW *joint = memnew(PinJoint2DSW(p_pos, A, B));
 	RID self = joint_owner.make_rid(joint);
 	joint->set_self(self);
 
 	return self;
 }
 
-RID Physics2DServerSW::groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b) {
-
+RID Physics2DServerSW::groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) {
 
-	Body2DSW *A=body_owner.get(p_body_a);
-	ERR_FAIL_COND_V(!A,RID());
+	Body2DSW *A = body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A, RID());
 
-	Body2DSW *B=body_owner.get(p_body_b);
-	ERR_FAIL_COND_V(!B,RID());
+	Body2DSW *B = body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B, RID());
 
-	Joint2DSW *joint = memnew( GrooveJoint2DSW(p_a_groove1,p_a_groove2,p_b_anchor,A,B) );
+	Joint2DSW *joint = memnew(GrooveJoint2DSW(p_a_groove1, p_a_groove2, p_b_anchor, A, B));
 	RID self = joint_owner.make_rid(joint);
 	joint->set_self(self);
 	return self;
-
-
 }
 
-RID Physics2DServerSW::damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b) {
+RID Physics2DServerSW::damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) {
 
-	Body2DSW *A=body_owner.get(p_body_a);
-	ERR_FAIL_COND_V(!A,RID());
+	Body2DSW *A = body_owner.get(p_body_a);
+	ERR_FAIL_COND_V(!A, RID());
 
-	Body2DSW *B=body_owner.get(p_body_b);
-	ERR_FAIL_COND_V(!B,RID());
+	Body2DSW *B = body_owner.get(p_body_b);
+	ERR_FAIL_COND_V(!B, RID());
 
-	Joint2DSW *joint = memnew( DampedSpringJoint2DSW(p_anchor_a,p_anchor_b,A,B) );
+	Joint2DSW *joint = memnew(DampedSpringJoint2DSW(p_anchor_a, p_anchor_b, A, B));
 	RID self = joint_owner.make_rid(joint);
 	joint->set_self(self);
 	return self;
-
 }
 
 void Physics2DServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
 
 	Joint2DSW *j = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!j);
-	ERR_FAIL_COND(j->get_type()!=JOINT_PIN);
+	ERR_FAIL_COND(j->get_type() != JOINT_PIN);
 
-	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW*>(j);
+	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j);
 	pin_joint->set_param(p_param, p_value);
 }
 
 real_t Physics2DServerSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
 	Joint2DSW *j = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!j,0);
-	ERR_FAIL_COND_V(j->get_type()!=JOINT_PIN,0);
+	ERR_FAIL_COND_V(!j, 0);
+	ERR_FAIL_COND_V(j->get_type() != JOINT_PIN, 0);
 
-	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW*>(j);
+	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j);
 	return pin_joint->get_param(p_param);
 }
 
 void Physics2DServerSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) {
 
-
 	Joint2DSW *j = joint_owner.get(p_joint);
 	ERR_FAIL_COND(!j);
-	ERR_FAIL_COND(j->get_type()!=JOINT_DAMPED_SPRING);
+	ERR_FAIL_COND(j->get_type() != JOINT_DAMPED_SPRING);
 
-	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j);
-	dsj->set_param(p_param,p_value);
+	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j);
+	dsj->set_param(p_param, p_value);
 }
 
 real_t Physics2DServerSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const {
 
 	Joint2DSW *j = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!j,0);
-	ERR_FAIL_COND_V(j->get_type()!=JOINT_DAMPED_SPRING,0);
+	ERR_FAIL_COND_V(!j, 0);
+	ERR_FAIL_COND_V(j->get_type() != JOINT_DAMPED_SPRING, 0);
 
-	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW*>(j);
+	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j);
 	return dsj->get_param(p_param);
 }
 
 Physics2DServer::JointType Physics2DServerSW::joint_get_type(RID p_joint) const {
 
-
 	Joint2DSW *joint = joint_owner.get(p_joint);
-	ERR_FAIL_COND_V(!joint,JOINT_PIN);
+	ERR_FAIL_COND_V(!joint, JOINT_PIN);
 
 	return joint->get_type();
 }
 
-
-
 void Physics2DServerSW::free(RID p_rid) {
 
 	if (shape_owner.owns(p_rid)) {
 
 		Shape2DSW *shape = shape_owner.get(p_rid);
 
-		while(shape->get_owners().size()) {
-			ShapeOwner2DSW *so=shape->get_owners().front()->key();
+		while (shape->get_owners().size()) {
+			ShapeOwner2DSW *so = shape->get_owners().front()->key();
 			so->remove_shape(shape);
 		}
 
@@ -1189,16 +1096,15 @@ void Physics2DServerSW::free(RID p_rid) {
 
 		Body2DSW *body = body_owner.get(p_rid);
 
-//		if (body->get_state_query())
-//			_clear_query(body->get_state_query());
+		//		if (body->get_state_query())
+		//			_clear_query(body->get_state_query());
 
-//		if (body->get_direct_state_query())
-//			_clear_query(body->get_direct_state_query());
+		//		if (body->get_direct_state_query())
+		//			_clear_query(body->get_direct_state_query());
 
 		body->set_space(NULL);
 
-
-		while( body->get_shape_count() ) {
+		while (body->get_shape_count()) {
 
 			body->remove_shape(0);
 		}
@@ -1216,12 +1122,12 @@ void Physics2DServerSW::free(RID p_rid) {
 
 		Area2DSW *area = area_owner.get(p_rid);
 
-//		if (area->get_monitor_query())
-//			_clear_query(area->get_monitor_query());
+		//		if (area->get_monitor_query())
+		//			_clear_query(area->get_monitor_query());
 
 		area->set_space(NULL);
 
-		while( area->get_shape_count() ) {
+		while (area->get_shape_count()) {
 
 			area->remove_shape(0);
 		}
@@ -1232,7 +1138,7 @@ void Physics2DServerSW::free(RID p_rid) {
 
 		Space2DSW *space = space_owner.get(p_rid);
 
-		while(space->get_objects().size()) {
+		while (space->get_objects().size()) {
 			CollisionObject2DSW *co = (CollisionObject2DSW *)space->get_objects().front()->get();
 			co->set_space(NULL);
 		}
@@ -1253,52 +1159,46 @@ void Physics2DServerSW::free(RID p_rid) {
 		ERR_EXPLAIN("Invalid ID");
 		ERR_FAIL();
 	}
-
-
 };
 
 void Physics2DServerSW::set_active(bool p_active) {
 
-	active=p_active;
+	active = p_active;
 };
 
 void Physics2DServerSW::init() {
 
-	doing_sync=false;
-	last_step=0.001;
-	iterations=8;// 8?
-	stepper = memnew( Step2DSW );
-	direct_state = memnew( Physics2DDirectBodyStateSW );
+	doing_sync = false;
+	last_step = 0.001;
+	iterations = 8; // 8?
+	stepper = memnew(Step2DSW);
+	direct_state = memnew(Physics2DDirectBodyStateSW);
 };
 
-
 void Physics2DServerSW::step(float p_step) {
 
-
 	if (!active)
 		return;
 
-	doing_sync=false;
+	doing_sync = false;
 
-	last_step=p_step;
-	Physics2DDirectBodyStateSW::singleton->step=p_step;
-	island_count=0;
-	active_objects=0;
-	collision_pairs=0;
-	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+	last_step = p_step;
+	Physics2DDirectBodyStateSW::singleton->step = p_step;
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
+	for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-		stepper->step((Space2DSW*)E->get(),p_step,iterations);
-		island_count+=E->get()->get_island_count();
-		active_objects+=E->get()->get_active_objects();
-		collision_pairs+=E->get()->get_collision_pairs();
+		stepper->step((Space2DSW *)E->get(), p_step, iterations);
+		island_count += E->get()->get_island_count();
+		active_objects += E->get()->get_active_objects();
+		collision_pairs += E->get()->get_collision_pairs();
 	}
-
-
 };
 
 void Physics2DServerSW::sync() {
 
-	doing_sync=true;
+	doing_sync = true;
 };
 
 void Physics2DServerSW::flush_queries() {
@@ -1308,17 +1208,16 @@ void Physics2DServerSW::flush_queries() {
 
 	uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
 
-	for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+	for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-		Space2DSW *space=(Space2DSW *)E->get();
+		Space2DSW *space = (Space2DSW *)E->get();
 		space->call_queries();
 	}
 
-
 	if (ScriptDebugger::get_singleton() && ScriptDebugger::get_singleton()->is_profiling()) {
 
 		uint64_t total_time[Space2DSW::ELAPSED_TIME_MAX];
-		static const char* time_name[Space2DSW::ELAPSED_TIME_MAX]={
+		static const char *time_name[Space2DSW::ELAPSED_TIME_MAX] = {
 			"integrate_forces",
 			"generate_islands",
 			"setup_constraints",
@@ -1326,39 +1225,34 @@ void Physics2DServerSW::flush_queries() {
 			"integrate_velocities"
 		};
 
-		for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) {
-			total_time[i]=0;
+		for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) {
+			total_time[i] = 0;
 		}
 
-		for( Set<const Space2DSW*>::Element *E=active_spaces.front();E;E=E->next()) {
+		for (Set<const Space2DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
 
-			for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) {
-				total_time[i]+=E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i));
+			for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) {
+				total_time[i] += E->get()->get_elapsed_time(Space2DSW::ElapsedTime(i));
 			}
-
 		}
 
 		Array values;
-		values.resize(Space2DSW::ELAPSED_TIME_MAX*2);
-		for(int i=0;i<Space2DSW::ELAPSED_TIME_MAX;i++) {
-			values[i*2+0]=time_name[i];
-			values[i*2+1]=USEC_TO_SEC(total_time[i]);
+		values.resize(Space2DSW::ELAPSED_TIME_MAX * 2);
+		for (int i = 0; i < Space2DSW::ELAPSED_TIME_MAX; i++) {
+			values[i * 2 + 0] = time_name[i];
+			values[i * 2 + 1] = USEC_TO_SEC(total_time[i]);
 		}
 		values.push_back("flush_queries");
-		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec()-time_beg));
-
-		ScriptDebugger::get_singleton()->add_profiling_frame_data("physics_2d",values);
+		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg));
 
+		ScriptDebugger::get_singleton()->add_profiling_frame_data("physics_2d", values);
 	}
-
 }
 
 void Physics2DServerSW::end_sync() {
-	doing_sync=false;
+	doing_sync = false;
 }
 
-
-
 void Physics2DServerSW::finish() {
 
 	memdelete(stepper);
@@ -1367,7 +1261,7 @@ void Physics2DServerSW::finish() {
 
 int Physics2DServerSW::get_process_info(ProcessInfo p_info) {
 
-	switch(p_info) {
+	switch (p_info) {
 
 		case INFO_ACTIVE_OBJECTS: {
 
@@ -1380,32 +1274,26 @@ int Physics2DServerSW::get_process_info(ProcessInfo p_info) {
 
 			return island_count;
 		} break;
-
 	}
 
 	return 0;
 }
 
-
-Physics2DServerSW *Physics2DServerSW::singletonsw=NULL;
+Physics2DServerSW *Physics2DServerSW::singletonsw = NULL;
 
 Physics2DServerSW::Physics2DServerSW() {
 
-	singletonsw=this;
-	BroadPhase2DSW::create_func=BroadPhase2DHashGrid::_create;
-//	BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
-
-	active=true;
-	island_count=0;
-	active_objects=0;
-	collision_pairs=0;
-	using_threads=int(Globals::get_singleton()->get("physics_2d/thread_model"))==2;
+	singletonsw = this;
+	BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
+	//	BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
 
+	active = true;
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
+	using_threads = int(Globals::get_singleton()->get("physics_2d/thread_model")) == 2;
 };
 
-
-Physics2DServerSW::~Physics2DServerSW() {
+Physics2DServerSW::~Physics2DServerSW(){
 
 };
-
-
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 99ab99996..7a3fe5eaa 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -29,20 +29,18 @@
 #ifndef PHYSICS_2D_SERVER_SW
 #define PHYSICS_2D_SERVER_SW
 
-
+#include "joints_2d_sw.h"
 #include "servers/physics_2d_server.h"
 #include "shape_2d_sw.h"
 #include "space_2d_sw.h"
 #include "step_2d_sw.h"
-#include "joints_2d_sw.h"
-
 
 class Physics2DServerSW : public Physics2DServer {
 
-	OBJ_TYPE( Physics2DServerSW, Physics2DServer );
+	OBJ_TYPE(Physics2DServerSW, Physics2DServer);
 
-friend class Physics2DDirectSpaceStateSW;
-friend class Physics2DDirectBodyStateSW;
+	friend class Physics2DDirectSpaceStateSW;
+	friend class Physics2DDirectBodyStateSW;
 	bool active;
 	int iterations;
 	bool doing_sync;
@@ -54,10 +52,8 @@ friend class Physics2DDirectBodyStateSW;
 
 	bool using_threads;
 
-
-
 	Step2DSW *stepper;
-	Set<const Space2DSW*> active_spaces;
+	Set<const Space2DSW *> active_spaces;
 
 	Physics2DDirectBodyStateSW *direct_state;
 
@@ -69,10 +65,8 @@ friend class Physics2DDirectBodyStateSW;
 
 	static Physics2DServerSW *singletonsw;
 
-
-//	void _clear_query(Query2DSW *p_query);
+	//	void _clear_query(Query2DSW *p_query);
 public:
-
 	struct CollCbkData {
 
 		Vector2 valid_dir;
@@ -82,36 +76,33 @@ public:
 		Vector2 *ptr;
 	};
 
-	static void _shape_col_cbk(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata);
-
+	static void _shape_col_cbk(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
 
 	virtual RID shape_create(ShapeType p_shape);
-	virtual void shape_set_data(RID p_shape, const Variant& p_data);
+	virtual void shape_set_data(RID p_shape, const Variant &p_data);
 	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias);
 
 	virtual ShapeType shape_get_type(RID p_shape) const;
 	virtual Variant shape_get_data(RID p_shape) const;
 	virtual real_t shape_get_custom_solver_bias(RID p_shape) const;
 
-	virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count);
+	virtual bool shape_collide(RID p_shape_A, const Matrix32 &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Matrix32 &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count);
 
 	/* SPACE API */
 
 	virtual RID space_create();
-	virtual void space_set_active(RID p_space,bool p_active);
+	virtual void space_set_active(RID p_space, bool p_active);
 	virtual bool space_is_active(RID p_space) const;
 
-	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
-	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value);
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const;
 
-	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts);
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts);
 	virtual Vector<Vector2> space_get_contacts(RID p_space) const;
 	virtual int space_get_contact_count(RID p_space) const;
 
-
 	// this function only works on fixed process, errors and returns null otherwise
-	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space);
-
+	virtual Physics2DDirectSpaceState *space_get_direct_state(RID p_space);
 
 	/* AREA API */
 
@@ -123,9 +114,9 @@ public:
 	virtual void area_set_space(RID p_area, RID p_space);
 	virtual RID area_get_space(RID p_area) const;
 
-	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32());
-	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape);
-	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform);
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32 &p_transform = Matrix32());
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape);
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32 &p_transform);
 
 	virtual int area_get_shape_count(RID p_area) const;
 	virtual RID area_get_shape(RID p_area, int p_shape_idx) const;
@@ -134,28 +125,27 @@ public:
 	virtual void area_remove_shape(RID p_area, int p_shape_idx);
 	virtual void area_clear_shapes(RID p_area);
 
-	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID);
+	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID);
 	virtual ObjectID area_get_object_instance_ID(RID p_area) const;
 
-	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value);
-	virtual void area_set_transform(RID p_area, const Matrix32& p_transform);
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value);
+	virtual void area_set_transform(RID p_area, const Matrix32 &p_transform);
 
-	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
+	virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const;
 	virtual Matrix32 area_get_transform(RID p_area) const;
-	virtual void area_set_monitorable(RID p_area,bool p_monitorable);
-	virtual void area_set_collision_mask(RID p_area,uint32_t p_mask);
-	virtual void area_set_layer_mask(RID p_area,uint32_t p_mask);
-
-	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
-	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable);
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask);
+	virtual void area_set_layer_mask(RID p_area, uint32_t p_mask);
 
-	virtual void area_set_pickable(RID p_area,bool p_pickable);
+	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
+	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method);
 
+	virtual void area_set_pickable(RID p_area, bool p_pickable);
 
 	/* BODY API */
 
 	// create a body of a given type
-	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false);
+	virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false);
 
 	virtual void body_set_space(RID p_body, RID p_space);
 	virtual RID body_get_space(RID p_body) const;
@@ -163,28 +153,26 @@ public:
 	virtual void body_set_mode(RID p_body, BodyMode p_mode);
 	virtual BodyMode body_get_mode(RID p_body) const;
 
-	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32());
-	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape);
-	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform);
-	virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata);
-
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32 &p_transform = Matrix32());
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape);
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32 &p_transform);
+	virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata);
 
 	virtual int body_get_shape_count(RID p_body) const;
 	virtual RID body_get_shape(RID p_body, int p_shape_idx) const;
 	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const;
 	virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const;
 
-
 	virtual void body_remove_shape(RID p_body, int p_shape_idx);
 	virtual void body_clear_shapes(RID p_body);
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable);
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable);
 	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const;
 
-	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID);
+	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID);
 	virtual uint32_t body_get_object_instance_ID(RID p_body) const;
 
-	virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode);
+	virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode);
 	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const;
 
 	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask);
@@ -196,20 +184,19 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value);
 	virtual float body_get_param(RID p_body, BodyParameter p_param) const;
 
-
-	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant);
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
 
-	virtual void body_set_applied_force(RID p_body, const Vector2& p_force);
+	virtual void body_set_applied_force(RID p_body, const Vector2 &p_force);
 	virtual Vector2 body_get_applied_force(RID p_body) const;
 
 	virtual void body_set_applied_torque(RID p_body, float p_torque);
 	virtual float body_get_applied_torque(RID p_body) const;
 
-	virtual void body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force);
+	virtual void body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force);
 
-	virtual void body_apply_impulse(RID p_body, const Vector2& p_pos, const Vector2& p_impulse);
-	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity);
+	virtual void body_apply_impulse(RID p_body, const Vector2 &p_pos, const Vector2 &p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity);
 
 	virtual void body_add_collision_exception(RID p_body, RID p_body_b);
 	virtual void body_remove_collision_exception(RID p_body, RID p_body_b);
@@ -218,36 +205,34 @@ public:
 	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold);
 	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const;
 
-	virtual void body_set_omit_force_integration(RID p_body,bool p_omit);
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit);
 	virtual bool body_is_omitting_force_integration(RID p_body) const;
 
 	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts);
 	virtual int body_get_max_contacts_reported(RID p_body) const;
 
-	virtual void body_set_one_way_collision_direction(RID p_body,const Vector2& p_direction);
+	virtual void body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction);
 	virtual Vector2 body_get_one_way_collision_direction(RID p_body) const;
 
-	virtual void body_set_one_way_collision_max_depth(RID p_body,float p_max_depth);
+	virtual void body_set_one_way_collision_max_depth(RID p_body, float p_max_depth);
 	virtual float body_get_one_way_collision_max_depth(RID p_body) const;
 
+	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant());
+	virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count);
 
-	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
-	virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count);
-
-	virtual void body_set_pickable(RID p_body,bool p_pickable);
-
-	virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
-	virtual bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
+	virtual void body_set_pickable(RID p_body, bool p_pickable);
 
+	virtual bool body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+	virtual bool body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
 
 	/* JOINT API */
 
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
-	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
+	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const;
 
-	virtual RID pin_joint_create(const Vector2& p_pos,RID p_body_a,RID p_body_b=RID());
-	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b);
-	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID());
+	virtual RID pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b = RID());
+	virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b);
+	virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID());
 	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value);
 	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const;
 	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value);
@@ -271,8 +256,6 @@ public:
 
 	Physics2DServerSW();
 	~Physics2DServerSW();
-
 };
 
 #endif
-
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.cpp b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
index 0242d1266..0192da368 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.cpp
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.cpp
@@ -32,35 +32,33 @@
 
 void Physics2DServerWrapMT::thread_exit() {
 
-	exit=true;
+	exit = true;
 }
 
 void Physics2DServerWrapMT::thread_step(float p_delta) {
 
 	physics_2d_server->step(p_delta);
 	step_sem->post();
-
 }
 
 void Physics2DServerWrapMT::_thread_callback(void *_instance) {
 
-	Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT*>(_instance);
-
+	Physics2DServerWrapMT *vsmt = reinterpret_cast<Physics2DServerWrapMT *>(_instance);
 
 	vsmt->thread_loop();
 }
 
 void Physics2DServerWrapMT::thread_loop() {
 
-	server_thread=Thread::get_caller_ID();
+	server_thread = Thread::get_caller_ID();
 
 	OS::get_singleton()->make_rendering_thread();
 
 	physics_2d_server->init();
 
-	exit=false;
-	step_thread_up=true;
-	while(!exit) {
+	exit = false;
+	step_thread_up = true;
+	while (!exit) {
 		// flush commands one by one, until exit is requested
 		command_queue.wait_and_flush_one();
 	}
@@ -68,18 +66,15 @@ void Physics2DServerWrapMT::thread_loop() {
 	command_queue.flush_all(); // flush all
 
 	physics_2d_server->finish();
-
 }
 
-
 /* EVENT QUEUING */
 
-
 void Physics2DServerWrapMT::step(float p_step) {
 
 	if (create_thread) {
 
-		command_queue.push( this, &Physics2DServerWrapMT::thread_step,p_step);
+		command_queue.push(this, &Physics2DServerWrapMT::thread_step, p_step);
 	} else {
 
 		command_queue.flush_all(); //flush all pending from other threads
@@ -91,14 +86,14 @@ void Physics2DServerWrapMT::sync() {
 
 	if (step_sem) {
 		if (first_frame)
-			first_frame=false;
+			first_frame = false;
 		else
 			step_sem->wait(); //must not wait if a step was not issued
 	}
 	physics_2d_server->sync();
 }
 
-void Physics2DServerWrapMT::flush_queries(){
+void Physics2DServerWrapMT::flush_queries() {
 
 	physics_2d_server->flush_queries();
 }
@@ -116,10 +111,10 @@ void Physics2DServerWrapMT::init() {
 		print_line("CREATING PHYSICS 2D THREAD");
 		//OS::get_singleton()->release_rendering_thread();
 		if (create_thread) {
-			thread = Thread::create( _thread_callback, this );
+			thread = Thread::create(_thread_callback, this);
 			print_line("STARTING PHYISICS 2D THREAD");
 		}
-		while(!step_thread_up) {
+		while (!step_thread_up) {
 			OS::get_singleton()->delay_usec(1000);
 		}
 		print_line("DONE PHYSICS 2D THREAD");
@@ -127,19 +122,17 @@ void Physics2DServerWrapMT::init() {
 
 		physics_2d_server->init();
 	}
-
 }
 
 void Physics2DServerWrapMT::finish() {
 
-
 	if (thread) {
 
-		command_queue.push( this, &Physics2DServerWrapMT::thread_exit);
-		Thread::wait_to_finish( thread );
+		command_queue.push(this, &Physics2DServerWrapMT::thread_exit);
+		Thread::wait_to_finish(thread);
 		memdelete(thread);
 
-/*
+		/*
 		shape_free_cached_ids();
 		area_free_cached_ids();
 		body_free_cached_ids();
@@ -147,51 +140,46 @@ void Physics2DServerWrapMT::finish() {
 		groove_joint_free_cached_ids();
 		damped_string_free_cached_ids();
 */
-		thread=NULL;
+		thread = NULL;
 	} else {
 		physics_2d_server->finish();
 	}
 
 	if (step_sem)
 		memdelete(step_sem);
-
 }
 
+Physics2DServerWrapMT::Physics2DServerWrapMT(Physics2DServer *p_contained, bool p_create_thread)
+	: command_queue(p_create_thread) {
 
-Physics2DServerWrapMT::Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread) : command_queue(p_create_thread) {
-
-	physics_2d_server=p_contained;
-	create_thread=p_create_thread;
-	thread=NULL;
-	step_sem=NULL;
-	step_pending=0;
-	step_thread_up=false;
-	alloc_mutex=Mutex::create();
+	physics_2d_server = p_contained;
+	create_thread = p_create_thread;
+	thread = NULL;
+	step_sem = NULL;
+	step_pending = 0;
+	step_thread_up = false;
+	alloc_mutex = Mutex::create();
 
-	shape_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
-	area_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
-	body_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
-	pin_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
-	groove_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
-	damped_spring_joint_pool_max_size=GLOBAL_DEF("core/thread_rid_pool_prealloc",20);
+	shape_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
+	area_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
+	body_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
+	pin_joint_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
+	groove_joint_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
+	damped_spring_joint_pool_max_size = GLOBAL_DEF("core/thread_rid_pool_prealloc", 20);
 
 	if (!p_create_thread) {
-		server_thread=Thread::get_caller_ID();
+		server_thread = Thread::get_caller_ID();
 	} else {
-		server_thread=0;
+		server_thread = 0;
 	}
 
 	main_thread = Thread::get_caller_ID();
-	first_frame=true;
+	first_frame = true;
 }
 
-
 Physics2DServerWrapMT::~Physics2DServerWrapMT() {
 
 	memdelete(physics_2d_server);
 	memdelete(alloc_mutex);
 	//finish();
-
 }
-
-
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 160991967..63312be5c 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -29,19 +29,17 @@
 #ifndef PHYSICS2DSERVERWRAPMT_H
 #define PHYSICS2DSERVERWRAPMT_H
 
-
-#include "servers/physics_2d_server.h"
 #include "command_queue_mt.h"
-#include "os/thread.h"
 #include "globals.h"
+#include "os/thread.h"
+#include "servers/physics_2d_server.h"
 
 #ifdef DEBUG_SYNC
-#define SYNC_DEBUG print_line("sync on: "+String(__FUNCTION__));
+#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__));
 #else
 #define SYNC_DEBUG
 #endif
 
-
 class Physics2DServerWrapMT : public Physics2DServer {
 
 	mutable Physics2DServer *physics_2d_server;
@@ -65,7 +63,7 @@ class Physics2DServerWrapMT : public Physics2DServer {
 
 	void thread_exit();
 
-	Mutex*alloc_mutex;
+	Mutex *alloc_mutex;
 	bool first_frame;
 
 	int shape_pool_max_size;
@@ -81,232 +79,215 @@ class Physics2DServerWrapMT : public Physics2DServer {
 	int damped_spring_joint_pool_max_size;
 	List<RID> damped_spring_joint_id_pool;
 
-
 public:
-
 #define ServerName Physics2DServer
 #define ServerNameWrapMT Physics2DServerWrapMT
 #define server_name physics_2d_server
 #include "servers/server_wrap_mt_common.h"
 
 	//FUNC1RID(shape,ShapeType); todo fix
-	FUNC1R(RID,shape_create,ShapeType);
-	FUNC2(shape_set_data,RID,const Variant& );
-	FUNC2(shape_set_custom_solver_bias,RID,real_t );
-
-	FUNC1RC(ShapeType,shape_get_type,RID );
-	FUNC1RC(Variant,shape_get_data,RID);
-	FUNC1RC(real_t,shape_get_custom_solver_bias,RID);
+	FUNC1R(RID, shape_create, ShapeType);
+	FUNC2(shape_set_data, RID, const Variant &);
+	FUNC2(shape_set_custom_solver_bias, RID, real_t);
 
+	FUNC1RC(ShapeType, shape_get_type, RID);
+	FUNC1RC(Variant, shape_get_data, RID);
+	FUNC1RC(real_t, shape_get_custom_solver_bias, RID);
 
 	//these work well, but should be used from the main thread only
-	bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count) {
+	bool shape_collide(RID p_shape_A, const Matrix32 &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Matrix32 &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
-		return physics_2d_server->shape_collide(p_shape_A,p_xform_A,p_motion_A,p_shape_B,p_xform_B,p_motion_B,r_results,p_result_max,r_result_count);
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), false);
+		return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count);
 	}
 
 	/* SPACE API */
 
-	FUNC0R(RID,space_create);
-	FUNC2(space_set_active,RID,bool);
-	FUNC1RC(bool,space_is_active,RID);
+	FUNC0R(RID, space_create);
+	FUNC2(space_set_active, RID, bool);
+	FUNC1RC(bool, space_is_active, RID);
 
-	FUNC3(space_set_param,RID,SpaceParameter,real_t);
-	FUNC2RC(real_t,space_get_param,RID,SpaceParameter);
+	FUNC3(space_set_param, RID, SpaceParameter, real_t);
+	FUNC2RC(real_t, space_get_param, RID, SpaceParameter);
 
 	// this function only works on fixed process, errors and returns null otherwise
-	Physics2DDirectSpaceState* space_get_direct_state(RID p_space) {
+	Physics2DDirectSpaceState *space_get_direct_state(RID p_space) {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),NULL);
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), NULL);
 		return physics_2d_server->space_get_direct_state(p_space);
 	}
 
-	FUNC2(space_set_debug_contacts,RID,int);
+	FUNC2(space_set_debug_contacts, RID, int);
 	virtual Vector<Vector2> space_get_contacts(RID p_space) const {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),Vector<Vector2>());
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), Vector<Vector2>());
 		return physics_2d_server->space_get_contacts(p_space);
-
 	}
 
 	virtual int space_get_contact_count(RID p_space) const {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),0);
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), 0);
 		return physics_2d_server->space_get_contact_count(p_space);
-
 	}
 
-
-
 	/* AREA API */
 
 	//FUNC0RID(area);
-	FUNC0R(RID,area_create);
-
-	FUNC2(area_set_space,RID,RID);
-	FUNC1RC(RID,area_get_space,RID);
+	FUNC0R(RID, area_create);
 
-	FUNC2(area_set_space_override_mode,RID,AreaSpaceOverrideMode);
-	FUNC1RC(AreaSpaceOverrideMode,area_get_space_override_mode,RID);
+	FUNC2(area_set_space, RID, RID);
+	FUNC1RC(RID, area_get_space, RID);
 
-	FUNC3(area_add_shape,RID,RID,const Matrix32&);
-	FUNC3(area_set_shape,RID,int,RID);
-	FUNC3(area_set_shape_transform,RID,int,const Matrix32&);
+	FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode);
+	FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID);
 
-	FUNC1RC(int,area_get_shape_count,RID);
-	FUNC2RC(RID,area_get_shape,RID,int);
-	FUNC2RC(Matrix32,area_get_shape_transform,RID,int);
-	FUNC2(area_remove_shape,RID,int);
-	FUNC1(area_clear_shapes,RID);
+	FUNC3(area_add_shape, RID, RID, const Matrix32 &);
+	FUNC3(area_set_shape, RID, int, RID);
+	FUNC3(area_set_shape_transform, RID, int, const Matrix32 &);
 
-	FUNC2(area_attach_object_instance_ID,RID,ObjectID);
-	FUNC1RC(ObjectID,area_get_object_instance_ID,RID);
+	FUNC1RC(int, area_get_shape_count, RID);
+	FUNC2RC(RID, area_get_shape, RID, int);
+	FUNC2RC(Matrix32, area_get_shape_transform, RID, int);
+	FUNC2(area_remove_shape, RID, int);
+	FUNC1(area_clear_shapes, RID);
 
-	FUNC3(area_set_param,RID,AreaParameter,const Variant&);
-	FUNC2(area_set_transform,RID,const Matrix32&);
+	FUNC2(area_attach_object_instance_ID, RID, ObjectID);
+	FUNC1RC(ObjectID, area_get_object_instance_ID, RID);
 
-	FUNC2RC(Variant,area_get_param,RID,AreaParameter);
-	FUNC1RC(Matrix32,area_get_transform,RID);
+	FUNC3(area_set_param, RID, AreaParameter, const Variant &);
+	FUNC2(area_set_transform, RID, const Matrix32 &);
 
-	FUNC2(area_set_collision_mask,RID,uint32_t);
-	FUNC2(area_set_layer_mask,RID,uint32_t);
+	FUNC2RC(Variant, area_get_param, RID, AreaParameter);
+	FUNC1RC(Matrix32, area_get_transform, RID);
 
-	FUNC2(area_set_monitorable,RID,bool);
-	FUNC2(area_set_pickable,RID,bool);
+	FUNC2(area_set_collision_mask, RID, uint32_t);
+	FUNC2(area_set_layer_mask, RID, uint32_t);
 
-	FUNC3(area_set_monitor_callback,RID,Object*,const StringName&);
-	FUNC3(area_set_area_monitor_callback,RID,Object*,const StringName&);
+	FUNC2(area_set_monitorable, RID, bool);
+	FUNC2(area_set_pickable, RID, bool);
 
+	FUNC3(area_set_monitor_callback, RID, Object *, const StringName &);
+	FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &);
 
 	/* BODY API */
 
 	//FUNC2RID(body,BodyMode,bool);
-	FUNC2R(RID,body_create,BodyMode,bool)
+	FUNC2R(RID, body_create, BodyMode, bool)
 
-	FUNC2(body_set_space,RID,RID);
-	FUNC1RC(RID,body_get_space,RID);
+	FUNC2(body_set_space, RID, RID);
+	FUNC1RC(RID, body_get_space, RID);
 
-	FUNC2(body_set_mode,RID,BodyMode);
-	FUNC1RC(BodyMode,body_get_mode,RID);
+	FUNC2(body_set_mode, RID, BodyMode);
+	FUNC1RC(BodyMode, body_get_mode, RID);
 
+	FUNC3(body_add_shape, RID, RID, const Matrix32 &);
+	FUNC3(body_set_shape, RID, int, RID);
+	FUNC3(body_set_shape_transform, RID, int, const Matrix32 &);
+	FUNC3(body_set_shape_metadata, RID, int, const Variant &);
 
-	FUNC3(body_add_shape,RID,RID,const Matrix32&);
-	FUNC3(body_set_shape,RID,int,RID);
-	FUNC3(body_set_shape_transform,RID,int,const Matrix32&);
-	FUNC3(body_set_shape_metadata,RID,int,const Variant&);
+	FUNC1RC(int, body_get_shape_count, RID);
+	FUNC2RC(Matrix32, body_get_shape_transform, RID, int);
+	FUNC2RC(Variant, body_get_shape_metadata, RID, int);
+	FUNC2RC(RID, body_get_shape, RID, int);
 
-	FUNC1RC(int,body_get_shape_count,RID);
-	FUNC2RC(Matrix32,body_get_shape_transform,RID,int);
-	FUNC2RC(Variant,body_get_shape_metadata,RID,int);
-	FUNC2RC(RID,body_get_shape,RID,int);
+	FUNC3(body_set_shape_as_trigger, RID, int, bool);
+	FUNC2RC(bool, body_is_shape_set_as_trigger, RID, int);
 
-	FUNC3(body_set_shape_as_trigger,RID,int,bool);
-	FUNC2RC(bool,body_is_shape_set_as_trigger,RID,int);
+	FUNC2(body_remove_shape, RID, int);
+	FUNC1(body_clear_shapes, RID);
 
-	FUNC2(body_remove_shape,RID,int);
-	FUNC1(body_clear_shapes,RID);
+	FUNC2(body_attach_object_instance_ID, RID, uint32_t);
+	FUNC1RC(uint32_t, body_get_object_instance_ID, RID);
 
-	FUNC2(body_attach_object_instance_ID,RID,uint32_t);
-	FUNC1RC(uint32_t,body_get_object_instance_ID,RID);
+	FUNC2(body_set_continuous_collision_detection_mode, RID, CCDMode);
+	FUNC1RC(CCDMode, body_get_continuous_collision_detection_mode, RID);
 
-	FUNC2(body_set_continuous_collision_detection_mode,RID,CCDMode);
-	FUNC1RC(CCDMode,body_get_continuous_collision_detection_mode,RID);
+	FUNC2(body_set_layer_mask, RID, uint32_t);
+	FUNC1RC(uint32_t, body_get_layer_mask, RID);
 
-	FUNC2(body_set_layer_mask,RID,uint32_t);
-	FUNC1RC(uint32_t,body_get_layer_mask,RID);
+	FUNC2(body_set_collision_mask, RID, uint32_t);
+	FUNC1RC(uint32_t, body_get_collision_mask, RID);
 
-	FUNC2(body_set_collision_mask,RID,uint32_t);
-	FUNC1RC(uint32_t,body_get_collision_mask,RID);
+	FUNC3(body_set_param, RID, BodyParameter, float);
+	FUNC2RC(float, body_get_param, RID, BodyParameter);
 
+	FUNC3(body_set_state, RID, BodyState, const Variant &);
+	FUNC2RC(Variant, body_get_state, RID, BodyState);
 
-	FUNC3(body_set_param,RID,BodyParameter,float);
-	FUNC2RC(float,body_get_param,RID,BodyParameter);
+	FUNC2(body_set_applied_force, RID, const Vector2 &);
+	FUNC1RC(Vector2, body_get_applied_force, RID);
 
+	FUNC2(body_set_applied_torque, RID, float);
+	FUNC1RC(float, body_get_applied_torque, RID);
 
-	FUNC3(body_set_state,RID,BodyState,const Variant&);
-	FUNC2RC(Variant,body_get_state,RID,BodyState);
+	FUNC3(body_add_force, RID, const Vector2 &, const Vector2 &);
+	FUNC3(body_apply_impulse, RID, const Vector2 &, const Vector2 &);
+	FUNC2(body_set_axis_velocity, RID, const Vector2 &);
 
-	FUNC2(body_set_applied_force,RID,const Vector2&);
-	FUNC1RC(Vector2,body_get_applied_force,RID);
+	FUNC2(body_add_collision_exception, RID, RID);
+	FUNC2(body_remove_collision_exception, RID, RID);
+	FUNC2S(body_get_collision_exceptions, RID, List<RID> *);
 
-	FUNC2(body_set_applied_torque,RID,float);
-	FUNC1RC(float,body_get_applied_torque,RID);
+	FUNC2(body_set_max_contacts_reported, RID, int);
+	FUNC1RC(int, body_get_max_contacts_reported, RID);
 
-	FUNC3(body_add_force,RID,const Vector2&,const Vector2&);
-	FUNC3(body_apply_impulse,RID,const Vector2&,const Vector2&);
-	FUNC2(body_set_axis_velocity,RID,const Vector2&);
+	FUNC2(body_set_one_way_collision_direction, RID, const Vector2 &);
+	FUNC1RC(Vector2, body_get_one_way_collision_direction, RID);
 
-	FUNC2(body_add_collision_exception,RID,RID);
-	FUNC2(body_remove_collision_exception,RID,RID);
-	FUNC2S(body_get_collision_exceptions,RID,List<RID>*);
+	FUNC2(body_set_one_way_collision_max_depth, RID, float);
+	FUNC1RC(float, body_get_one_way_collision_max_depth, RID);
 
-	FUNC2(body_set_max_contacts_reported,RID,int);
-	FUNC1RC(int,body_get_max_contacts_reported,RID);
+	FUNC2(body_set_contacts_reported_depth_treshold, RID, float);
+	FUNC1RC(float, body_get_contacts_reported_depth_treshold, RID);
 
-	FUNC2(body_set_one_way_collision_direction,RID,const Vector2&);
-	FUNC1RC(Vector2,body_get_one_way_collision_direction,RID);
+	FUNC2(body_set_omit_force_integration, RID, bool);
+	FUNC1RC(bool, body_is_omitting_force_integration, RID);
 
-	FUNC2(body_set_one_way_collision_max_depth,RID,float);
-	FUNC1RC(float,body_get_one_way_collision_max_depth,RID);
+	FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &);
 
-
-	FUNC2(body_set_contacts_reported_depth_treshold,RID,float);
-	FUNC1RC(float,body_get_contacts_reported_depth_treshold,RID);
-
-	FUNC2(body_set_omit_force_integration,RID,bool);
-	FUNC1RC(bool,body_is_omitting_force_integration,RID);
-
-	FUNC4(body_set_force_integration_callback,RID ,Object *,const StringName& ,const Variant& );
-
-
-	bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count) {
-		return physics_2d_server->body_collide_shape(p_body,p_body_shape,p_shape,p_shape_xform,p_motion,r_results,p_result_max,r_result_count);
+	bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) {
+		return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count);
 	}
 
-	FUNC2(body_set_pickable,RID,bool);
+	FUNC2(body_set_pickable, RID, bool);
 
-	bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) {
+	bool body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
-		return physics_2d_server->body_test_motion(p_body,p_motion,p_margin,r_result);
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), false);
+		return physics_2d_server->body_test_motion(p_body, p_motion, p_margin, r_result);
 	}
 
-	bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) {
+	bool body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) {
 
-		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
-		return physics_2d_server->body_test_motion_from(p_body,p_from,p_motion,p_margin,r_result);
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_ID(), false);
+		return physics_2d_server->body_test_motion_from(p_body, p_from, p_motion, p_margin, r_result);
 	}
 
 	/* JOINT API */
 
-
-	FUNC3(joint_set_param,RID,JointParam,real_t);
-	FUNC2RC(real_t,joint_get_param,RID,JointParam);
-
+	FUNC3(joint_set_param, RID, JointParam, real_t);
+	FUNC2RC(real_t, joint_get_param, RID, JointParam);
 
 	///FUNC3RID(pin_joint,const Vector2&,RID,RID);
 	///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID);
 	///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID);
 
-	FUNC3R(RID,pin_joint_create,const Vector2&,RID,RID);
-	FUNC5R(RID,groove_joint_create,const Vector2&,const Vector2&,const Vector2&,RID,RID);
-	FUNC4R(RID,damped_spring_joint_create,const Vector2&,const Vector2&,RID,RID);
+	FUNC3R(RID, pin_joint_create, const Vector2 &, RID, RID);
+	FUNC5R(RID, groove_joint_create, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID);
+	FUNC4R(RID, damped_spring_joint_create, const Vector2 &, const Vector2 &, RID, RID);
 
-	FUNC3(pin_joint_set_param,RID,PinJointParam,real_t);
-	FUNC2RC(real_t,pin_joint_get_param,RID,PinJointParam);
+	FUNC3(pin_joint_set_param, RID, PinJointParam, real_t);
+	FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam);
 
-	FUNC3(damped_string_joint_set_param,RID,DampedStringParam,real_t);
-	FUNC2RC(real_t,damped_string_joint_get_param,RID,DampedStringParam);
-
-	FUNC1RC(JointType,joint_get_type,RID);
+	FUNC3(damped_string_joint_set_param, RID, DampedStringParam, real_t);
+	FUNC2RC(real_t, damped_string_joint_get_param, RID, DampedStringParam);
 
+	FUNC1RC(JointType, joint_get_type, RID);
 
 	/* MISC */
 
-
-	FUNC1(free,RID);
-	FUNC1(set_active,bool);
+	FUNC1(free, RID);
+	FUNC1(set_active, bool);
 
 	virtual void init();
 	virtual void step(float p_step);
@@ -319,28 +300,24 @@ public:
 		return physics_2d_server->get_process_info(p_info);
 	}
 
-	Physics2DServerWrapMT(Physics2DServer* p_contained,bool p_create_thread);
+	Physics2DServerWrapMT(Physics2DServer *p_contained, bool p_create_thread);
 	~Physics2DServerWrapMT();
 
+	template <class T>
+	static Physics2DServer *init_server() {
 
-	template<class T>
-	static Physics2DServer* init_server() {
-
-		int tm = GLOBAL_DEF("physics_2d/thread_model",1);
-		if (tm==0) //single unsafe
-			return memnew( T );
-		else if (tm==1) //single saef
-			return memnew( Physics2DServerWrapMT( memnew( T ), false ));
+		int tm = GLOBAL_DEF("physics_2d/thread_model", 1);
+		if (tm == 0) //single unsafe
+			return memnew(T);
+		else if (tm == 1) //single saef
+			return memnew(Physics2DServerWrapMT(memnew(T), false));
 		else //single unsafe
-			return memnew( Physics2DServerWrapMT( memnew( T ), true ));
-
-
+			return memnew(Physics2DServerWrapMT(memnew(T), true));
 	}
 
 #undef ServerNameWrapMT
 #undef ServerName
 #undef server_name
-
 };
 
 #ifdef DEBUG_SYNC
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 6506349ea..c438908ca 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -30,108 +30,98 @@
 #include "geometry.h"
 #include "sort.h"
 
-
-
-void Shape2DSW::configure(const Rect2& p_aabb) {
-	aabb=p_aabb;
-	configured=true;
-	for (Map<ShapeOwner2DSW*,int>::Element *E=owners.front();E;E=E->next()) {
-		ShapeOwner2DSW* co=(ShapeOwner2DSW*)E->key();
+void Shape2DSW::configure(const Rect2 &p_aabb) {
+	aabb = p_aabb;
+	configured = true;
+	for (Map<ShapeOwner2DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
+		ShapeOwner2DSW *co = (ShapeOwner2DSW *)E->key();
 		co->_shape_changed();
 	}
 }
 
-
-Vector2 Shape2DSW::get_support(const Vector2& p_normal) const {
+Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const {
 
 	Vector2 res[2];
 	int amnt;
-	get_supports(p_normal,res,amnt);
+	get_supports(p_normal, res, amnt);
 	return res[0];
 }
 
 void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
 
-	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+	Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
 	if (E) {
 		E->get()++;
 	} else {
-		owners[p_owner]=1;
+		owners[p_owner] = 1;
 	}
 }
 
-void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner){
+void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
 
-	Map<ShapeOwner2DSW*,int>::Element *E=owners.find(p_owner);
+	Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner);
 	ERR_FAIL_COND(!E);
 	E->get()--;
-	if (E->get()==0) {
+	if (E->get() == 0) {
 		owners.erase(E);
 	}
-
 }
 
-bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const{
+bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const {
 
 	return owners.has(p_owner);
-
 }
 
-const Map<ShapeOwner2DSW*,int>& Shape2DSW::get_owners() const{
+const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const {
 	return owners;
 }
 
-
 Shape2DSW::Shape2DSW() {
 
-	custom_bias=0;
-	configured=false;
+	custom_bias = 0;
+	configured = false;
 }
 
-
 Shape2DSW::~Shape2DSW() {
 
 	ERR_FAIL_COND(owners.size());
 }
 
-
 /*********************************************************/
 /*********************************************************/
 /*********************************************************/
 
+void LineShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-
-void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	r_amount=0;
+	r_amount = 0;
 }
 
-bool LineShape2DSW::contains_point(const Vector2& p_point) const {
+bool LineShape2DSW::contains_point(const Vector2 &p_point) const {
 
 	return normal.dot(p_point) < d;
 }
 
-bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+bool LineShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
-	Vector2 segment= p_begin - p_end;
-	real_t den=normal.dot( segment );
+	Vector2 segment = p_begin - p_end;
+	real_t den = normal.dot(segment);
 
 	//printf("den is %i\n",den);
-	if (Math::abs(den)<=CMP_EPSILON) {
+	if (Math::abs(den) <= CMP_EPSILON) {
 
 		return false;
 	}
 
-	real_t dist=(normal.dot( p_begin ) - d) / den;
+	real_t dist = (normal.dot(p_begin) - d) / den;
 	//printf("dist is %i\n",dist);
 
-	if (dist<-CMP_EPSILON || dist > (1.0 +CMP_EPSILON)) {
+	if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
 
 		return false;
 	}
 
 	r_point = p_begin + segment * -dist;
-	r_normal=normal;
+	r_normal = normal;
 
 	return true;
 }
@@ -141,24 +131,22 @@ real_t LineShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale)
 	return 0;
 }
 
+void LineShape2DSW::set_data(const Variant &p_data) {
 
-void LineShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY);
+	ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
 	Array arr = p_data;
-	ERR_FAIL_COND(arr.size()!=2);
-	normal=arr[0];
-	d=arr[1];
-	configure(Rect2(Vector2(-1e4,-1e4),Vector2(1e4*2,1e4*2)));
-
+	ERR_FAIL_COND(arr.size() != 2);
+	normal = arr[0];
+	d = arr[1];
+	configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2)));
 }
 
 Variant LineShape2DSW::get_data() const {
 
 	Array arr;
 	arr.resize(2);
-	arr[0]=normal;
-	arr[1]=d;
+	arr[0] = normal;
+	arr[1] = d;
 	return arr;
 }
 
@@ -166,29 +154,24 @@ Variant LineShape2DSW::get_data() const {
 /*********************************************************/
 /*********************************************************/
 
+void RayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
+	r_amount = 1;
 
-void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-
-	r_amount=1;
-
-	if (p_normal.y>0)
-		*r_supports=Vector2(0,length);
+	if (p_normal.y > 0)
+		*r_supports = Vector2(0, length);
 	else
-		*r_supports=Vector2();
-
+		*r_supports = Vector2();
 }
 
-bool RayShape2DSW::contains_point(const Vector2& p_point) const {
+bool RayShape2DSW::contains_point(const Vector2 &p_point) const {
 
 	return false;
 }
 
-bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+bool RayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
 	return false; //rays can't be intersected
-
 }
 
 real_t RayShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
@@ -196,10 +179,10 @@ real_t RayShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) c
 	return 0; //rays are mass-less
 }
 
-void RayShape2DSW::set_data(const Variant& p_data) {
+void RayShape2DSW::set_data(const Variant &p_data) {
 
-	length=p_data;
-	configure(Rect2(0,0,0.001,length));
+	length = p_data;
+	configure(Rect2(0, 0, 0.001, length));
 }
 
 Variant RayShape2DSW::get_data() const {
@@ -207,46 +190,41 @@ Variant RayShape2DSW::get_data() const {
 	return length;
 }
 
-
 /*********************************************************/
 /*********************************************************/
 /*********************************************************/
 
+void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-
-void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	if (Math::abs(p_normal.dot(n))>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)  {
-		r_supports[0]=a;
-		r_supports[1]=b;
-		r_amount=2;
+	if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
+		r_supports[0] = a;
+		r_supports[1] = b;
+		r_amount = 2;
 		return;
-
 	}
 
-	float dp=p_normal.dot(b-a);
-	if (dp>0)
-		*r_supports=b;
+	float dp = p_normal.dot(b - a);
+	if (dp > 0)
+		*r_supports = b;
 	else
-		*r_supports=a;
-	r_amount=1;
-
+		*r_supports = a;
+	r_amount = 1;
 }
 
-bool SegmentShape2DSW::contains_point(const Vector2& p_point) const {
+bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const {
 
 	return false;
 }
 
-bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
+bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
-	if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
+	if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &r_point))
 		return false;
 
 	if (n.dot(p_begin) > n.dot(a)) {
-		r_normal=n;
+		r_normal = n;
 	} else {
-		r_normal=-n;
+		r_normal = -n;
 	}
 
 	return true;
@@ -254,38 +232,38 @@ bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p
 
 real_t SegmentShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
 
-	Vector2 s[2]={a*p_scale,b*p_scale};
+	Vector2 s[2] = { a * p_scale, b * p_scale };
 
 	real_t l = s[1].distance_to(s[0]);
-	Vector2 ofs = (s[0]+s[1])*0.5;
+	Vector2 ofs = (s[0] + s[1]) * 0.5;
 
-	return p_mass*(l*l/12.0f + ofs.length_squared());
+	return p_mass * (l * l / 12.0f + ofs.length_squared());
 }
 
-void SegmentShape2DSW::set_data(const Variant& p_data) {
+void SegmentShape2DSW::set_data(const Variant &p_data) {
 
-	ERR_FAIL_COND(p_data.get_type()!=Variant::RECT2);
+	ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
 
 	Rect2 r = p_data;
-	a=r.pos;
-	b=r.size;
-	n=(b-a).tangent();
+	a = r.pos;
+	b = r.size;
+	n = (b - a).tangent();
 
 	Rect2 aabb;
-	aabb.pos=a;
+	aabb.pos = a;
 	aabb.expand_to(b);
-	if (aabb.size.x==0)
-		aabb.size.x=0.001;
-	if (aabb.size.y==0)
-		aabb.size.y=0.001;
+	if (aabb.size.x == 0)
+		aabb.size.x = 0.001;
+	if (aabb.size.y == 0)
+		aabb.size.y = 0.001;
 	configure(aabb);
 }
 
 Variant SegmentShape2DSW::get_data() const {
 
 	Rect2 r;
-	r.pos=a;
-	r.size=b;
+	r.pos = a;
+	r.size = b;
 	return r;
 }
 
@@ -293,24 +271,18 @@ Variant SegmentShape2DSW::get_data() const {
 /*********************************************************/
 /*********************************************************/
 
+void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-
-void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	r_amount=1;
-	*r_supports=p_normal*radius;
-
+	r_amount = 1;
+	*r_supports = p_normal * radius;
 }
 
+bool CircleShape2DSW::contains_point(const Vector2 &p_point) const {
 
-bool CircleShape2DSW::contains_point(const Vector2& p_point) const {
-
-	return p_point.length_squared() < radius*radius;
+	return p_point.length_squared() < radius * radius;
 }
 
-
-bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
+bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
 	Vector2 line_vec = p_end - p_begin;
 
@@ -320,33 +292,32 @@ bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_
 	b = 2 * p_begin.dot(line_vec);
 	c = p_begin.dot(p_begin) - radius * radius;
 
-	real_t sqrtterm = b*b - 4*a*c;
+	real_t sqrtterm = b * b - 4 * a * c;
 
-	if(sqrtterm < 0)
+	if (sqrtterm < 0)
 		return false;
 	sqrtterm = Math::sqrt(sqrtterm);
-	real_t res = ( -b - sqrtterm ) / (2 * a);
+	real_t res = (-b - sqrtterm) / (2 * a);
 
-	if (res <0 || res >1+CMP_EPSILON) {
+	if (res < 0 || res > 1 + CMP_EPSILON) {
 		return false;
 	}
 
-	r_point=p_begin+line_vec*res;
-	r_normal=r_point.normalized();
+	r_point = p_begin + line_vec * res;
+	r_normal = r_point.normalized();
 	return true;
 }
 
 real_t CircleShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
 
-	return (radius*radius)*(p_scale.x*0.5+p_scale.y*0.5);
-
+	return (radius * radius) * (p_scale.x * 0.5 + p_scale.y * 0.5);
 }
 
-void CircleShape2DSW::set_data(const Variant& p_data) {
+void CircleShape2DSW::set_data(const Variant &p_data) {
 
 	ERR_FAIL_COND(!p_data.is_num());
-	radius=p_data;
-	configure(Rect2(-radius,-radius,radius*2,radius*2));
+	radius = p_data;
+	configure(Rect2(-radius, -radius, radius * 2, radius * 2));
 }
 
 Variant CircleShape2DSW::get_data() const {
@@ -354,71 +325,63 @@ Variant CircleShape2DSW::get_data() const {
 	return radius;
 }
 
-
 /*********************************************************/
 /*********************************************************/
 /*********************************************************/
 
+void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-
-void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	for(int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
 		Vector2 ag;
-		ag[i]=1.0;
+		ag[i] = 1.0;
 		float dp = ag.dot(p_normal);
-		if (Math::abs(dp)<_SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
+		if (Math::abs(dp) < _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)
 			continue;
 
-		float sgn = dp>0 ? 1.0 : -1.0;
+		float sgn = dp > 0 ? 1.0 : -1.0;
 
-		r_amount=2;
+		r_amount = 2;
 
-		r_supports[0][i]=half_extents[i]*sgn;
-		r_supports[0][i^1]=half_extents[i^1];
+		r_supports[0][i] = half_extents[i] * sgn;
+		r_supports[0][i ^ 1] = half_extents[i ^ 1];
 
-		r_supports[1][i]=half_extents[i]*sgn;
-		r_supports[1][i^1]=-half_extents[i^1];
+		r_supports[1][i] = half_extents[i] * sgn;
+		r_supports[1][i ^ 1] = -half_extents[i ^ 1];
 
 		return;
-
-
 	}
 
 	/* USE POINT */
 
-	r_amount=1;
-	r_supports[0]=Vector2(
-		(p_normal.x<0) ? -half_extents.x : half_extents.x,
-		(p_normal.y<0) ? -half_extents.y : half_extents.y
-	);
-
+	r_amount = 1;
+	r_supports[0] = Vector2(
+			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
+			(p_normal.y < 0) ? -half_extents.y : half_extents.y);
 }
 
-bool RectangleShape2DSW::contains_point(const Vector2& p_point) const {
+bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
 
-	return Math::abs(p_point.x)<half_extents.x && Math::abs(p_point.y)<half_extents.y;
+	return Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y;
 }
 
-bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
+bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
-	return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
+	return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
 }
 
-real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass,const Size2& p_scale) const {
+real_t RectangleShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
 
-	Vector2 he2=half_extents*2*p_scale;
-	return p_mass*he2.dot(he2)/12.0f;
+	Vector2 he2 = half_extents * 2 * p_scale;
+	return p_mass * he2.dot(he2) / 12.0f;
 }
 
-void RectangleShape2DSW::set_data(const Variant& p_data) {
+void RectangleShape2DSW::set_data(const Variant &p_data) {
 
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2);
+	ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
 
-	half_extents=p_data;
-	configure(Rect2(-half_extents,half_extents*2.0));
+	half_extents = p_data;
+	configure(Rect2(-half_extents, half_extents * 2.0));
 }
 
 Variant RectangleShape2DSW::get_data() const {
@@ -426,71 +389,65 @@ Variant RectangleShape2DSW::get_data() const {
 	return half_extents;
 }
 
-
-
 /*********************************************************/
 /*********************************************************/
 /*********************************************************/
 
+void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-
-void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	Vector2 n=p_normal;
+	Vector2 n = p_normal;
 
 	float d = n.y;
 
-	if (Math::abs( d )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+	if (Math::abs(d) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) {
 
 		// make it flat
-		n.y=0.0;
+		n.y = 0.0;
 		n.normalize();
-		n*=radius;
+		n *= radius;
 
-		r_amount=2;
-		r_supports[0]=n;
-		r_supports[0].y+=height*0.5;
-		r_supports[1]=n;
-		r_supports[1].y-=height*0.5;
+		r_amount = 2;
+		r_supports[0] = n;
+		r_supports[0].y += height * 0.5;
+		r_supports[1] = n;
+		r_supports[1].y -= height * 0.5;
 
 	} else {
 
 		float h = (d > 0) ? height : -height;
 
-		n*=radius;
-		n.y += h*0.5;
-		r_amount=1;
-		*r_supports=n;
-
+		n *= radius;
+		n.y += h * 0.5;
+		r_amount = 1;
+		*r_supports = n;
 	}
 }
 
-bool CapsuleShape2DSW::contains_point(const Vector2& p_point) const {
+bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const {
 
 	Vector2 p = p_point;
-	p.y=Math::abs(p.y);
-	p.y-=height*0.5;
-	if (p.y<0)
-		p.y=0;
+	p.y = Math::abs(p.y);
+	p.y -= height * 0.5;
+	if (p.y < 0)
+		p.y = 0;
 
-	return p.length_squared() < radius*radius;
+	return p.length_squared() < radius * radius;
 }
 
-bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
+bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
 	float d = 1e10;
-	Vector2 n = (p_end-p_begin).normalized();
-	bool collided=false;
+	Vector2 n = (p_end - p_begin).normalized();
+	bool collided = false;
 
 	//try spheres
-	for(int i=0;i<2;i++) {
+	for (int i = 0; i < 2; i++) {
 
 		Vector2 begin = p_begin;
 		Vector2 end = p_end;
-		float ofs = (i==0)?-height*0.5:height*0.5;
-		begin.y+=ofs;
-		end.y+=ofs;
+		float ofs = (i == 0) ? -height * 0.5 : height * 0.5;
+		begin.y += ofs;
+		end.y += ofs;
 
 		Vector2 line_vec = end - begin;
 
@@ -500,39 +457,38 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p
 		b = 2 * begin.dot(line_vec);
 		c = begin.dot(begin) - radius * radius;
 
-		real_t sqrtterm = b*b - 4*a*c;
+		real_t sqrtterm = b * b - 4 * a * c;
 
-		if(sqrtterm < 0)
+		if (sqrtterm < 0)
 			continue;
 
 		sqrtterm = Math::sqrt(sqrtterm);
-		real_t res = ( -b - sqrtterm ) / (2 * a);
+		real_t res = (-b - sqrtterm) / (2 * a);
 
-		if (res <0 || res >1+CMP_EPSILON) {
+		if (res < 0 || res > 1 + CMP_EPSILON) {
 			continue;
 		}
 
-		Vector2 point = begin+line_vec*res;
-		Vector2 pointf(point.x,point.y-ofs);
+		Vector2 point = begin + line_vec * res;
+		Vector2 pointf(point.x, point.y - ofs);
 		real_t pd = n.dot(pointf);
-		if (pd<d) {
-			r_point=pointf;
-			r_normal=point.normalized();
-			d=pd;
-			collided=true;
+		if (pd < d) {
+			r_point = pointf;
+			r_normal = point.normalized();
+			d = pd;
+			collided = true;
 		}
 	}
 
-
-	Vector2 rpos,rnorm;
-	if (Rect2( Point2(-radius,-height*0.5), Size2(radius*2.0,height) ).intersects_segment(p_begin,p_end,&rpos,&rnorm)) {
+	Vector2 rpos, rnorm;
+	if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
 
 		real_t pd = n.dot(rpos);
-		if (pd<d) {
-			r_point=rpos;
-			r_normal=rnorm;
-			d=pd;
-			collided=true;
+		if (pd < d) {
+			r_point = rpos;
+			r_normal = rnorm;
+			d = pd;
+			collided = true;
 		}
 	}
 
@@ -542,198 +498,184 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p
 
 real_t CapsuleShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
 
-	Vector2 he2=Vector2(radius*2,height+radius*2)*p_scale;
-	return p_mass*he2.dot(he2)/12.0f;
+	Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale;
+	return p_mass * he2.dot(he2) / 12.0f;
 }
 
-void CapsuleShape2DSW::set_data(const Variant& p_data) {
+void CapsuleShape2DSW::set_data(const Variant &p_data) {
 
-	ERR_FAIL_COND(p_data.get_type()!=Variant::ARRAY && p_data.get_type()!=Variant::VECTOR2);
+	ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
 
-	if (p_data.get_type()==Variant::ARRAY) {
-		Array arr=p_data;
-		ERR_FAIL_COND(arr.size()!=2);
-		height=arr[0];
-		radius=arr[1];
+	if (p_data.get_type() == Variant::ARRAY) {
+		Array arr = p_data;
+		ERR_FAIL_COND(arr.size() != 2);
+		height = arr[0];
+		radius = arr[1];
 	} else {
 
 		Point2 p = p_data;
-		radius=p.x;
-		height=p.y;
+		radius = p.x;
+		height = p.y;
 	}
 
-	Point2 he(radius,height*0.5+radius);
-	configure(Rect2(-he,he*2));
-
+	Point2 he(radius, height * 0.5 + radius);
+	configure(Rect2(-he, he * 2));
 }
 
 Variant CapsuleShape2DSW::get_data() const {
 
-	return Point2(height,radius);
+	return Point2(height, radius);
 }
 
-
-
 /*********************************************************/
 /*********************************************************/
 /*********************************************************/
 
+void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
+	int support_idx = -1;
+	real_t d = -1e10;
 
-void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	int support_idx=-1;
-	real_t d=-1e10;
-
-	for(int i=0;i<point_count;i++) {
+	for (int i = 0; i < point_count; i++) {
 
 		//test point
 		real_t ld = p_normal.dot(points[i].pos);
-		if (ld>d) {
-			support_idx=i;
-			d=ld;
+		if (ld > d) {
+			support_idx = i;
+			d = ld;
 		}
 
 		//test segment
-		if (points[i].normal.dot(p_normal)>_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
+		if (points[i].normal.dot(p_normal) > _SEGMENT_IS_VALID_SUPPORT_TRESHOLD) {
 
-			r_amount=2;
-			r_supports[0]=points[i].pos;
-			r_supports[1]=points[(i+1)%point_count].pos;
+			r_amount = 2;
+			r_supports[0] = points[i].pos;
+			r_supports[1] = points[(i + 1) % point_count].pos;
 			return;
 		}
 	}
 
-	ERR_FAIL_COND(support_idx==-1);
-
-	r_amount=1;
-	r_supports[0]=points[support_idx].pos;
+	ERR_FAIL_COND(support_idx == -1);
 
+	r_amount = 1;
+	r_supports[0] = points[support_idx].pos;
 }
 
+bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const {
 
-bool ConvexPolygonShape2DSW::contains_point(const Vector2& p_point) const {
+	bool out = false;
+	bool in = false;
 
-	bool out=false;
-	bool in=false;
-
-	for(int i=0;i<point_count;i++) {
+	for (int i = 0; i < point_count; i++) {
 
 		float d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
-		if (d>0)
-			out=true;
+		if (d > 0)
+			out = true;
 		else
-			in=true;
+			in = true;
 	}
 
 	return (in && !out) || (!in && out);
 }
 
+bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
-bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
-
-	Vector2 n = (p_end-p_begin).normalized();
-	real_t d=1e10;
-	bool inters=false;
+	Vector2 n = (p_end - p_begin).normalized();
+	real_t d = 1e10;
+	bool inters = false;
 
-	for(int i=0;i<point_count;i++) {
+	for (int i = 0; i < point_count; i++) {
 
 		//hmm crap.. no can do..
 		//if (d.dot(points[i].normal)>=0)
 		//	continue;
 
-
 		Vector2 res;
 
-		if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,points[i].pos,points[(i+1)%point_count].pos,&res))
+		if (!Geometry::segment_intersects_segment_2d(p_begin, p_end, points[i].pos, points[(i + 1) % point_count].pos, &res))
 			continue;
 
 		float nd = n.dot(res);
-		if (nd<d) {
+		if (nd < d) {
 
-			d=nd;
-			r_point=res;
-			r_normal=points[i].normal;
-			inters=true;
+			d = nd;
+			r_point = res;
+			r_normal = points[i].normal;
+			inters = true;
 		}
-
 	}
 
-
 	if (inters) {
 
-		if (n.dot(r_normal)>0)
-			r_normal=-r_normal;
+		if (n.dot(r_normal) > 0)
+			r_normal = -r_normal;
 	}
 
 	//return get_aabb().intersects_segment(p_begin,p_end,&r_point,&r_normal);
 	return inters; //todo
 }
 
-real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass,const Size2& p_scale) const {
+real_t ConvexPolygonShape2DSW::get_moment_of_inertia(float p_mass, const Size2 &p_scale) const {
 
 	Rect2 aabb;
-	aabb.pos=points[0].pos*p_scale;
-	for(int i=0;i<point_count;i++) {
+	aabb.pos = points[0].pos * p_scale;
+	for (int i = 0; i < point_count; i++) {
 
-		aabb.expand_to(points[i].pos*p_scale);
+		aabb.expand_to(points[i].pos * p_scale);
 	}
 
-	return p_mass*aabb.size.dot(aabb.size)/12.0f + p_mass * (aabb.pos+aabb.size*0.5).length_squared();
+	return p_mass * aabb.size.dot(aabb.size) / 12.0f + p_mass * (aabb.pos + aabb.size * 0.5).length_squared();
 }
 
-void ConvexPolygonShape2DSW::set_data(const Variant& p_data) {
-
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
 
+	ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2_ARRAY && p_data.get_type() != Variant::REAL_ARRAY);
 
 	if (points)
 		memdelete_arr(points);
-	points=NULL;
-	point_count=0;
+	points = NULL;
+	point_count = 0;
 
-	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
-		DVector<Vector2> arr=p_data;
-		ERR_FAIL_COND(arr.size()==0);
-		point_count=arr.size();
-		points = memnew_arr(Point,point_count);
+	if (p_data.get_type() == Variant::VECTOR2_ARRAY) {
+		DVector<Vector2> arr = p_data;
+		ERR_FAIL_COND(arr.size() == 0);
+		point_count = arr.size();
+		points = memnew_arr(Point, point_count);
 		DVector<Vector2>::Read r = arr.read();
 
-		for(int i=0;i<point_count;i++) {
-			points[i].pos=r[i];
+		for (int i = 0; i < point_count; i++) {
+			points[i].pos = r[i];
 		}
 
-		for(int i=0;i<point_count;i++) {
+		for (int i = 0; i < point_count; i++) {
 
 			Vector2 p = points[i].pos;
-			Vector2 pn = points[(i+1)%point_count].pos;
-			points[i].normal=(pn-p).tangent().normalized();
+			Vector2 pn = points[(i + 1) % point_count].pos;
+			points[i].normal = (pn - p).tangent().normalized();
 		}
 	} else {
 
 		DVector<real_t> dvr = p_data;
-		point_count=dvr.size()/4;
-		ERR_FAIL_COND(point_count==0);
+		point_count = dvr.size() / 4;
+		ERR_FAIL_COND(point_count == 0);
 
-		points = memnew_arr(Point,point_count);
+		points = memnew_arr(Point, point_count);
 		DVector<real_t>::Read r = dvr.read();
 
-		for(int i=0;i<point_count;i++) {
-
-			int idx=i<<2;
-			points[i].pos.x=r[idx+0];
-			points[i].pos.y=r[idx+1];
-			points[i].normal.x=r[idx+2];
-			points[i].normal.y=r[idx+3];
+		for (int i = 0; i < point_count; i++) {
 
+			int idx = i << 2;
+			points[i].pos.x = r[idx + 0];
+			points[i].pos.y = r[idx + 1];
+			points[i].normal.x = r[idx + 2];
+			points[i].normal.y = r[idx + 3];
 		}
 	}
 
-
-	ERR_FAIL_COND(point_count==0);
+	ERR_FAIL_COND(point_count == 0);
 	Rect2 aabb;
-	aabb.pos=points[0].pos;
-	for(int i=1;i<point_count;i++)
+	aabb.pos = points[0].pos;
+	for (int i = 1; i < point_count; i++)
 		aabb.expand_to(points[i].pos);
 
 	configure(aabb);
@@ -745,408 +687,378 @@ Variant ConvexPolygonShape2DSW::get_data() const {
 
 	dvr.resize(point_count);
 
-	for(int i=0;i<point_count;i++) {
-		dvr.set(i,points[i].pos);
+	for (int i = 0; i < point_count; i++) {
+		dvr.set(i, points[i].pos);
 	}
 
 	return dvr;
 }
 
-
 ConvexPolygonShape2DSW::ConvexPolygonShape2DSW() {
 
-	points=NULL;
-	point_count=0;
-
+	points = NULL;
+	point_count = 0;
 }
 
-ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW(){
+ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() {
 
 	if (points)
 		memdelete_arr(points);
-
 }
 
 //////////////////////////////////////////////////
 
+void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
 
-void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const {
-
-	real_t d=-1e10;
-	int idx=-1;
-	for(int i=0;i<points.size();i++) {
+	real_t d = -1e10;
+	int idx = -1;
+	for (int i = 0; i < points.size(); i++) {
 
 		real_t ld = p_normal.dot(points[i]);
-		if (ld>d) {
-			d=ld;
-			idx=i;
+		if (ld > d) {
+			d = ld;
+			idx = i;
 		}
 	}
 
-
-	r_amount=1;
-	ERR_FAIL_COND(idx==-1);
-	*r_supports=points[idx];
-
+	r_amount = 1;
+	ERR_FAIL_COND(idx == -1);
+	*r_supports = points[idx];
 }
 
-bool ConcavePolygonShape2DSW::contains_point(const Vector2& p_point) const {
+bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const {
 
 	return false; //sorry
 }
 
+bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
 
-bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
-
-	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
 
 	enum {
-		TEST_AABB_BIT=0,
-		VISIT_LEFT_BIT=1,
-		VISIT_RIGHT_BIT=2,
-		VISIT_DONE_BIT=3,
-		VISITED_BIT_SHIFT=29,
-		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
-		VISITED_BIT_MASK=~NODE_IDX_MASK,
-
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
 
 	};
 
-	Vector2 n = (p_end-p_begin).normalized();
-	real_t d=1e10;
-	bool inters=false;
+	Vector2 n = (p_end - p_begin).normalized();
+	real_t d = 1e10;
+	bool inters = false;
 
 	//for(int i=0;i<bvh_depth;i++)
 	//	stack[i]=0;
 
-	int level=0;
+	int level = 0;
 
-	const Segment *segmentptr=&segments[0];
-	const Vector2 *pointptr=&points[0];
+	const Segment *segmentptr = &segments[0];
+	const Vector2 *pointptr = &points[0];
 	const BVH *bvhptr = &bvh[0];
 
+	stack[0] = 0;
+	while (true) {
 
-	stack[0]=0;
-	while(true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &b = bvhptr[node];
+		bool done = false;
 
-		uint32_t node = stack[level]&NODE_IDX_MASK;
-		const BVH &b = bvhptr[ node ];
-		bool done=false;
-
-		switch(stack[level]>>VISITED_BIT_SHIFT) {
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
 			case TEST_AABB_BIT: {
 
-
-				bool valid = b.aabb.intersects_segment(p_begin,p_end);
+				bool valid = b.aabb.intersects_segment(p_begin, p_end);
 				if (!valid) {
 
-					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
 
 				} else {
 
-					if (b.left<0) {
-
-						const Segment &s=segmentptr[ b.right ];
-						Vector2 a = pointptr[ s.points[0] ];
-						Vector2 b = pointptr[ s.points[1] ];
+					if (b.left < 0) {
 
+						const Segment &s = segmentptr[b.right];
+						Vector2 a = pointptr[s.points[0]];
+						Vector2 b = pointptr[s.points[1]];
 
 						Vector2 res;
 
-						if (Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&res)) {
+						if (Geometry::segment_intersects_segment_2d(p_begin, p_end, a, b, &res)) {
 
 							float nd = n.dot(res);
-							if (nd<d) {
+							if (nd < d) {
 
-								d=nd;
-								r_point=res;
-								r_normal=(b-a).tangent().normalized();
-								inters=true;
+								d = nd;
+								r_point = res;
+								r_normal = (b - a).tangent().normalized();
+								inters = true;
 							}
-
 						}
 
-						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
 
 					} else {
 
-						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
 					}
 				}
-
-			} continue;
+			}
+				continue;
 			case VISIT_LEFT_BIT: {
 
-				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.left|TEST_AABB_BIT;
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = b.left | TEST_AABB_BIT;
 				level++;
-
-			} continue;
+			}
+				continue;
 			case VISIT_RIGHT_BIT: {
 
-				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.right|TEST_AABB_BIT;
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = b.right | TEST_AABB_BIT;
 				level++;
-			} continue;
+			}
+				continue;
 			case VISIT_DONE_BIT: {
 
-				if (level==0) {
-					done=true;
+				if (level == 0) {
+					done = true;
 					break;
 				} else
 					level--;
-
-			} continue;
+			}
+				continue;
 		}
 
-
 		if (done)
 			break;
 	}
 
-
 	if (inters) {
 
-		if (n.dot(r_normal)>0)
-			r_normal=-r_normal;
+		if (n.dot(r_normal) > 0)
+			r_normal = -r_normal;
 	}
 
 	return inters;
-
 }
 
+int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
 
+	if (p_len == 1) {
 
-int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh,int p_len,int p_depth) {
-
-	if (p_len==1) {
-
-		bvh_depth=MAX(p_depth,bvh_depth);
+		bvh_depth = MAX(p_depth, bvh_depth);
 		bvh.push_back(*p_bvh);
-		return bvh.size()-1;
+		return bvh.size() - 1;
 	}
 
 	//else sort best
 
-	Rect2 global_aabb=p_bvh[0].aabb;
-	for(int i=1;i<p_len;i++) {
-		global_aabb=global_aabb.merge(p_bvh[i].aabb);
+	Rect2 global_aabb = p_bvh[0].aabb;
+	for (int i = 1; i < p_len; i++) {
+		global_aabb = global_aabb.merge(p_bvh[i].aabb);
 	}
 
 	if (global_aabb.size.x > global_aabb.size.y) {
 
-		SortArray<BVH,BVH_CompareX> sort;
-		sort.sort(p_bvh,p_len);
+		SortArray<BVH, BVH_CompareX> sort;
+		sort.sort(p_bvh, p_len);
 
 	} else {
 
-		SortArray<BVH,BVH_CompareY> sort;
-		sort.sort(p_bvh,p_len);
-
+		SortArray<BVH, BVH_CompareY> sort;
+		sort.sort(p_bvh, p_len);
 	}
 
-	int median = p_len/2;
-
+	int median = p_len / 2;
 
 	BVH node;
-	node.aabb=global_aabb;
+	node.aabb = global_aabb;
 	int node_idx = bvh.size();
 	bvh.push_back(node);
 
-	int l = _generate_bvh(p_bvh,median,p_depth+1);
-	int r = _generate_bvh(&p_bvh[median],p_len-median,p_depth+1);
-	bvh[node_idx].left=l;
-	bvh[node_idx].right=r;
+	int l = _generate_bvh(p_bvh, median, p_depth + 1);
+	int r = _generate_bvh(&p_bvh[median], p_len - median, p_depth + 1);
+	bvh[node_idx].left = l;
+	bvh[node_idx].right = r;
 
 	return node_idx;
-
 }
 
-void ConcavePolygonShape2DSW::set_data(const Variant& p_data) {
+void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
 
-	ERR_FAIL_COND(p_data.get_type()!=Variant::VECTOR2_ARRAY && p_data.get_type()!=Variant::REAL_ARRAY);
+	ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2_ARRAY && p_data.get_type() != Variant::REAL_ARRAY);
 
 	Rect2 aabb;
 
-	if (p_data.get_type()==Variant::VECTOR2_ARRAY) {
+	if (p_data.get_type() == Variant::VECTOR2_ARRAY) {
 
 		DVector<Vector2> p2arr = p_data;
 		int len = p2arr.size();
-		ERR_FAIL_COND(len%2);
+		ERR_FAIL_COND(len % 2);
 
 		segments.clear();
 		points.clear();
 		bvh.clear();
-		bvh_depth=1;
+		bvh_depth = 1;
 
-		if (len==0) {
+		if (len == 0) {
 			configure(aabb);
 			return;
 		}
 
 		DVector<Vector2>::Read arr = p2arr.read();
 
-		Map<Point2,int> pointmap;
-		for(int i=0;i<len;i+=2) {
+		Map<Point2, int> pointmap;
+		for (int i = 0; i < len; i += 2) {
 
-			Point2 p1 =arr[i];
-			Point2 p2 =arr[i+1];
-			int idx_p1,idx_p2;
+			Point2 p1 = arr[i];
+			Point2 p2 = arr[i + 1];
+			int idx_p1, idx_p2;
 
 			if (pointmap.has(p1)) {
-				idx_p1=pointmap[p1];
+				idx_p1 = pointmap[p1];
 			} else {
-				idx_p1=pointmap.size();
-				pointmap[p1]=idx_p1;
+				idx_p1 = pointmap.size();
+				pointmap[p1] = idx_p1;
 			}
 
 			if (pointmap.has(p2)) {
-				idx_p2=pointmap[p2];
+				idx_p2 = pointmap[p2];
 			} else {
-				idx_p2=pointmap.size();
-				pointmap[p2]=idx_p2;
+				idx_p2 = pointmap.size();
+				pointmap[p2] = idx_p2;
 			}
 
 			Segment s;
-			s.points[0]=idx_p1;
-			s.points[1]=idx_p2;
+			s.points[0] = idx_p1;
+			s.points[1] = idx_p2;
 			segments.push_back(s);
 		}
 
 		points.resize(pointmap.size());
-		aabb.pos=pointmap.front()->key();
-		for(Map<Point2,int>::Element *E=pointmap.front();E;E=E->next()) {
+		aabb.pos = pointmap.front()->key();
+		for (Map<Point2, int>::Element *E = pointmap.front(); E; E = E->next()) {
 
 			aabb.expand_to(E->key());
-			points[E->get()]=E->key();
+			points[E->get()] = E->key();
 		}
 
 		Vector<BVH> main_vbh;
 		main_vbh.resize(segments.size());
-		for(int i=0;i<main_vbh.size();i++) {
-
+		for (int i = 0; i < main_vbh.size(); i++) {
 
-			main_vbh[i].aabb.pos=points[segments[i].points[0]];
+			main_vbh[i].aabb.pos = points[segments[i].points[0]];
 			main_vbh[i].aabb.expand_to(points[segments[i].points[1]]);
-			main_vbh[i].left=-1;
-			main_vbh[i].right=i;
+			main_vbh[i].left = -1;
+			main_vbh[i].right = i;
 		}
 
-		_generate_bvh(&main_vbh[0],main_vbh.size(),1);
-
+		_generate_bvh(&main_vbh[0], main_vbh.size(), 1);
 
 	} else {
 		//dictionary with arrays
-
 	}
 
-
 	configure(aabb);
 }
 Variant ConcavePolygonShape2DSW::get_data() const {
 
-
 	DVector<Vector2> rsegments;
 	int len = segments.size();
-	rsegments.resize(len*2);
+	rsegments.resize(len * 2);
 	DVector<Vector2>::Write w = rsegments.write();
-	for(int i=0;i<len;i++) {
+	for (int i = 0; i < len; i++) {
 
-		w[(i<<1)+0]=points[segments[i].points[0]];
-		w[(i<<1)+1]=points[segments[i].points[1]];
+		w[(i << 1) + 0] = points[segments[i].points[0]];
+		w[(i << 1) + 1] = points[segments[i].points[1]];
 	}
 
-	w=DVector<Vector2>::Write();
+	w = DVector<Vector2>::Write();
 
 	return rsegments;
 }
 
-void ConcavePolygonShape2DSW::cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const {
+void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const {
 
-	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
+	uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
 
 	enum {
-		TEST_AABB_BIT=0,
-		VISIT_LEFT_BIT=1,
-		VISIT_RIGHT_BIT=2,
-		VISIT_DONE_BIT=3,
-		VISITED_BIT_SHIFT=29,
-		NODE_IDX_MASK=(1<<VISITED_BIT_SHIFT)-1,
-		VISITED_BIT_MASK=~NODE_IDX_MASK,
-
+		TEST_AABB_BIT = 0,
+		VISIT_LEFT_BIT = 1,
+		VISIT_RIGHT_BIT = 2,
+		VISIT_DONE_BIT = 3,
+		VISITED_BIT_SHIFT = 29,
+		NODE_IDX_MASK = (1 << VISITED_BIT_SHIFT) - 1,
+		VISITED_BIT_MASK = ~NODE_IDX_MASK,
 
 	};
 
 	//for(int i=0;i<bvh_depth;i++)
 	//	stack[i]=0;
 
+	int level = 0;
 
-	int level=0;
-
-	const Segment *segmentptr=&segments[0];
-	const Vector2 *pointptr=&points[0];
+	const Segment *segmentptr = &segments[0];
+	const Vector2 *pointptr = &points[0];
 	const BVH *bvhptr = &bvh[0];
 
+	stack[0] = 0;
+	while (true) {
 
-	stack[0]=0;
-	while(true) {
+		uint32_t node = stack[level] & NODE_IDX_MASK;
+		const BVH &b = bvhptr[node];
 
-		uint32_t node = stack[level]&NODE_IDX_MASK;
-		const BVH &b = bvhptr[ node ];
-
-		switch(stack[level]>>VISITED_BIT_SHIFT) {
+		switch (stack[level] >> VISITED_BIT_SHIFT) {
 			case TEST_AABB_BIT: {
 
-
 				bool valid = p_local_aabb.intersects(b.aabb);
 				if (!valid) {
 
-					stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+					stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
 
 				} else {
 
-					if (b.left<0) {
+					if (b.left < 0) {
 
-						const Segment &s=segmentptr[ b.right ];
-						Vector2 a = pointptr[ s.points[0] ];
-						Vector2 b = pointptr[ s.points[1] ];
+						const Segment &s = segmentptr[b.right];
+						Vector2 a = pointptr[s.points[0]];
+						Vector2 b = pointptr[s.points[1]];
 
-						SegmentShape2DSW ss(a,b,(b-a).tangent().normalized());
+						SegmentShape2DSW ss(a, b, (b - a).tangent().normalized());
 
-						p_callback(p_userdata,&ss);
-						stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
+						p_callback(p_userdata, &ss);
+						stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
 
 					} else {
 
-						stack[level]=(VISIT_LEFT_BIT<<VISITED_BIT_SHIFT)|node;
+						stack[level] = (VISIT_LEFT_BIT << VISITED_BIT_SHIFT) | node;
 					}
 				}
-
-			} continue;
+			}
+				continue;
 			case VISIT_LEFT_BIT: {
 
-				stack[level]=(VISIT_RIGHT_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.left|TEST_AABB_BIT;
+				stack[level] = (VISIT_RIGHT_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = b.left | TEST_AABB_BIT;
 				level++;
-
-			} continue;
+			}
+				continue;
 			case VISIT_RIGHT_BIT: {
 
-				stack[level]=(VISIT_DONE_BIT<<VISITED_BIT_SHIFT)|node;
-				stack[level+1]=b.right|TEST_AABB_BIT;
+				stack[level] = (VISIT_DONE_BIT << VISITED_BIT_SHIFT) | node;
+				stack[level + 1] = b.right | TEST_AABB_BIT;
 				level++;
-			} continue;
+			}
+				continue;
 			case VISIT_DONE_BIT: {
 
-				if (level==0)
+				if (level == 0)
 					return;
 				else
 					level--;
-
-			} continue;
+			}
+				continue;
 		}
 	}
-
 }
-
-
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index 785771663..10c0f9f59 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -48,9 +48,8 @@ class Shape2DSW;
 
 class ShapeOwner2DSW {
 public:
-
-	virtual void _shape_changed()=0;
-	virtual void remove_shape(Shape2DSW *p_shape)=0;
+	virtual void _shape_changed() = 0;
+	virtual void remove_shape(Shape2DSW *p_shape) = 0;
 
 	virtual ~ShapeOwner2DSW() {}
 };
@@ -62,77 +61,75 @@ class Shape2DSW {
 	bool configured;
 	real_t custom_bias;
 
-	Map<ShapeOwner2DSW*,int> owners;
+	Map<ShapeOwner2DSW *, int> owners;
+
 protected:
+	void configure(const Rect2 &p_aabb);
 
-	void configure(const Rect2& p_aabb);
 public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
-	_FORCE_INLINE_ RID get_self() const {return  self; }
-
-	virtual Physics2DServer::ShapeType get_type() const=0;
+	virtual Physics2DServer::ShapeType get_type() const = 0;
 
 	_FORCE_INLINE_ Rect2 get_aabb() const { return aabb; }
 	_FORCE_INLINE_ bool is_configured() const { return configured; }
 
 	virtual bool is_concave() const { return false; }
 
-	virtual bool contains_point(const Vector2& p_point) const=0;
+	virtual bool contains_point(const Vector2 &p_point) const = 0;
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
-	virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
-	virtual Vector2 get_support(const Vector2& p_normal) const;
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const=0;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const = 0;
+	virtual Vector2 get_support(const Vector2 &p_normal) const;
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const = 0;
 
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const=0;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const=0;
-	virtual void set_data(const Variant& p_data)=0;
-	virtual Variant get_data() const=0;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const = 0;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const = 0;
+	virtual void set_data(const Variant &p_data) = 0;
+	virtual Variant get_data() const = 0;
 
-	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
 
 	void add_owner(ShapeOwner2DSW *p_owner);
 	void remove_owner(ShapeOwner2DSW *p_owner);
 	bool is_owner(ShapeOwner2DSW *p_owner) const;
-	const Map<ShapeOwner2DSW*,int>& get_owners() const;
-
+	const Map<ShapeOwner2DSW *, int> &get_owners() const;
 
-	_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2& p_cast,const Vector2& p_normal,const Matrix32& p_xform,Vector2 *r_supports,int & r_amount) const {
+	_FORCE_INLINE_ void get_supports_transformed_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_xform, Vector2 *r_supports, int &r_amount) const {
 
-		get_supports(p_xform.basis_xform_inv(p_normal).normalized(),r_supports,r_amount);
-		for(int i=0;i<r_amount;i++)
-			r_supports[i]=p_xform.xform(r_supports[i]);
+		get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount);
+		for (int i = 0; i < r_amount; i++)
+			r_supports[i] = p_xform.xform(r_supports[i]);
 
-		if (r_amount==1) {
+		if (r_amount == 1) {
 
-			if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+			if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) {
 				//make line because they are parallel
-				r_amount=2;
-				r_supports[1]=r_supports[0]+p_cast;
-			} else if (p_cast.dot(p_normal)>0) {
+				r_amount = 2;
+				r_supports[1] = r_supports[0] + p_cast;
+			} else if (p_cast.dot(p_normal) > 0) {
 				//normal points towards cast, add cast
-				r_supports[0]+=p_cast;
+				r_supports[0] += p_cast;
 			}
 
 		} else {
 
-			if (Math::abs( p_normal.dot(p_cast.normalized()) )<(1.0-_SEGMENT_IS_VALID_SUPPORT_TRESHOLD) ) {
+			if (Math::abs(p_normal.dot(p_cast.normalized())) < (1.0 - _SEGMENT_IS_VALID_SUPPORT_TRESHOLD)) {
 				//optimize line and make it larger because they are parallel
-				if ((r_supports[1]-r_supports[0]).dot(p_cast)>0) {
+				if ((r_supports[1] - r_supports[0]).dot(p_cast) > 0) {
 					//larger towards 1
-					r_supports[1]+=p_cast;
+					r_supports[1] += p_cast;
 				} else {
 					//larger towards 0
-					r_supports[0]+=p_cast;
+					r_supports[0] += p_cast;
 				}
-			} else if (p_cast.dot(p_normal)>0) {
+			} else if (p_cast.dot(p_normal) > 0) {
 				//normal points towards cast, add cast
-				r_supports[0]+=p_cast;
-				r_supports[1]+=p_cast;
+				r_supports[0] += p_cast;
+				r_supports[1] += p_cast;
 			}
-
 		}
 	}
 
@@ -141,305 +138,274 @@ public:
 };
 
 //let the optimizer do the magic
-#define DEFAULT_PROJECT_RANGE_CAST \
-virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
-	project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);\
-}\
-_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {\
-\
-	real_t mina,maxa;\
-	real_t minb,maxb;\
-	Matrix32 ofsb=p_transform;\
-	ofsb.elements[2]+=p_cast;\
-	project_range(p_normal,p_transform,mina,maxa);\
-	project_range(p_normal,ofsb,minb,maxb);	\
-	r_min=MIN(mina,minb);\
-	r_max=MAX(maxa,maxb);\
-}
+#define DEFAULT_PROJECT_RANGE_CAST                                                                                                                            \
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {       \
+		project_range_cast(p_cast, p_normal, p_transform, r_min, r_max);                                                                                      \
+	}                                                                                                                                                         \
+	_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { \
+                                                                                                                                                              \
+		real_t mina, maxa;                                                                                                                                    \
+		real_t minb, maxb;                                                                                                                                    \
+		Matrix32 ofsb = p_transform;                                                                                                                          \
+		ofsb.elements[2] += p_cast;                                                                                                                           \
+		project_range(p_normal, p_transform, mina, maxa);                                                                                                     \
+		project_range(p_normal, ofsb, minb, maxb);                                                                                                            \
+		r_min = MIN(mina, minb);                                                                                                                              \
+		r_max = MAX(maxa, maxb);                                                                                                                              \
+	}
 
 class LineShape2DSW : public Shape2DSW {
 
-
 	Vector2 normal;
 	real_t d;
 
 public:
-
 	_FORCE_INLINE_ Vector2 get_normal() const { return normal; }
 	_FORCE_INLINE_ real_t get_d() const { return d; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_LINE; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		//real large
-		r_min=-1e10;
-		r_max=1e10;
+		r_min = -1e10;
+		r_max = 1e10;
 	}
 
-	virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
-		project_range_cast(p_cast,p_normal,p_transform,r_min,r_max);
+	virtual void project_range_castv(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
+		project_range_cast(p_cast, p_normal, p_transform, r_min, r_max);
 	}
 
-	_FORCE_INLINE_ void project_range_cast(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range_cast(const Vector2 &p_cast, const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		//real large
-		r_min=-1e10;
-		r_max=1e10;
+		r_min = -1e10;
+		r_max = 1e10;
 	}
-
-
-
 };
 
-
 class RayShape2DSW : public Shape2DSW {
 
-
 	real_t length;
 
 public:
-
-
-	_FORCE_INLINE_  real_t get_length() const { return length; }
+	_FORCE_INLINE_ real_t get_length() const { return length; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RAY; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		//real large
 		r_max = p_normal.dot(p_transform.get_origin());
-		r_min = p_normal.dot(p_transform.xform(Vector2(0,length)));
-		if (r_max<r_min) {
+		r_min = p_normal.dot(p_transform.xform(Vector2(0, length)));
+		if (r_max < r_min) {
 
-			SWAP(r_max,r_min);
+			SWAP(r_max, r_min);
 		}
 	}
 
 	DEFAULT_PROJECT_RANGE_CAST
 
-
 	_FORCE_INLINE_ RayShape2DSW() {}
-	_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length=p_length; }
+	_FORCE_INLINE_ RayShape2DSW(real_t p_length) { length = p_length; }
 };
 
-
 class SegmentShape2DSW : public Shape2DSW {
 
-
 	Vector2 a;
 	Vector2 b;
 	Vector2 n;
 
 public:
-
-
-	_FORCE_INLINE_ const Vector2& get_a() const { return a; }
-	_FORCE_INLINE_ const Vector2& get_b() const { return b; }
-	_FORCE_INLINE_ const Vector2& get_normal() const { return n; }
+	_FORCE_INLINE_ const Vector2 &get_a() const { return a; }
+	_FORCE_INLINE_ const Vector2 &get_b() const { return b; }
+	_FORCE_INLINE_ const Vector2 &get_normal() const { return n; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_SEGMENT; }
 
-	_FORCE_INLINE_ Vector2 get_xformed_normal(const Matrix32& p_xform) const {
+	_FORCE_INLINE_ Vector2 get_xformed_normal(const Matrix32 &p_xform) const {
 
 		return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent();
 	}
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		//real large
 		r_max = p_normal.dot(p_transform.xform(a));
 		r_min = p_normal.dot(p_transform.xform(b));
-		if (r_max<r_min) {
+		if (r_max < r_min) {
 
-			SWAP(r_max,r_min);
+			SWAP(r_max, r_min);
 		}
 	}
 
 	DEFAULT_PROJECT_RANGE_CAST
 
 	_FORCE_INLINE_ SegmentShape2DSW() {}
-	_FORCE_INLINE_ SegmentShape2DSW(const Vector2& p_a,const Vector2& p_b,const Vector2& p_n) { a=p_a; b=p_b; n=p_n; }
+	_FORCE_INLINE_ SegmentShape2DSW(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) {
+		a = p_a;
+		b = p_b;
+		n = p_n;
+	}
 };
 
-
 class CircleShape2DSW : public Shape2DSW {
 
-
 	real_t radius;
 
 public:
-
-	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
+	_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CIRCLE; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		//real large
-		real_t d = p_normal.dot( p_transform.get_origin() );
+		real_t d = p_normal.dot(p_transform.get_origin());
 
 		// figure out scale at point
 		Vector2 local_normal = p_transform.basis_xform_inv(p_normal);
 		real_t scale = local_normal.length();
 
-		r_min = d - (radius) * scale;
-		r_max = d + (radius) * scale;
+		r_min = d - (radius)*scale;
+		r_max = d + (radius)*scale;
 	}
 
-
 	DEFAULT_PROJECT_RANGE_CAST
-
 };
 
-
-
 class RectangleShape2DSW : public Shape2DSW {
 
-
 	Vector2 half_extents;
 
 public:
-
-	_FORCE_INLINE_ const Vector2& get_half_extents() const { return half_extents; }
+	_FORCE_INLINE_ const Vector2 &get_half_extents() const { return half_extents; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_RECTANGLE; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		// no matter the angle, the box is mirrored anyway
-		r_max=-1e20;
-		r_min=1e20;
-		for(int i=0;i<4;i++) {
-
-			real_t d=p_normal.dot(p_transform.xform(Vector2( ((i&1)*2-1)*half_extents.x, ((i>>1)*2-1)*half_extents.y )));
+		r_max = -1e20;
+		r_min = 1e20;
+		for (int i = 0; i < 4; i++) {
 
-			if (d>r_max)
-				r_max=d;
-			if (d<r_min)
-				r_min=d;
+			real_t d = p_normal.dot(p_transform.xform(Vector2(((i & 1) * 2 - 1) * half_extents.x, ((i >> 1) * 2 - 1) * half_extents.y)));
 
+			if (d > r_max)
+				r_max = d;
+			if (d < r_min)
+				r_min = d;
 		}
 	}
 
-
-
-	_FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const Vector2& p_circle) const {
+	_FORCE_INLINE_ Vector2 get_circle_axis(const Matrix32 &p_xform, const Matrix32 &p_xform_inv, const Vector2 &p_circle) const {
 
 		Vector2 local_v = p_xform_inv.xform(p_circle);
 
 		Vector2 he(
-			(local_v.x<0) ? -half_extents.x : half_extents.x,
-			(local_v.y<0) ? -half_extents.y : half_extents.y
-		);
+				(local_v.x < 0) ? -half_extents.x : half_extents.x,
+				(local_v.y < 0) ? -half_extents.y : half_extents.y);
 
-		return (p_xform.xform(he)-p_circle).normalized();
+		return (p_xform.xform(he) - p_circle).normalized();
 	}
 
-	_FORCE_INLINE_ Vector2 get_box_axis(const Matrix32& p_xform, const Matrix32& p_xform_inv,const RectangleShape2DSW *p_B,const Matrix32& p_B_xform, const Matrix32& p_B_xform_inv) const {
+	_FORCE_INLINE_ Vector2 get_box_axis(const Matrix32 &p_xform, const Matrix32 &p_xform_inv, const RectangleShape2DSW *p_B, const Matrix32 &p_B_xform, const Matrix32 &p_B_xform_inv) const {
 
-		Vector2 a,b;
+		Vector2 a, b;
 
 		{
 			Vector2 local_v = p_xform_inv.xform(p_B_xform.get_origin());
 
 			Vector2 he(
-				(local_v.x<0) ? -half_extents.x : half_extents.x,
-				(local_v.y<0) ? -half_extents.y : half_extents.y
-			);
-
-			a=p_xform.xform(he);
+					(local_v.x < 0) ? -half_extents.x : half_extents.x,
+					(local_v.y < 0) ? -half_extents.y : half_extents.y);
 
+			a = p_xform.xform(he);
 		}
 		{
 			Vector2 local_v = p_B_xform_inv.xform(p_xform.get_origin());
 
 			Vector2 he(
-				(local_v.x<0) ? -p_B->half_extents.x : p_B->half_extents.x,
-				(local_v.y<0) ? -p_B->half_extents.y : p_B->half_extents.y
-			);
-
-			b=p_B_xform.xform(he);
+					(local_v.x < 0) ? -p_B->half_extents.x : p_B->half_extents.x,
+					(local_v.y < 0) ? -p_B->half_extents.y : p_B->half_extents.y);
 
+			b = p_B_xform.xform(he);
 		}
 
-		return (a-b).normalized();
+		return (a - b).normalized();
 	}
 
-
 	DEFAULT_PROJECT_RANGE_CAST
-
 };
 
 class CapsuleShape2DSW : public Shape2DSW {
 
-
 	real_t radius;
 	real_t height;
 
 public:
-
-	_FORCE_INLINE_ const real_t& get_radius() const { return radius; }
-	_FORCE_INLINE_ const real_t& get_height() const { return height; }
+	_FORCE_INLINE_ const real_t &get_radius() const { return radius; }
+	_FORCE_INLINE_ const real_t &get_height() const { return height; }
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CAPSULE; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		// no matter the angle, the box is mirrored anyway
-		Vector2 n=p_transform.basis_xform_inv(p_normal).normalized();
+		Vector2 n = p_transform.basis_xform_inv(p_normal).normalized();
 		float h = (n.y > 0) ? height : -height;
 
 		n *= radius;
@@ -448,24 +414,19 @@ public:
 		r_max = p_normal.dot(p_transform.xform(n));
 		r_min = p_normal.dot(p_transform.xform(-n));
 
-		if (r_max<r_min) {
+		if (r_max < r_min) {
 
-			SWAP(r_max,r_min);
+			SWAP(r_max, r_min);
 		}
 
 		//ERR_FAIL_COND( r_max < r_min );
 	}
 
 	DEFAULT_PROJECT_RANGE_CAST
-
 };
 
-
-
-
 class ConvexPolygonShape2DSW : public Shape2DSW {
 
-
 	struct Point {
 
 		Vector2 pos;
@@ -476,44 +437,41 @@ class ConvexPolygonShape2DSW : public Shape2DSW {
 	int point_count;
 
 public:
-
 	_FORCE_INLINE_ int get_point_count() const { return point_count; }
-	_FORCE_INLINE_ const Vector2& get_point(int p_idx) const { return points[p_idx].pos; }
-	_FORCE_INLINE_ const Vector2& get_segment_normal(int p_idx) const { return points[p_idx].normal; }
-	_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Matrix32& p_xform, int p_idx) const {
+	_FORCE_INLINE_ const Vector2 &get_point(int p_idx) const { return points[p_idx].pos; }
+	_FORCE_INLINE_ const Vector2 &get_segment_normal(int p_idx) const { return points[p_idx].normal; }
+	_FORCE_INLINE_ Vector2 get_xformed_segment_normal(const Matrix32 &p_xform, int p_idx) const {
 
 		Vector2 a = points[p_idx].pos;
 		p_idx++;
-		Vector2 b = points[p_idx==point_count?0:p_idx].pos;
-		return (p_xform.xform(b)-p_xform.xform(a)).normalized().tangent();
+		Vector2 b = points[p_idx == point_count ? 0 : p_idx].pos;
+		return (p_xform.xform(b) - p_xform.xform(a)).normalized().tangent();
 	}
 
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONVEX_POLYGON; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal, p_transform, r_min, r_max); }
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const;
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	_FORCE_INLINE_ void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const {
+	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const {
 		// no matter the angle, the box is mirrored anyway
 
 		r_min = r_max = p_normal.dot(p_transform.xform(points[0].pos));
-		for(int i=1;i<point_count;i++) {
+		for (int i = 1; i < point_count; i++) {
 
 			float d = p_normal.dot(p_transform.xform(points[i].pos));
-			if (d>r_max)
-				r_max=d;
-			if (d<r_min)
-				r_min=d;
-
+			if (d > r_max)
+				r_max = d;
+			if (d < r_min)
+				r_min = d;
 		}
-
 	}
 
 	DEFAULT_PROJECT_RANGE_CAST
@@ -522,16 +480,13 @@ public:
 	~ConvexPolygonShape2DSW();
 };
 
-
 class ConcaveShape2DSW : public Shape2DSW {
 
 public:
-
 	virtual bool is_concave() const { return true; }
-	typedef void (*Callback)(void* p_userdata,Shape2DSW *p_convex);
-
-	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+	typedef void (*Callback)(void *p_userdata, Shape2DSW *p_convex);
 
+	virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
 };
 
 class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
@@ -547,53 +502,50 @@ class ConcavePolygonShape2DSW : public ConcaveShape2DSW {
 	struct BVH {
 
 		Rect2 aabb;
-		int left,right;
+		int left, right;
 	};
 
-
 	Vector<BVH> bvh;
 	int bvh_depth;
 
-
 	struct BVH_CompareX {
 
-		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+		_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
 
-			return (a.aabb.pos.x+a.aabb.size.x*0.5) < (b.aabb.pos.x+b.aabb.size.x*0.5);
+			return (a.aabb.pos.x + a.aabb.size.x * 0.5) < (b.aabb.pos.x + b.aabb.size.x * 0.5);
 		}
 	};
 
 	struct BVH_CompareY {
 
-		_FORCE_INLINE_ bool operator ()(const BVH& a, const BVH& b) const {
+		_FORCE_INLINE_ bool operator()(const BVH &a, const BVH &b) const {
 
-			return (a.aabb.pos.y+a.aabb.size.y*0.5) < (b.aabb.pos.y+b.aabb.size.y*0.5);
+			return (a.aabb.pos.y + a.aabb.size.y * 0.5) < (b.aabb.pos.y + b.aabb.size.y * 0.5);
 		}
 	};
 
-	int _generate_bvh(BVH *p_bvh,int p_len,int p_depth);
+	int _generate_bvh(BVH *p_bvh, int p_len, int p_depth);
 
 public:
-
 	virtual Physics2DServer::ShapeType get_type() const { return Physics2DServer::SHAPE_CONCAVE_POLYGON; }
 
-	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
-	virtual void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
-	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
+	virtual void project_rangev(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/
+	}
+	virtual void project_range(const Vector2 &p_normal, const Matrix32 &p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/
+	}
+	virtual void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const;
 
-	virtual bool contains_point(const Vector2& p_point) const;
-	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
+	virtual bool contains_point(const Vector2 &p_point) const;
+	virtual bool intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const;
 
-	virtual real_t get_moment_of_inertia(float p_mass,const Size2& p_scale) const { return 0; }
+	virtual real_t get_moment_of_inertia(float p_mass, const Size2 &p_scale) const { return 0; }
 
-	virtual void set_data(const Variant& p_data);
+	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 
-	virtual void cull(const Rect2& p_local_aabb,Callback p_callback,void* p_userdata) const;
-
+	virtual void cull(const Rect2 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	DEFAULT_PROJECT_RANGE_CAST
-
 };
 
 #undef DEFAULT_PROJECT_RANGE_CAST
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 8a08e8160..6a40c24e5 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -30,109 +30,101 @@
 #include "collision_solver_2d_sw.h"
 #include "physics_2d_server_sw.h"
 
-
 _FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) {
 
-	if ((p_object->get_layer_mask()&p_layer_mask)==0)
+	if ((p_object->get_layer_mask() & p_layer_mask) == 0)
 		return false;
 
-	if (p_object->get_type()==CollisionObject2DSW::TYPE_AREA)
-		return p_type_mask&Physics2DDirectSpaceState::TYPE_MASK_AREA;
-
-	Body2DSW *body = static_cast<Body2DSW*>(p_object);
+	if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA)
+		return p_type_mask & Physics2DDirectSpaceState::TYPE_MASK_AREA;
 
-	return (1<<body->get_mode())&p_type_mask;
+	Body2DSW *body = static_cast<Body2DSW *>(p_object);
 
+	return (1 << body->get_mode()) & p_type_mask;
 }
 
+int Physics2DDirectSpaceStateSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask, bool p_pick_point) {
 
-int Physics2DDirectSpaceStateSW::intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask,bool p_pick_point) {
-
-	if (p_result_max<=0)
+	if (p_result_max <= 0)
 		return 0;
 
 	Rect2 aabb;
-	aabb.pos=p_point-Vector2(0.00001,0.00001);
-	aabb.size=Vector2(0.00002,0.00002);
+	aabb.pos = p_point - Vector2(0.00001, 0.00001);
+	aabb.size = Vector2(0.00002, 0.00002);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	int cc=0;
+	int cc = 0;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue;
 
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
 
 		if (p_pick_point && !col_obj->is_pickable())
 			continue;
 
-		int shape_idx=space->intersection_query_subindex_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		Shape2DSW * shape = col_obj->get_shape(shape_idx);
+		Shape2DSW *shape = col_obj->get_shape(shape_idx);
 
 		Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
 
 		if (!shape->contains_point(local_point))
 			continue;
 
-		if (cc>=p_result_max)
+		if (cc >= p_result_max)
 			continue;
 
-		r_results[cc].collider_id=col_obj->get_instance_id();
-		if (r_results[cc].collider_id!=0)
-			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
-		r_results[cc].rid=col_obj->get_self();
-		r_results[cc].shape=shape_idx;
-		r_results[cc].metadata=col_obj->get_shape_metadata(shape_idx);
+		r_results[cc].collider_id = col_obj->get_instance_id();
+		if (r_results[cc].collider_id != 0)
+			r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+		r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
 
 		cc++;
 	}
 
 	return cc;
-
-
 }
 
-bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
-
+bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-	ERR_FAIL_COND_V(space->locked,false);
+	ERR_FAIL_COND_V(space->locked, false);
 
-	Vector2 begin,end;
+	Vector2 begin, end;
 	Vector2 normal;
-	begin=p_from;
-	end=p_to;
-	normal=(end-begin).normalized();
+	begin = p_from;
+	end = p_to;
+	normal = (end - begin).normalized();
 
-	int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
 
-	bool collided=false;
-	Vector2 res_point,res_normal;
+	bool collided = false;
+	Vector2 res_point, res_normal;
 	int res_shape;
 	const CollisionObject2DSW *res_obj;
-	real_t min_d=1e10;
-
+	real_t min_d = 1e10;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue;
 
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
 
-		int shape_idx=space->intersection_query_subindex_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 		Matrix32 inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
 
 		Vector2 local_from = inv_xform.xform(begin);
@@ -146,129 +138,111 @@ bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vec
 
 		const Shape2DSW *shape = col_obj->get_shape(shape_idx);
 
-		Vector2 shape_point,shape_normal;
-
-
-		if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
-
+		Vector2 shape_point, shape_normal;
 
+		if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
 
 			Matrix32 xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
-			shape_point=xform.xform(shape_point);
+			shape_point = xform.xform(shape_point);
 
 			real_t ld = normal.dot(shape_point);
 
+			if (ld < min_d) {
 
-			if (ld<min_d) {
-
-				min_d=ld;
-				res_point=shape_point;
-				res_normal=inv_xform.basis_xform_inv(shape_normal).normalized();
-				res_shape=shape_idx;
-				res_obj=col_obj;
-				collided=true;
+				min_d = ld;
+				res_point = shape_point;
+				res_normal = inv_xform.basis_xform_inv(shape_normal).normalized();
+				res_shape = shape_idx;
+				res_obj = col_obj;
+				collided = true;
 			}
 		}
-
 	}
 
 	if (!collided)
 		return false;
 
-
-	r_result.collider_id=res_obj->get_instance_id();
-	if (r_result.collider_id!=0)
-		r_result.collider=ObjectDB::get_instance(r_result.collider_id);
-	r_result.normal=res_normal;
-	r_result.metadata=res_obj->get_shape_metadata(res_shape);
-	r_result.position=res_point;
-	r_result.rid=res_obj->get_self();
-	r_result.shape=res_shape;
+	r_result.collider_id = res_obj->get_instance_id();
+	if (r_result.collider_id != 0)
+		r_result.collider = ObjectDB::get_instance(r_result.collider_id);
+	r_result.normal = res_normal;
+	r_result.metadata = res_obj->get_shape_metadata(res_shape);
+	r_result.position = res_point;
+	r_result.rid = res_obj->get_self();
+	r_result.shape = res_shape;
 
 	return true;
-
 }
 
+int Physics2DDirectSpaceStateSW::intersect_shape(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-int Physics2DDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
-	if (p_result_max<=0)
+	if (p_result_max <= 0)
 		return 0;
 
 	Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	Rect2 aabb = p_xform.xform(shape->get_aabb());
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.grow(p_margin);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,p_result_max,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, p_result_max, space->intersection_query_subindex_results);
 
-	int cc=0;
+	int cc = 0;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue;
 
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
-
-		if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),NULL,NULL,NULL,p_margin))
+		if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), NULL, NULL, NULL, p_margin))
 			continue;
 
-		r_results[cc].collider_id=col_obj->get_instance_id();
-		if (r_results[cc].collider_id!=0)
-			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
-		r_results[cc].rid=col_obj->get_self();
-		r_results[cc].shape=shape_idx;
-		r_results[cc].metadata=col_obj->get_shape_metadata(shape_idx);
+		r_results[cc].collider_id = col_obj->get_instance_id();
+		if (r_results[cc].collider_id != 0)
+			r_results[cc].collider = ObjectDB::get_instance(r_results[cc].collider_id);
+		r_results[cc].rid = col_obj->get_self();
+		r_results[cc].shape = shape_idx;
+		r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
 
 		cc++;
-
 	}
 
 	return cc;
-
 }
 
-
-
-bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
-
+bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
 	Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,false);
+	ERR_FAIL_COND_V(!shape, false);
 
 	Rect2 aabb = p_xform.xform(shape->get_aabb());
-	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion
+	aabb = aabb.grow(p_margin);
 
 	//if (p_motion!=Vector2())
 	//	print_line(p_motion);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	float best_safe=1;
-	float best_unsafe=1;
+	float best_safe = 1;
+	float best_unsafe = 1;
 
-	for(int i=0;i<amount;i++) {
+	for (int i = 0; i < amount; i++) {
 
-
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+		if (p_exclude.has(space->intersection_query_results[i]->get_self()))
 			continue; //ignore excluded
 
-
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
-
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
 		/*if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
 
@@ -279,21 +253,19 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32
 			}
 		}*/
 
-
 		Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		//test initial overlap, does it collide if going all the way?
-		if (!CollisionSolver2DSW::solve(shape,p_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+		if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) {
 			continue;
 		}
 
-
 		//test initial overlap
-		if (CollisionSolver2DSW::solve(shape,p_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,p_margin)) {
+		if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, p_margin)) {
 
-			if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+			if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 				//if one way collision direction ignore initial overlap
-				const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-				if (body->get_one_way_collision_direction()!=Vector2()) {
+				const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+				if (body->get_one_way_collision_direction() != Vector2()) {
 					continue;
 				}
 			}
@@ -301,131 +273,119 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID& p_shape, const Matrix32
 			return false;
 		}
 
-
 		//just do kinematic solving
-		float low=0;
-		float hi=1;
-		Vector2 mnormal=p_motion.normalized();
+		float low = 0;
+		float hi = 1;
+		Vector2 mnormal = p_motion.normalized();
 
-		for(int i=0;i<8;i++) { //steps should be customizable..
+		for (int i = 0; i < 8; i++) { //steps should be customizable..
 
-			float ofs = (low+hi)*0.5;
+			float ofs = (low + hi) * 0.5;
 
-			Vector2 sep=mnormal; //important optimization for this to work fast enough
-			bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,p_margin);
+			Vector2 sep = mnormal; //important optimization for this to work fast enough
+			bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, p_margin);
 
 			if (collided) {
 
-				hi=ofs;
+				hi = ofs;
 			} else {
 
-				low=ofs;
+				low = ofs;
 			}
 		}
 
-		if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+		if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-			const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-			if (body->get_one_way_collision_direction()!=Vector2()) {
+			const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+			if (body->get_one_way_collision_direction() != Vector2()) {
 
 				Vector2 cd[2];
 				Physics2DServerSW::CollCbkData cbk;
-				cbk.max=1;
-				cbk.amount=0;
-				cbk.ptr=cd;
-				cbk.valid_dir=body->get_one_way_collision_direction();
-				cbk.valid_depth=body->get_one_way_collision_max_depth();
+				cbk.max = 1;
+				cbk.amount = 0;
+				cbk.ptr = cd;
+				cbk.valid_dir = body->get_one_way_collision_direction();
+				cbk.valid_depth = body->get_one_way_collision_max_depth();
 
-				Vector2 sep=mnormal; //important optimization for this to work fast enough
-				bool collided = CollisionSolver2DSW::solve(shape,p_xform,p_motion*(hi+space->contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,p_margin);
-				if (!collided || cbk.amount==0) {
+				Vector2 sep = mnormal; //important optimization for this to work fast enough
+				bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * (hi + space->contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, p_margin);
+				if (!collided || cbk.amount == 0) {
 					continue;
 				}
-
 			}
 		}
 
-
-		if (low<best_safe) {
-			best_safe=low;
-			best_unsafe=hi;
+		if (low < best_safe) {
+			best_safe = low;
+			best_unsafe = hi;
 		}
-
 	}
 
-	p_closest_safe=best_safe;
-	p_closest_unsafe=best_unsafe;
+	p_closest_safe = best_safe;
+	p_closest_unsafe = best_unsafe;
 
 	return true;
-
-
 }
 
+bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
-bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
-
-	if (p_result_max<=0)
+	if (p_result_max <= 0)
 		return 0;
 
 	Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion
+	aabb = aabb.grow(p_margin);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
-	bool collided=false;
-	r_result_count=0;
+	bool collided = false;
+	r_result_count = 0;
 
 	Physics2DServerSW::CollCbkData cbk;
-	cbk.max=p_result_max;
-	cbk.amount=0;
-	cbk.ptr=r_results;
-	CollisionSolver2DSW::CallbackResult cbkres=NULL;
+	cbk.max = p_result_max;
+	cbk.amount = 0;
+	cbk.ptr = r_results;
+	CollisionSolver2DSW::CallbackResult cbkres = NULL;
 
-	Physics2DServerSW::CollCbkData *cbkptr=NULL;
-	if (p_result_max>0) {
-		cbkptr=&cbk;
-		cbkres=Physics2DServerSW::_shape_col_cbk;
+	Physics2DServerSW::CollCbkData *cbkptr = NULL;
+	if (p_result_max > 0) {
+		cbkptr = &cbk;
+		cbkres = Physics2DServerSW::_shape_col_cbk;
 	}
 
+	for (int i = 0; i < amount; i++) {
 
-	for(int i=0;i<amount;i++) {
-
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (p_exclude.has( col_obj->get_self() ))
+		if (p_exclude.has(col_obj->get_self()))
 			continue;
-		if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+		if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-			const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-			cbk.valid_dir=body->get_one_way_collision_direction();
-			cbk.valid_depth=body->get_one_way_collision_max_depth();
+			const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+			cbk.valid_dir = body->get_one_way_collision_direction();
+			cbk.valid_depth = body->get_one_way_collision_max_depth();
 		} else {
-			cbk.valid_dir=Vector2();
-			cbk.valid_depth=0;
+			cbk.valid_dir = Vector2();
+			cbk.valid_depth = 0;
 		}
 
-		if (CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
-			collided=p_result_max==0 || cbk.amount>0;
+		if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
+			collided = p_result_max == 0 || cbk.amount > 0;
 		}
-
 	}
 
-
-	r_result_count=cbk.amount;
+	r_result_count = cbk.amount;
 
 	return collided;
 }
 
-
 struct _RestCallbackData2D {
 
 	const CollisionObject2DSW *object;
@@ -439,20 +399,18 @@ struct _RestCallbackData2D {
 	float valid_depth;
 };
 
-static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,void *p_userdata) {
-
+static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
 
-	_RestCallbackData2D *rd=(_RestCallbackData2D*)p_userdata;
+	_RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata;
 
-	if (rd->valid_dir!=Vector2()) {
+	if (rd->valid_dir != Vector2()) {
 
-		if (rd->valid_dir!=Vector2()) {
-			if (p_point_A.distance_squared_to(p_point_B)>rd->valid_depth*rd->valid_depth)
+		if (rd->valid_dir != Vector2()) {
+			if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth)
 				return;
-			if (rd->valid_dir.dot((p_point_A-p_point_B).normalized())<Math_PI*0.25)
+			if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25)
 				return;
 		}
-
 	}
 
 	Vector2 contact_rel = p_point_B - p_point_A;
@@ -460,139 +418,121 @@ static void _rest_cbk_result(const Vector2& p_point_A,const Vector2& p_point_B,v
 	if (len <= rd->best_len)
 		return;
 
-
-	rd->best_len=len;
-	rd->best_contact=p_point_B;
-	rd->best_normal=contact_rel/len;
-	rd->best_object=rd->object;
-	rd->best_shape=rd->shape;
-
-
+	rd->best_len = len;
+	rd->best_contact = p_point_B;
+	rd->best_normal = contact_rel / len;
+	rd->best_object = rd->object;
+	rd->best_shape = rd->shape;
 }
 
-
-bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
-
+bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_layer_mask, uint32_t p_object_type_mask) {
 
 	Shape2DSW *shape = Physics2DServerSW::singletonsw->shape_owner.get(p_shape);
-	ERR_FAIL_COND_V(!shape,0);
+	ERR_FAIL_COND_V(!shape, 0);
 
 	Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb=aabb.merge(Rect2(aabb.pos+p_motion,aabb.size)); //motion
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.merge(Rect2(aabb.pos + p_motion, aabb.size)); //motion
+	aabb = aabb.grow(p_margin);
 
-	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	_RestCallbackData2D rcd;
-	rcd.best_len=0;
-	rcd.best_object=NULL;
-	rcd.best_shape=0;
-
-	for(int i=0;i<amount;i++) {
+	rcd.best_len = 0;
+	rcd.best_object = NULL;
+	rcd.best_shape = 0;
 
+	for (int i = 0; i < amount; i++) {
 
-		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+		if (!_match_object_type_query(space->intersection_query_results[i], p_layer_mask, p_object_type_mask))
 			continue;
 
-		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
-		int shape_idx=space->intersection_query_subindex_results[i];
+		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
+		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (p_exclude.has( col_obj->get_self() ))
+		if (p_exclude.has(col_obj->get_self()))
 			continue;
 
-		if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+		if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-			const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-			rcd.valid_dir=body->get_one_way_collision_direction();
-			rcd.valid_depth=body->get_one_way_collision_max_depth();
+			const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+			rcd.valid_dir = body->get_one_way_collision_direction();
+			rcd.valid_depth = body->get_one_way_collision_max_depth();
 		} else {
-			rcd.valid_dir=Vector2();
-			rcd.valid_depth=0;
+			rcd.valid_dir = Vector2();
+			rcd.valid_depth = 0;
 		}
 
-
-		rcd.object=col_obj;
-		rcd.shape=shape_idx;
-		bool sc = CollisionSolver2DSW::solve(shape,p_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
+		rcd.object = col_obj;
+		rcd.shape = shape_idx;
+		bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
 		if (!sc)
 			continue;
-
-
 	}
 
-	if (rcd.best_len==0)
+	if (rcd.best_len == 0)
 		return false;
 
-	r_info->collider_id=rcd.best_object->get_instance_id();
-	r_info->shape=rcd.best_shape;
-	r_info->normal=rcd.best_normal;
-	r_info->point=rcd.best_contact;
-	r_info->rid=rcd.best_object->get_self();
-	r_info->metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
-	if (rcd.best_object->get_type()==CollisionObject2DSW::TYPE_BODY) {
+	r_info->collider_id = rcd.best_object->get_instance_id();
+	r_info->shape = rcd.best_shape;
+	r_info->normal = rcd.best_normal;
+	r_info->point = rcd.best_contact;
+	r_info->rid = rcd.best_object->get_self();
+	r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
+	if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-		const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
-		Vector2 rel_vec = r_info->point-body->get_transform().get_origin();
+		const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
+		Vector2 rel_vec = r_info->point - body->get_transform().get_origin();
 		r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
 	} else {
-		r_info->linear_velocity=Vector2();
+		r_info->linear_velocity = Vector2();
 	}
 
 	return true;
 }
 
-
 Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
 
-
-	space=NULL;
+	space = NULL;
 }
 
-
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
 
+	int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
 
+	for (int i = 0; i < amount; i++) {
 
-int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) {
-
-
-	int amount = broadphase->cull_aabb(p_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
-
-	for(int i=0;i<amount;i++) {
+		bool keep = true;
 
-		bool keep=true;
-
-		if (intersection_query_results[i]==p_body)
-			keep=false;
-		else if (intersection_query_results[i]->get_type()==CollisionObject2DSW::TYPE_AREA)
-			keep=false;
-		else if ((static_cast<Body2DSW*>(intersection_query_results[i])->test_collision_mask(p_body))==0)
-			keep=false;
-		else if (static_cast<Body2DSW*>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
-			keep=false;
-		else if (static_cast<Body2DSW*>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i]))
-			keep=false;
+		if (intersection_query_results[i] == p_body)
+			keep = false;
+		else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA)
+			keep = false;
+		else if ((static_cast<Body2DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0)
+			keep = false;
+		else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self()))
+			keep = false;
+		else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_trigger(intersection_query_subindex_results[i]))
+			keep = false;
 
 		if (!keep) {
 
-			if (i<amount-1) {
-				SWAP(intersection_query_results[i],intersection_query_results[amount-1]);
-				SWAP(intersection_query_subindex_results[i],intersection_query_subindex_results[amount-1]);
-
+			if (i < amount - 1) {
+				SWAP(intersection_query_results[i], intersection_query_results[amount - 1]);
+				SWAP(intersection_query_subindex_results[i], intersection_query_subindex_results[amount - 1]);
 			}
 
 			amount--;
 			i--;
-
 		}
 	}
 
 	return amount;
 }
 
-bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result) {
+bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin, Physics2DServer::MotionResult *r_result) {
 
 	//give me back regular physics engine logic
 	//this is madness
@@ -602,24 +542,23 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 	//but is it right? who knows at this point..
 
 	if (r_result) {
-		r_result->collider_id=0;
-		r_result->collider_shape=0;
-
+		r_result->collider_id = 0;
+		r_result->collider_shape = 0;
 	}
 	Rect2 body_aabb;
 
-	for(int i=0;i<p_body->get_shape_count();i++) {
+	for (int i = 0; i < p_body->get_shape_count(); i++) {
 
-		if (i==0)
-			body_aabb=p_body->get_shape_aabb(i);
+		if (i == 0)
+			body_aabb = p_body->get_shape_aabb(i);
 		else
-			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
+			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
 	}
 
 	// Undo the currently transform the physics server is aware of and apply the provided one
-	body_aabb=p_from.xform(p_body->get_inv_transform().xform(body_aabb));
+	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 
-	body_aabb=body_aabb.grow(p_margin);
+	body_aabb = body_aabb.grow(p_margin);
 
 	Matrix32 body_transform = p_from;
 
@@ -627,69 +566,67 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 		//STEP 1, FREE BODY IF STUCK
 
 		const int max_results = 32;
-		int recover_attempts=4;
-		Vector2 sr[max_results*2];
+		int recover_attempts = 4;
+		Vector2 sr[max_results * 2];
 
 		do {
 
 			Physics2DServerSW::CollCbkData cbk;
-			cbk.max=max_results;
-			cbk.amount=0;
-			cbk.ptr=sr;
+			cbk.max = max_results;
+			cbk.amount = 0;
+			cbk.ptr = sr;
 
+			CollisionSolver2DSW::CallbackResult cbkres = NULL;
 
-			CollisionSolver2DSW::CallbackResult cbkres=NULL;
+			Physics2DServerSW::CollCbkData *cbkptr = NULL;
+			cbkptr = &cbk;
+			cbkres = Physics2DServerSW::_shape_col_cbk;
 
-			Physics2DServerSW::CollCbkData *cbkptr=NULL;
-			cbkptr=&cbk;
-			cbkres=Physics2DServerSW::_shape_col_cbk;
+			bool collided = false;
 
-			bool collided=false;
+			int amount = _cull_aabb_for_body(p_body, body_aabb);
 
-			int amount = _cull_aabb_for_body(p_body,body_aabb);
-
-			for(int j=0;j<p_body->get_shape_count();j++) {
+			for (int j = 0; j < p_body->get_shape_count(); j++) {
 				if (p_body->is_shape_set_as_trigger(j))
 					continue;
 
 				Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
 				Shape2DSW *body_shape = p_body->get_shape(j);
-				for(int i=0;i<amount;i++) {
+				for (int i = 0; i < amount; i++) {
 
-					const CollisionObject2DSW *col_obj=intersection_query_results[i];
-					int shape_idx=intersection_query_subindex_results[i];
+					const CollisionObject2DSW *col_obj = intersection_query_results[i];
+					int shape_idx = intersection_query_subindex_results[i];
 
-					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+					if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
+						const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
 
 						Vector2 cdir = body->get_one_way_collision_direction();
 						//if (cdir!=Vector2() && p_motion.dot(cdir)<0)
 						//	continue;
 
-						cbk.valid_dir=cdir;
-						cbk.valid_depth=body->get_one_way_collision_max_depth();
+						cbk.valid_dir = cdir;
+						cbk.valid_depth = body->get_one_way_collision_max_depth();
 					} else {
-						cbk.valid_dir=Vector2();
-						cbk.valid_depth=0;
+						cbk.valid_dir = Vector2();
+						cbk.valid_depth = 0;
 					}
 
-					if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2(),cbkres,cbkptr,NULL,p_margin)) {
-						collided=cbk.amount>0;
+					if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
+						collided = cbk.amount > 0;
 					}
 				}
 			}
 
-
 			if (!collided)
 				break;
 
 			Vector2 recover_motion;
 
-			for(int i=0;i<cbk.amount;i++) {
+			for (int i = 0; i < cbk.amount; i++) {
 
-				Vector2 a = sr[i*2+0];
-				Vector2 b = sr[i*2+1];
+				Vector2 a = sr[i * 2 + 0];
+				Vector2 b = sr[i * 2 + 1];
 
 #if 0
 				Vector2 rel = b-a;
@@ -702,42 +639,40 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 				a+=n*traveled;
 
 #endif
-			//	float d = a.distance_to(b);
+				//	float d = a.distance_to(b);
 
 				//if (d<margin)
 				///	continue;
-				recover_motion+=(b-a)*0.4;
+				recover_motion += (b - a) * 0.4;
 			}
 
-			if (recover_motion==Vector2()) {
-				collided=false;
+			if (recover_motion == Vector2()) {
+				collided = false;
 				break;
 			}
 
-			body_transform.elements[2]+=recover_motion;
-			body_aabb.pos+=recover_motion;
+			body_transform.elements[2] += recover_motion;
+			body_aabb.pos += recover_motion;
 
 			recover_attempts--;
 
 		} while (recover_attempts);
 	}
 
-
-
 	float safe = 1.0;
 	float unsafe = 1.0;
-	int best_shape=-1;
+	int best_shape = -1;
 
 	{
 		// STEP 2 ATTEMPT MOTION
 
-		Rect2 motion_aabb=body_aabb;
-		motion_aabb.pos+=p_motion;
-		motion_aabb=motion_aabb.merge(body_aabb);
+		Rect2 motion_aabb = body_aabb;
+		motion_aabb.pos += p_motion;
+		motion_aabb = motion_aabb.merge(body_aabb);
 
-		int amount = _cull_aabb_for_body(p_body,motion_aabb);
+		int amount = _cull_aabb_for_body(p_body, motion_aabb);
 
-		for(int j=0;j<p_body->get_shape_count();j++) {
+		for (int j = 0; j < p_body->get_shape_count(); j++) {
 
 			if (p_body->is_shape_set_as_trigger(j))
 				continue;
@@ -745,201 +680,189 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 			Matrix32 body_shape_xform = body_transform * p_body->get_shape_transform(j);
 			Shape2DSW *body_shape = p_body->get_shape(j);
 
-			bool stuck=false;
+			bool stuck = false;
 
-			float best_safe=1;
-			float best_unsafe=1;
+			float best_safe = 1;
+			float best_unsafe = 1;
 
-			for(int i=0;i<amount;i++) {
-
-				const CollisionObject2DSW *col_obj=intersection_query_results[i];
-				int shape_idx=intersection_query_subindex_results[i];
+			for (int i = 0; i < amount; i++) {
 
+				const CollisionObject2DSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
 
 				Matrix32 col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 				//test initial overlap, does it collide if going all the way?
-				if (!CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion,col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+				if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
 					continue;
 				}
 
-
 				//test initial overlap
-				if (CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj_xform,Vector2() ,NULL,NULL,NULL,0)) {
+				if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
 
-					if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+					if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 						//if one way collision direction ignore initial overlap
-						const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-						if (body->get_one_way_collision_direction()!=Vector2()) {
+						const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+						if (body->get_one_way_collision_direction() != Vector2()) {
 							continue;
 						}
 					}
 
-					stuck=true;
+					stuck = true;
 					break;
 				}
 
-
 				//just do kinematic solving
-				float low=0;
-				float hi=1;
-				Vector2 mnormal=p_motion.normalized();
+				float low = 0;
+				float hi = 1;
+				Vector2 mnormal = p_motion.normalized();
 
-				for(int i=0;i<8;i++) { //steps should be customizable..
+				for (int i = 0; i < 8; i++) { //steps should be customizable..
 
-					float ofs = (low+hi)*0.5;
+					float ofs = (low + hi) * 0.5;
 
-					Vector2 sep=mnormal; //important optimization for this to work fast enough
-					bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*ofs,col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),NULL,NULL,&sep,0);
+					Vector2 sep = mnormal; //important optimization for this to work fast enough
+					bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, 0);
 
 					if (collided) {
 
-						hi=ofs;
+						hi = ofs;
 					} else {
 
-						low=ofs;
+						low = ofs;
 					}
 				}
 
-				if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
+				if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-					const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-					if (body->get_one_way_collision_direction()!=Vector2()) {
+					const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+					if (body->get_one_way_collision_direction() != Vector2()) {
 
 						Vector2 cd[2];
 						Physics2DServerSW::CollCbkData cbk;
-						cbk.max=1;
-						cbk.amount=0;
-						cbk.ptr=cd;
-						cbk.valid_dir=body->get_one_way_collision_direction();
-						cbk.valid_depth=body->get_one_way_collision_max_depth();
+						cbk.max = 1;
+						cbk.amount = 0;
+						cbk.ptr = cd;
+						cbk.valid_dir = body->get_one_way_collision_direction();
+						cbk.valid_depth = body->get_one_way_collision_max_depth();
 
-						Vector2 sep=mnormal; //important optimization for this to work fast enough
-						bool collided = CollisionSolver2DSW::solve(body_shape,body_shape_xform,p_motion*(hi+contact_max_allowed_penetration),col_obj->get_shape(shape_idx),col_obj_xform,Vector2(),Physics2DServerSW::_shape_col_cbk,&cbk,&sep,0);
-						if (!collided || cbk.amount==0) {
+						Vector2 sep = mnormal; //important optimization for this to work fast enough
+						bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
+						if (!collided || cbk.amount == 0) {
 							continue;
 						}
-
 					}
 				}
 
-
-				if (low<best_safe) {
-					best_safe=low;
-					best_unsafe=hi;
+				if (low < best_safe) {
+					best_safe = low;
+					best_unsafe = hi;
 				}
 			}
 
 			if (stuck) {
 
-				safe=0;
-				unsafe=0;
-				best_shape=j; //sadly it's the best
+				safe = 0;
+				unsafe = 0;
+				best_shape = j; //sadly it's the best
 				break;
 			}
-			if (best_safe==1.0) {
+			if (best_safe == 1.0) {
 				continue;
 			}
 			if (best_safe < safe) {
 
-				safe=best_safe;
-				unsafe=best_unsafe;
-				best_shape=j;
+				safe = best_safe;
+				unsafe = best_unsafe;
+				best_shape = j;
 			}
 		}
 	}
 
-	bool collided=false;
-	if (safe>=1) {
+	bool collided = false;
+	if (safe >= 1) {
 		//not collided
-		collided=false;
+		collided = false;
 		if (r_result) {
 
-			r_result->motion=p_motion;
-			r_result->remainder=Vector2();			
-			r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+			r_result->motion = p_motion;
+			r_result->remainder = Vector2();
+			r_result->motion += (body_transform.elements[2] - p_from.elements[2]);
 		}
 
 	} else {
 
 		//it collided, let's get the rest info in unsafe advance
 		Matrix32 ugt = body_transform;
-		ugt.elements[2]+=p_motion*unsafe;
+		ugt.elements[2] += p_motion * unsafe;
 
 		_RestCallbackData2D rcd;
-		rcd.best_len=0;
-		rcd.best_object=NULL;
-		rcd.best_shape=0;
+		rcd.best_len = 0;
+		rcd.best_object = NULL;
+		rcd.best_shape = 0;
 
 		Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
 		Shape2DSW *body_shape = p_body->get_shape(best_shape);
 
-		body_aabb.pos+=p_motion*unsafe;
-
-		int amount = _cull_aabb_for_body(p_body,body_aabb);
+		body_aabb.pos += p_motion * unsafe;
 
+		int amount = _cull_aabb_for_body(p_body, body_aabb);
 
-		for(int i=0;i<amount;i++) {
+		for (int i = 0; i < amount; i++) {
 
+			const CollisionObject2DSW *col_obj = intersection_query_results[i];
+			int shape_idx = intersection_query_subindex_results[i];
 
-			const CollisionObject2DSW *col_obj=intersection_query_results[i];
-			int shape_idx=intersection_query_subindex_results[i];
+			if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
 
-			if (col_obj->get_type()==CollisionObject2DSW::TYPE_BODY) {
-
-				const Body2DSW *body=static_cast<const Body2DSW*>(col_obj);
-				rcd.valid_dir=body->get_one_way_collision_direction();
-				rcd.valid_depth=body->get_one_way_collision_max_depth();
+				const Body2DSW *body = static_cast<const Body2DSW *>(col_obj);
+				rcd.valid_dir = body->get_one_way_collision_direction();
+				rcd.valid_depth = body->get_one_way_collision_max_depth();
 			} else {
-				rcd.valid_dir=Vector2();
-				rcd.valid_depth=0;
+				rcd.valid_dir = Vector2();
+				rcd.valid_depth = 0;
 			}
 
-
-			rcd.object=col_obj;
-			rcd.shape=shape_idx;
-			bool sc = CollisionSolver2DSW::solve(body_shape,body_shape_xform,Vector2(),col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx),Vector2() ,_rest_cbk_result,&rcd,NULL,p_margin);
+			rcd.object = col_obj;
+			rcd.shape = shape_idx;
+			bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
 			if (!sc)
 				continue;
-
 		}
 
-		if (rcd.best_len!=0) {
+		if (rcd.best_len != 0) {
 
 			if (r_result) {
-				r_result->collider=rcd.best_object->get_self();
-				r_result->collider_id=rcd.best_object->get_instance_id();
-				r_result->collider_shape=rcd.best_shape;
-				r_result->collision_normal=rcd.best_normal;
-				r_result->collision_point=rcd.best_contact;
-				r_result->collider_metadata=rcd.best_object->get_shape_metadata(rcd.best_shape);
+				r_result->collider = rcd.best_object->get_self();
+				r_result->collider_id = rcd.best_object->get_instance_id();
+				r_result->collider_shape = rcd.best_shape;
+				r_result->collision_normal = rcd.best_normal;
+				r_result->collision_point = rcd.best_contact;
+				r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
 
-				const Body2DSW *body = static_cast<const Body2DSW*>(rcd.best_object);
-				Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
+				const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
+				Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
 				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
-				r_result->motion=safe*p_motion;
-				r_result->remainder=p_motion - safe * p_motion;
-				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
-
+				r_result->motion = safe * p_motion;
+				r_result->remainder = p_motion - safe * p_motion;
+				r_result->motion += (body_transform.elements[2] - p_from.elements[2]);
 			}
 
-			collided=true;
+			collided = true;
 		} else {
 			if (r_result) {
 
-				r_result->motion=p_motion;
-				r_result->remainder=Vector2();
-				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+				r_result->motion = p_motion;
+				r_result->remainder = Vector2();
+				r_result->motion += (body_transform.elements[2] - p_from.elements[2]);
 			}
 
-			collided=false;
-
+			collided = false;
 		}
 	}
 
 	return collided;
 
-
 #if 0
 	//give me back regular physics engine logic
 	//this is madness
@@ -1095,82 +1018,71 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const
 	return false;
 }
 
+void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {
 
+	CollisionObject2DSW::Type type_A = A->get_type();
+	CollisionObject2DSW::Type type_B = B->get_type();
+	if (type_A > type_B) {
 
-
-void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self) {
-
-
-	CollisionObject2DSW::Type type_A=A->get_type();
-	CollisionObject2DSW::Type type_B=B->get_type();
-	if (type_A>type_B) {
-
-		SWAP(A,B);
-		SWAP(p_subindex_A,p_subindex_B);
-		SWAP(type_A,type_B);
+		SWAP(A, B);
+		SWAP(p_subindex_A, p_subindex_B);
+		SWAP(type_A, type_B);
 	}
 
-	Space2DSW *self = (Space2DSW*)p_self;
+	Space2DSW *self = (Space2DSW *)p_self;
 	self->collision_pairs++;
 
-	if (type_A==CollisionObject2DSW::TYPE_AREA) {
+	if (type_A == CollisionObject2DSW::TYPE_AREA) {
 
-		Area2DSW *area=static_cast<Area2DSW*>(A);
-		if (type_B==CollisionObject2DSW::TYPE_AREA) {
+		Area2DSW *area = static_cast<Area2DSW *>(A);
+		if (type_B == CollisionObject2DSW::TYPE_AREA) {
 
-			Area2DSW *area_b=static_cast<Area2DSW*>(B);
-			Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b,p_subindex_B,area,p_subindex_A) );
+			Area2DSW *area_b = static_cast<Area2DSW *>(B);
+			Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A));
 			return area2_pair;
 		} else {
 
-			Body2DSW *body=static_cast<Body2DSW*>(B);
-			AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
+			Body2DSW *body = static_cast<Body2DSW *>(B);
+			AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A));
 			return area_pair;
 		}
 
-
 	} else {
 
-
-		BodyPair2DSW *b = memnew( BodyPair2DSW((Body2DSW*)A,p_subindex_A,(Body2DSW*)B,p_subindex_B) );
+		BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B));
 		return b;
-
 	}
 
 	return NULL;
 }
 
-void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self) {
+void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) {
 
-
-	Space2DSW *self = (Space2DSW*)p_self;
+	Space2DSW *self = (Space2DSW *)p_self;
 	self->collision_pairs--;
-	Constraint2DSW *c = (Constraint2DSW*)p_data;
+	Constraint2DSW *c = (Constraint2DSW *)p_data;
 	memdelete(c);
 }
 
-
-const SelfList<Body2DSW>::List& Space2DSW::get_active_body_list() const {
+const SelfList<Body2DSW>::List &Space2DSW::get_active_body_list() const {
 
 	return active_list;
 }
-void Space2DSW::body_add_to_active_list(SelfList<Body2DSW>* p_body) {
+void Space2DSW::body_add_to_active_list(SelfList<Body2DSW> *p_body) {
 
 	active_list.add(p_body);
 }
-void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW>* p_body) {
+void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) {
 
 	active_list.remove(p_body);
-
 }
 
-void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body) {
-
+void Space2DSW::body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body) {
 
 	inertia_update_list.add(p_body);
 }
 
-void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body) {
+void Space2DSW::body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body) {
 
 	inertia_update_list.remove(p_body);
 }
@@ -1182,109 +1094,103 @@ BroadPhase2DSW *Space2DSW::get_broadphase() {
 
 void Space2DSW::add_object(CollisionObject2DSW *p_object) {
 
-	ERR_FAIL_COND( objects.has(p_object) );
+	ERR_FAIL_COND(objects.has(p_object));
 	objects.insert(p_object);
 }
 
 void Space2DSW::remove_object(CollisionObject2DSW *p_object) {
 
-	ERR_FAIL_COND( !objects.has(p_object) );
+	ERR_FAIL_COND(!objects.has(p_object));
 	objects.erase(p_object);
 }
 
-const Set<CollisionObject2DSW*> &Space2DSW::get_objects() const {
+const Set<CollisionObject2DSW *> &Space2DSW::get_objects() const {
 
 	return objects;
 }
 
-void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW>* p_body) {
+void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW> *p_body) {
 
 	state_query_list.add(p_body);
 }
-void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW>* p_body) {
+void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW> *p_body) {
 
 	state_query_list.remove(p_body);
 }
 
-void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area) {
+void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW> *p_area) {
 
 	monitor_query_list.add(p_area);
 }
-void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area) {
+void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW> *p_area) {
 
 	monitor_query_list.remove(p_area);
 }
 
-void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW>* p_area) {
+void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW> *p_area) {
 
 	area_moved_list.add(p_area);
 }
 
-void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW>* p_area) {
+void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW> *p_area) {
 
 	area_moved_list.remove(p_area);
 }
 
-const SelfList<Area2DSW>::List& Space2DSW::get_moved_area_list() const {
+const SelfList<Area2DSW>::List &Space2DSW::get_moved_area_list() const {
 
 	return area_moved_list;
 }
 
-
 void Space2DSW::call_queries() {
 
-	while(state_query_list.first()) {
+	while (state_query_list.first()) {
 
-		Body2DSW * b = state_query_list.first()->self();
+		Body2DSW *b = state_query_list.first()->self();
 		b->call_queries();
 		state_query_list.remove(state_query_list.first());
 	}
 
-	while(monitor_query_list.first()) {
+	while (monitor_query_list.first()) {
 
-		Area2DSW * a = monitor_query_list.first()->self();
+		Area2DSW *a = monitor_query_list.first()->self();
 		a->call_queries();
 		monitor_query_list.remove(monitor_query_list.first());
 	}
-
 }
 
 void Space2DSW::setup() {
 
-	contact_debug_count=0;
+	contact_debug_count = 0;
 
-	while(inertia_update_list.first()) {
+	while (inertia_update_list.first()) {
 		inertia_update_list.first()->self()->update_inertias();
 		inertia_update_list.remove(inertia_update_list.first());
 	}
-
-
 }
 
 void Space2DSW::update() {
 
 	broadphase->update();
-
 }
 
-
 void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_value) {
 
-	switch(p_param) {
+	switch (p_param) {
 
-		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
-		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
-		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
-		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
-		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
-		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
-		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius = p_value; break;
+		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation = p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration = p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold = p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold = p_value; break;
+		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
+		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
 	}
 }
 
 real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 
 		case Physics2DServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
 		case Physics2DServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
@@ -1299,12 +1205,12 @@ real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
 
 void Space2DSW::lock() {
 
-	locked=true;
+	locked = true;
 }
 
 void Space2DSW::unlock() {
 
-	locked=false;
+	locked = false;
 }
 
 bool Space2DSW::is_locked() const {
@@ -1319,43 +1225,36 @@ Physics2DDirectSpaceStateSW *Space2DSW::get_direct_state() {
 
 Space2DSW::Space2DSW() {
 
+	collision_pairs = 0;
+	active_objects = 0;
+	island_count = 0;
 
-	collision_pairs=0;
-	active_objects=0;
-	island_count=0;
+	contact_debug_count = 0;
 
-	contact_debug_count=0;
-
-	locked=false;
-	contact_recycle_radius=1.0;
-	contact_max_separation=1.5;
-	contact_max_allowed_penetration= 0.3;
+	locked = false;
+	contact_recycle_radius = 1.0;
+	contact_max_separation = 1.5;
+	contact_max_allowed_penetration = 0.3;
 
 	constraint_bias = 0.2;
-	body_linear_velocity_sleep_treshold=GLOBAL_DEF("physics_2d/sleep_threashold_linear",2.0);
-	body_angular_velocity_sleep_treshold=GLOBAL_DEF("physics_2d/sleep_threshold_angular",(8.0 / 180.0 * Math_PI));
-	body_time_to_sleep=GLOBAL_DEF("physics_2d/time_before_sleep",0.5);
-
+	body_linear_velocity_sleep_treshold = GLOBAL_DEF("physics_2d/sleep_threashold_linear", 2.0);
+	body_angular_velocity_sleep_treshold = GLOBAL_DEF("physics_2d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI));
+	body_time_to_sleep = GLOBAL_DEF("physics_2d/time_before_sleep", 0.5);
 
 	broadphase = BroadPhase2DSW::create_func();
-	broadphase->set_pair_callback(_broadphase_pair,this);
-	broadphase->set_unpair_callback(_broadphase_unpair,this);
-	area=NULL;
-
-	direct_access = memnew( Physics2DDirectSpaceStateSW );
-	direct_access->space=this;
-
+	broadphase->set_pair_callback(_broadphase_pair, this);
+	broadphase->set_unpair_callback(_broadphase_unpair, this);
+	area = NULL;
 
-	for(int i=0;i<ELAPSED_TIME_MAX;i++)
-		elapsed_time[i]=0;
+	direct_access = memnew(Physics2DDirectSpaceStateSW);
+	direct_access->space = this;
 
+	for (int i = 0; i < ELAPSED_TIME_MAX; i++)
+		elapsed_time[i] = 0;
 }
 
 Space2DSW::~Space2DSW() {
 
 	memdelete(broadphase);
-	memdelete( direct_access );
+	memdelete(direct_access);
 }
-
-
-
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index b7976c589..7a23bb2fc 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -29,40 +29,36 @@
 #ifndef SPACE_2D_SW_H
 #define SPACE_2D_SW_H
 
-#include "typedefs.h"
-#include "hash_map.h"
-#include "body_2d_sw.h"
 #include "area_2d_sw.h"
-#include "body_pair_2d_sw.h"
 #include "area_pair_2d_sw.h"
+#include "body_2d_sw.h"
+#include "body_pair_2d_sw.h"
 #include "broad_phase_2d_sw.h"
 #include "collision_object_2d_sw.h"
 #include "globals.h"
-
+#include "hash_map.h"
+#include "typedefs.h"
 
 class Physics2DDirectSpaceStateSW : public Physics2DDirectSpaceState {
 
-	OBJ_TYPE( Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState );
-public:
+	OBJ_TYPE(Physics2DDirectSpaceStateSW, Physics2DDirectSpaceState);
 
+public:
 	Space2DSW *space;
 
-	virtual int intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION, bool p_pick_point=false);
-	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_point = false);
+	virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual int intersect_shape(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool cast_motion(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool collide_shape(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	virtual bool rest_info(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 
 	Physics2DDirectSpaceStateSW();
 };
 
-
-
 class Space2DSW {
 
 public:
-
 	enum ElapsedTime {
 		ELAPSED_TIME_INTEGRATE_FORCES,
 		ELAPSED_TIME_GENERATE_ISLANDS,
@@ -72,8 +68,8 @@ public:
 		ELAPSED_TIME_MAX
 
 	};
-private:
 
+private:
 	uint64_t elapsed_time[ELAPSED_TIME_MAX];
 
 	Physics2DDirectSpaceStateSW *direct_access;
@@ -86,10 +82,10 @@ private:
 	SelfList<Area2DSW>::List monitor_query_list;
 	SelfList<Area2DSW>::List area_moved_list;
 
-	static void* _broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_self);
-	static void _broadphase_unpair(CollisionObject2DSW *A,int p_subindex_A,CollisionObject2DSW *B,int p_subindex_B,void *p_data,void *p_self);
+	static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self);
+	static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self);
 
-	Set<CollisionObject2DSW*> objects;
+	Set<CollisionObject2DSW *> objects;
 
 	Area2DSW *area;
 
@@ -100,7 +96,7 @@ private:
 
 	enum {
 
-		INTERSECTION_QUERY_MAX=2048
+		INTERSECTION_QUERY_MAX = 2048
 	};
 
 	CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
@@ -116,44 +112,40 @@ private:
 	int active_objects;
 	int collision_pairs;
 
-	int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb);
+	int _cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb);
 
 	Vector<Vector2> contact_debug;
 	int contact_debug_count;
 
-friend class Physics2DDirectSpaceStateSW;
+	friend class Physics2DDirectSpaceStateSW;
 
 public:
-
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	void set_default_area(Area2DSW *p_area) { area=p_area; }
+	void set_default_area(Area2DSW *p_area) { area = p_area; }
 	Area2DSW *get_default_area() const { return area; }
 
-	const SelfList<Body2DSW>::List& get_active_body_list() const;
-	void body_add_to_active_list(SelfList<Body2DSW>* p_body);
-	void body_remove_from_active_list(SelfList<Body2DSW>* p_body);
-	void body_add_to_inertia_update_list(SelfList<Body2DSW>* p_body);
-	void body_remove_from_inertia_update_list(SelfList<Body2DSW>* p_body);
-	void area_add_to_moved_list(SelfList<Area2DSW>* p_area);
-	void area_remove_from_moved_list(SelfList<Area2DSW>* p_area);
-	const SelfList<Area2DSW>::List& get_moved_area_list() const;
-
-
-
+	const SelfList<Body2DSW>::List &get_active_body_list() const;
+	void body_add_to_active_list(SelfList<Body2DSW> *p_body);
+	void body_remove_from_active_list(SelfList<Body2DSW> *p_body);
+	void body_add_to_inertia_update_list(SelfList<Body2DSW> *p_body);
+	void body_remove_from_inertia_update_list(SelfList<Body2DSW> *p_body);
+	void area_add_to_moved_list(SelfList<Area2DSW> *p_area);
+	void area_remove_from_moved_list(SelfList<Area2DSW> *p_area);
+	const SelfList<Area2DSW>::List &get_moved_area_list() const;
 
-	void body_add_to_state_query_list(SelfList<Body2DSW>* p_body);
-	void body_remove_from_state_query_list(SelfList<Body2DSW>* p_body);
+	void body_add_to_state_query_list(SelfList<Body2DSW> *p_body);
+	void body_remove_from_state_query_list(SelfList<Body2DSW> *p_body);
 
-	void area_add_to_monitor_query_list(SelfList<Area2DSW>* p_area);
-	void area_remove_from_monitor_query_list(SelfList<Area2DSW>* p_area);
+	void area_add_to_monitor_query_list(SelfList<Area2DSW> *p_area);
+	void area_remove_from_monitor_query_list(SelfList<Area2DSW> *p_area);
 
 	BroadPhase2DSW *get_broadphase();
 
 	void add_object(CollisionObject2DSW *p_object);
 	void remove_object(CollisionObject2DSW *p_object);
-	const Set<CollisionObject2DSW*> &get_objects() const;
+	const Set<CollisionObject2DSW *> &get_objects() const;
 
 	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
 	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
@@ -163,13 +155,10 @@ public:
 	_FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
 	_FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
 
-
-
 	void update();
 	void setup();
 	void call_queries();
 
-
 	bool is_locked() const;
 	void lock();
 	void unlock();
@@ -177,32 +166,31 @@ public:
 	void set_param(Physics2DServer::SpaceParameter p_param, real_t p_value);
 	real_t get_param(Physics2DServer::SpaceParameter p_param) const;
 
-	void set_island_count(int p_island_count) { island_count=p_island_count; }
+	void set_island_count(int p_island_count) { island_count = p_island_count; }
 	int get_island_count() const { return island_count; }
 
-	void set_active_objects(int p_active_objects) { active_objects=p_active_objects; }
+	void set_active_objects(int p_active_objects) { active_objects = p_active_objects; }
 	int get_active_objects() const { return active_objects; }
 
 	int get_collision_pairs() const { return collision_pairs; }
 
-	bool test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
-
+	bool test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
 
 	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
 	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }
-	_FORCE_INLINE_ void add_debug_contact(const Vector2& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }
+	_FORCE_INLINE_ void add_debug_contact(const Vector2 &p_contact) {
+		if (contact_debug_count < contact_debug.size()) contact_debug[contact_debug_count++] = p_contact;
+	}
 	_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
 	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
 
-
 	Physics2DDirectSpaceStateSW *get_direct_state();
 
-	void set_elapsed_time(ElapsedTime p_time,uint64_t p_msec) { elapsed_time[p_time]=p_msec; }
+	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
 	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
 
 	Space2DSW();
 	~Space2DSW();
 };
 
-
 #endif // SPACE_2D_SW_H
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp
index 05c0bf051..d87dab395 100644
--- a/servers/physics_2d/step_2d_sw.cpp
+++ b/servers/physics_2d/step_2d_sw.cpp
@@ -29,39 +29,38 @@
 #include "step_2d_sw.h"
 #include "os/os.h"
 
-void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island) {
+void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island) {
 
 	p_body->set_island_step(_step);
 	p_body->set_island_next(*p_island);
-	*p_island=p_body;
+	*p_island = p_body;
 
-	for(Map<Constraint2DSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) {
+	for (Map<Constraint2DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
 
-		Constraint2DSW *c=(Constraint2DSW*)E->key();
-		if (c->get_island_step()==_step)
+		Constraint2DSW *c = (Constraint2DSW *)E->key();
+		if (c->get_island_step() == _step)
 			continue; //already processed
 		c->set_island_step(_step);
 		c->set_island_next(*p_constraint_island);
-		*p_constraint_island=c;
+		*p_constraint_island = c;
 
-
-		for(int i=0;i<c->get_body_count();i++) {
-			if (i==E->get())
+		for (int i = 0; i < c->get_body_count(); i++) {
+			if (i == E->get())
 				continue;
 			Body2DSW *b = c->get_body_ptr()[i];
-			if (b->get_island_step()==_step || b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC)
+			if (b->get_island_step() == _step || b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC)
 				continue; //no go
-			_populate_island(c->get_body_ptr()[i],p_island,p_constraint_island);
+			_populate_island(c->get_body_ptr()[i], p_island, p_constraint_island);
 		}
 	}
 }
 
-bool Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) {
+bool Step2DSW::_setup_island(Constraint2DSW *p_island, float p_delta) {
 
-	Constraint2DSW *ci=p_island;
-	Constraint2DSW *prev_ci=NULL;
-	bool removed_root=false;
-	while(ci) {
+	Constraint2DSW *ci = p_island;
+	Constraint2DSW *prev_ci = NULL;
+	bool removed_root = false;
+	while (ci) {
 		bool process = ci->setup(p_delta);
 
 		if (!process) {
@@ -69,169 +68,161 @@ bool Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) {
 			if (prev_ci) {
 				prev_ci->set_island_next(ci->get_island_next());
 			} else {
-				removed_root=true;
-				prev_ci=ci;
+				removed_root = true;
+				prev_ci = ci;
 			}
 		} else {
-			prev_ci=ci;
+			prev_ci = ci;
 		}
-		ci=ci->get_island_next();
+		ci = ci->get_island_next();
 	}
 
 	return removed_root;
 }
 
-void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta){
-
+void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, float p_delta) {
 
-	for(int i=0;i<p_iterations;i++) {
+	for (int i = 0; i < p_iterations; i++) {
 
-		Constraint2DSW *ci=p_island;
-		while(ci) {
+		Constraint2DSW *ci = p_island;
+		while (ci) {
 			ci->solve(p_delta);
-			ci=ci->get_island_next();
+			ci = ci->get_island_next();
 		}
 	}
 }
 
-void Step2DSW::_check_suspend(Body2DSW *p_island,float p_delta) {
+void Step2DSW::_check_suspend(Body2DSW *p_island, float p_delta) {
 
-
-	bool can_sleep=true;
+	bool can_sleep = true;
 
 	Body2DSW *b = p_island;
-	while(b) {
+	while (b) {
 
-		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
-			b=b->get_island_next();
+		if (b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) {
+			b = b->get_island_next();
 			continue; //ignore for static
 		}
 
 		if (!b->sleep_test(p_delta))
-			can_sleep=false;
+			can_sleep = false;
 
-		b=b->get_island_next();
+		b = b->get_island_next();
 	}
 
 	//put all to sleep or wake up everyoen
 
 	b = p_island;
-	while(b) {
+	while (b) {
 
-		if (b->get_mode()==Physics2DServer::BODY_MODE_STATIC || b->get_mode()==Physics2DServer::BODY_MODE_KINEMATIC) {
-			b=b->get_island_next();
+		if (b->get_mode() == Physics2DServer::BODY_MODE_STATIC || b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC) {
+			b = b->get_island_next();
 			continue; //ignore for static
 		}
 
 		bool active = b->is_active();
 
-		if (active==can_sleep)
+		if (active == can_sleep)
 			b->set_active(!can_sleep);
 
-		b=b->get_island_next();
+		b = b->get_island_next();
 	}
 }
 
-void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
-
+void Step2DSW::step(Space2DSW *p_space, float p_delta, int p_iterations) {
 
 	p_space->lock(); // can't access space during this
 
 	p_space->setup(); //update inertias, etc
 
-	const SelfList<Body2DSW>::List * body_list = &p_space->get_active_body_list();
+	const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();
 
 	/* INTEGRATE FORCES */
 
 	uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
-	uint64_t profile_endtime=0;
+	uint64_t profile_endtime = 0;
 
+	int active_count = 0;
 
-	int active_count=0;
-
-	const SelfList<Body2DSW>*b = body_list->first();
-	while(b) {
+	const SelfList<Body2DSW> *b = body_list->first();
+	while (b) {
 
 		b->self()->integrate_forces(p_delta);
-		b=b->next();
+		b = b->next();
 		active_count++;
 	}
 
 	p_space->set_active_objects(active_count);
 
-
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* GENERATE CONSTRAINT ISLANDS */
 
-	Body2DSW *island_list=NULL;
-	Constraint2DSW *constraint_island_list=NULL;
+	Body2DSW *island_list = NULL;
+	Constraint2DSW *constraint_island_list = NULL;
 	b = body_list->first();
 
-	int island_count=0;
+	int island_count = 0;
 
-	while(b) {
+	while (b) {
 		Body2DSW *body = b->self();
 
+		if (body->get_island_step() != _step) {
 
-		if (body->get_island_step()!=_step) {
-
-			Body2DSW *island=NULL;
-			Constraint2DSW *constraint_island=NULL;
-			_populate_island(body,&island,&constraint_island);
+			Body2DSW *island = NULL;
+			Constraint2DSW *constraint_island = NULL;
+			_populate_island(body, &island, &constraint_island);
 
 			island->set_island_list_next(island_list);
-			island_list=island;
+			island_list = island;
 
 			if (constraint_island) {
 				constraint_island->set_island_list_next(constraint_island_list);
-				constraint_island_list=constraint_island;
+				constraint_island_list = constraint_island;
 				island_count++;
 			}
-
 		}
-		b=b->next();
+		b = b->next();
 	}
 
 	p_space->set_island_count(island_count);
 
 	const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list();
 
+	while (aml.first()) {
+		for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
 
-	while(aml.first()) {
-		for(const Set<Constraint2DSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) {
-
-			Constraint2DSW*c=E->get();
-			if (c->get_island_step()==_step)
+			Constraint2DSW *c = E->get();
+			if (c->get_island_step() == _step)
 				continue;
 			c->set_island_step(_step);
 			c->set_island_next(NULL);
 			c->set_island_list_next(constraint_island_list);
-			constraint_island_list=c;
+			constraint_island_list = c;
 		}
-		p_space->area_remove_from_moved_list((SelfList<Area2DSW>*)aml.first()); //faster to remove here
+		p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here
 	}
 
-//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
+	//	print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* SETUP CONSTRAINT ISLANDS */
 
 	{
-		Constraint2DSW *ci=constraint_island_list;
-		Constraint2DSW *prev_ci=NULL;
-		while(ci) {
+		Constraint2DSW *ci = constraint_island_list;
+		Constraint2DSW *prev_ci = NULL;
+		while (ci) {
 
-			if (_setup_island(ci,p_delta)==true) {
+			if (_setup_island(ci, p_delta) == true) {
 
 				//removed the root from the island graph because it is not to be processed
 
@@ -243,10 +234,9 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
 					if (prev_ci) {
 						prev_ci->set_island_list_next(next);
 					} else {
-						constraint_island_list=next;
-
+						constraint_island_list = next;
 					}
-					prev_ci=next;
+					prev_ci = next;
 				} else {
 
 					//list is empty, just skip
@@ -254,77 +244,73 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) {
 						prev_ci->set_island_list_next(ci->get_island_list_next());
 
 					} else {
-						constraint_island_list=ci->get_island_list_next();
+						constraint_island_list = ci->get_island_list_next();
 					}
-
 				}
 			} else {
-				prev_ci=ci;
+				prev_ci = ci;
 			}
 
-			ci=ci->get_island_list_next();
+			ci = ci->get_island_list_next();
 		}
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* SOLVE CONSTRAINT ISLANDS */
 
 	{
-		Constraint2DSW *ci=constraint_island_list;
-		while(ci) {
+		Constraint2DSW *ci = constraint_island_list;
+		while (ci) {
 			//iterating each island separatedly improves cache efficiency
-			_solve_island(ci,p_iterations,p_delta);
-			ci=ci->get_island_list_next();
+			_solve_island(ci, p_iterations, p_delta);
+			ci = ci->get_island_list_next();
 		}
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS,profile_endtime-profile_begtime);
-		profile_begtime=profile_endtime;
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
+		profile_begtime = profile_endtime;
 	}
 
 	/* INTEGRATE VELOCITIES */
 
 	b = body_list->first();
-	while(b) {
+	while (b) {
 
-		const SelfList<Body2DSW>*n=b->next();
+		const SelfList<Body2DSW> *n = b->next();
 		b->self()->integrate_velocities(p_delta);
-		b=n;  // in case it shuts itself down
+		b = n; // in case it shuts itself down
 	}
 
 	/* SLEEP / WAKE UP ISLANDS */
 
 	{
-		Body2DSW *bi=island_list;
-		while(bi) {
+		Body2DSW *bi = island_list;
+		while (bi) {
 
-			_check_suspend(bi,p_delta);
-			bi=bi->get_island_list_next();
+			_check_suspend(bi, p_delta);
+			bi = bi->get_island_list_next();
 		}
 	}
 
 	{ //profile
-		profile_endtime=OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES,profile_endtime-profile_begtime);
+		profile_endtime = OS::get_singleton()->get_ticks_usec();
+		p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
 		//profile_begtime=profile_endtime;
 	}
 
 	p_space->update();
 	p_space->unlock();
 	_step++;
-
-
-
 }
 
 Step2DSW::Step2DSW() {
 
-	_step=1;
+	_step = 1;
 }
diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h
index 917d69e7f..ed158acf0 100644
--- a/servers/physics_2d/step_2d_sw.h
+++ b/servers/physics_2d/step_2d_sw.h
@@ -35,13 +35,13 @@ class Step2DSW {
 
 	uint64_t _step;
 
-	void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island);
-	bool _setup_island(Constraint2DSW *p_island,float p_delta);
-	void _solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta);
-	void _check_suspend(Body2DSW *p_island,float p_delta);
-public:
+	void _populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island);
+	bool _setup_island(Constraint2DSW *p_island, float p_delta);
+	void _solve_island(Constraint2DSW *p_island, int p_iterations, float p_delta);
+	void _check_suspend(Body2DSW *p_island, float p_delta);
 
-	void step(Space2DSW* p_space,float p_delta,int p_iterations);
+public:
+	void step(Space2DSW *p_space, float p_delta, int p_iterations);
 	Step2DSW();
 };
 
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
index d618477ca..9f47fbc8c 100644
--- a/servers/physics_2d_server.cpp
+++ b/servers/physics_2d_server.cpp
@@ -28,103 +28,97 @@
 /*************************************************************************/
 #include "physics_2d_server.h"
 #include "print_string.h"
-Physics2DServer * Physics2DServer::singleton=NULL;
-
+Physics2DServer *Physics2DServer::singleton = NULL;
 
 void Physics2DDirectBodyState::integrate_forces() {
 
 	real_t step = get_step();
 	Vector2 lv = get_linear_velocity();
-	lv+=get_total_gravity() * step;
+	lv += get_total_gravity() * step;
 
 	real_t av = get_angular_velocity();
 
 	float damp = 1.0 - step * get_total_linear_damp();
 
-	if (damp<0) // reached zero in the given time
-		damp=0;
+	if (damp < 0) // reached zero in the given time
+		damp = 0;
 
-	lv*=damp;
+	lv *= damp;
 
 	damp = 1.0 - step * get_total_angular_damp();
 
-	if (damp<0) // reached zero in the given time
-		damp=0;
+	if (damp < 0) // reached zero in the given time
+		damp = 0;
 
-	av*=damp;
+	av *= damp;
 
 	set_linear_velocity(lv);
 	set_angular_velocity(av);
-
-
 }
 
-Object* Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+Object *Physics2DDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
 
 	ObjectID objid = get_contact_collider_id(p_contact_idx);
-	Object *obj = ObjectDB::get_instance( objid );
+	Object *obj = ObjectDB::get_instance(objid);
 	return obj;
 }
 
-Physics2DServer * Physics2DServer::get_singleton() {
+Physics2DServer *Physics2DServer::get_singleton() {
 
 	return singleton;
 }
 
 void Physics2DDirectBodyState::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&Physics2DDirectBodyState::get_total_gravity);
-	ObjectTypeDB::bind_method(_MD("get_total_linear_damp"),&Physics2DDirectBodyState::get_total_linear_damp);
-	ObjectTypeDB::bind_method(_MD("get_total_angular_damp"),&Physics2DDirectBodyState::get_total_angular_damp);
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"), &Physics2DDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_linear_damp"), &Physics2DDirectBodyState::get_total_linear_damp);
+	ObjectTypeDB::bind_method(_MD("get_total_angular_damp"), &Physics2DDirectBodyState::get_total_angular_damp);
 
-	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&Physics2DDirectBodyState::get_inverse_mass);
-	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&Physics2DDirectBodyState::get_inverse_inertia);
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"), &Physics2DDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"), &Physics2DDirectBodyState::get_inverse_inertia);
 
-	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&Physics2DDirectBodyState::set_linear_velocity);
-	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&Physics2DDirectBodyState::get_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity", "velocity"), &Physics2DDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"), &Physics2DDirectBodyState::get_linear_velocity);
 
-	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&Physics2DDirectBodyState::set_angular_velocity);
-	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&Physics2DDirectBodyState::get_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity", "velocity"), &Physics2DDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"), &Physics2DDirectBodyState::get_angular_velocity);
 
-	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DDirectBodyState::set_transform);
-	ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DDirectBodyState::get_transform);
+	ObjectTypeDB::bind_method(_MD("set_transform", "transform"), &Physics2DDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"), &Physics2DDirectBodyState::get_transform);
 
-	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&Physics2DDirectBodyState::set_sleep_state);
-	ObjectTypeDB::bind_method(_MD("is_sleeping"),&Physics2DDirectBodyState::is_sleeping);
+	ObjectTypeDB::bind_method(_MD("set_sleep_state", "enabled"), &Physics2DDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"), &Physics2DDirectBodyState::is_sleeping);
 
-	ObjectTypeDB::bind_method(_MD("get_contact_count"),&Physics2DDirectBodyState::get_contact_count);
-
-	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_local_pos);
-	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&Physics2DDirectBodyState::get_contact_local_normal);
-	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_local_shape);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&Physics2DDirectBodyState::get_contact_collider);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_pos);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_id);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_object);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape_metadata:Variant","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_shape_metadata);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&Physics2DDirectBodyState::get_contact_collider_velocity_at_pos);
-	ObjectTypeDB::bind_method(_MD("get_step"),&Physics2DDirectBodyState::get_step);
-	ObjectTypeDB::bind_method(_MD("integrate_forces"),&Physics2DDirectBodyState::integrate_forces);
-	ObjectTypeDB::bind_method(_MD("get_space_state:Physics2DDirectSpaceState"),&Physics2DDirectBodyState::get_space_state);
+	ObjectTypeDB::bind_method(_MD("get_contact_count"), &Physics2DDirectBodyState::get_contact_count);
 
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape", "contact_idx"), &Physics2DDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape_metadata:Variant", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_shape_metadata);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos", "contact_idx"), &Physics2DDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"), &Physics2DDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"), &Physics2DDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:Physics2DDirectSpaceState"), &Physics2DDirectBodyState::get_space_state);
 }
 
 Physics2DDirectBodyState::Physics2DDirectBodyState() {}
 
 ///////////////////////////////////////////////////////
 
-
-
 void Physics2DShapeQueryParameters::set_shape(const RES &p_shape) {
 
 	ERR_FAIL_COND(p_shape.is_null());
-	shape=p_shape->get_rid();
+	shape = p_shape->get_rid();
 }
 
-void Physics2DShapeQueryParameters::set_shape_rid(const RID& p_shape) {
+void Physics2DShapeQueryParameters::set_shape_rid(const RID &p_shape) {
 
-	shape=p_shape;
+	shape = p_shape;
 }
 
 RID Physics2DShapeQueryParameters::get_shape_rid() const {
@@ -132,125 +126,120 @@ RID Physics2DShapeQueryParameters::get_shape_rid() const {
 	return shape;
 }
 
-void Physics2DShapeQueryParameters::set_transform(const Matrix32& p_transform){
+void Physics2DShapeQueryParameters::set_transform(const Matrix32 &p_transform) {
 
-	transform=p_transform;
+	transform = p_transform;
 }
-Matrix32 Physics2DShapeQueryParameters::get_transform() const{
+Matrix32 Physics2DShapeQueryParameters::get_transform() const {
 
 	return transform;
 }
 
-void Physics2DShapeQueryParameters::set_motion(const Vector2& p_motion){
+void Physics2DShapeQueryParameters::set_motion(const Vector2 &p_motion) {
 
-	motion=p_motion;
+	motion = p_motion;
 }
-Vector2 Physics2DShapeQueryParameters::get_motion() const{
+Vector2 Physics2DShapeQueryParameters::get_motion() const {
 
 	return motion;
 }
 
-void Physics2DShapeQueryParameters::set_margin(float p_margin){
+void Physics2DShapeQueryParameters::set_margin(float p_margin) {
 
-	margin=p_margin;
+	margin = p_margin;
 }
-float Physics2DShapeQueryParameters::get_margin() const{
+float Physics2DShapeQueryParameters::get_margin() const {
 
 	return margin;
 }
 
-void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask){
+void Physics2DShapeQueryParameters::set_layer_mask(int p_layer_mask) {
 
-	layer_mask=p_layer_mask;
+	layer_mask = p_layer_mask;
 }
-int Physics2DShapeQueryParameters::get_layer_mask() const{
+int Physics2DShapeQueryParameters::get_layer_mask() const {
 
 	return layer_mask;
 }
 
+void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask) {
 
-void Physics2DShapeQueryParameters::set_object_type_mask(int p_object_type_mask){
-
-	object_type_mask=p_object_type_mask;
+	object_type_mask = p_object_type_mask;
 }
-int Physics2DShapeQueryParameters::get_object_type_mask() const{
+int Physics2DShapeQueryParameters::get_object_type_mask() const {
 
 	return object_type_mask;
 }
-void Physics2DShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) {
+void Physics2DShapeQueryParameters::set_exclude(const Vector<RID> &p_exclude) {
 
 	exclude.clear();
-	for(int i=0;i<p_exclude.size();i++)
+	for (int i = 0; i < p_exclude.size(); i++)
 		exclude.insert(p_exclude[i]);
-
 }
 
-Vector<RID> Physics2DShapeQueryParameters::get_exclude() const{
+Vector<RID> Physics2DShapeQueryParameters::get_exclude() const {
 
 	Vector<RID> ret;
 	ret.resize(exclude.size());
-	int idx=0;
-	for(Set<RID>::Element *E=exclude.front();E;E=E->next()) {
-		ret[idx]=E->get();
+	int idx = 0;
+	for (Set<RID>::Element *E = exclude.front(); E; E = E->next()) {
+		ret[idx] = E->get();
 	}
 	return ret;
 }
 
 void Physics2DShapeQueryParameters::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape2D"),&Physics2DShapeQueryParameters::set_shape);
-	ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&Physics2DShapeQueryParameters::set_shape_rid);
-	ObjectTypeDB::bind_method(_MD("get_shape_rid"),&Physics2DShapeQueryParameters::get_shape_rid);
-
-	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&Physics2DShapeQueryParameters::set_transform);
-	ObjectTypeDB::bind_method(_MD("get_transform"),&Physics2DShapeQueryParameters::get_transform);
+	ObjectTypeDB::bind_method(_MD("set_shape", "shape:Shape2D"), &Physics2DShapeQueryParameters::set_shape);
+	ObjectTypeDB::bind_method(_MD("set_shape_rid", "shape"), &Physics2DShapeQueryParameters::set_shape_rid);
+	ObjectTypeDB::bind_method(_MD("get_shape_rid"), &Physics2DShapeQueryParameters::get_shape_rid);
 
-	ObjectTypeDB::bind_method(_MD("set_motion","motion"),&Physics2DShapeQueryParameters::set_motion);
-	ObjectTypeDB::bind_method(_MD("get_motion"),&Physics2DShapeQueryParameters::get_motion);
+	ObjectTypeDB::bind_method(_MD("set_transform", "transform"), &Physics2DShapeQueryParameters::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"), &Physics2DShapeQueryParameters::get_transform);
 
-	ObjectTypeDB::bind_method(_MD("set_margin","margin"),&Physics2DShapeQueryParameters::set_margin);
-	ObjectTypeDB::bind_method(_MD("get_margin"),&Physics2DShapeQueryParameters::get_margin);
+	ObjectTypeDB::bind_method(_MD("set_motion", "motion"), &Physics2DShapeQueryParameters::set_motion);
+	ObjectTypeDB::bind_method(_MD("get_motion"), &Physics2DShapeQueryParameters::get_motion);
 
-	ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&Physics2DShapeQueryParameters::set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&Physics2DShapeQueryParameters::get_layer_mask);
+	ObjectTypeDB::bind_method(_MD("set_margin", "margin"), &Physics2DShapeQueryParameters::set_margin);
+	ObjectTypeDB::bind_method(_MD("get_margin"), &Physics2DShapeQueryParameters::get_margin);
 
-	ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&Physics2DShapeQueryParameters::set_object_type_mask);
-	ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&Physics2DShapeQueryParameters::get_object_type_mask);
-
-	ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&Physics2DShapeQueryParameters::set_exclude);
-	ObjectTypeDB::bind_method(_MD("get_exclude"),&Physics2DShapeQueryParameters::get_exclude);
+	ObjectTypeDB::bind_method(_MD("set_layer_mask", "layer_mask"), &Physics2DShapeQueryParameters::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"), &Physics2DShapeQueryParameters::get_layer_mask);
 
+	ObjectTypeDB::bind_method(_MD("set_object_type_mask", "object_type_mask"), &Physics2DShapeQueryParameters::set_object_type_mask);
+	ObjectTypeDB::bind_method(_MD("get_object_type_mask"), &Physics2DShapeQueryParameters::get_object_type_mask);
 
+	ObjectTypeDB::bind_method(_MD("set_exclude", "exclude"), &Physics2DShapeQueryParameters::set_exclude);
+	ObjectTypeDB::bind_method(_MD("get_exclude"), &Physics2DShapeQueryParameters::get_exclude);
 }
 
 Physics2DShapeQueryParameters::Physics2DShapeQueryParameters() {
 
-	margin=0;
-	layer_mask=0x7FFFFFFF;
-	object_type_mask=Physics2DDirectSpaceState::TYPE_MASK_COLLISION;
+	margin = 0;
+	layer_mask = 0x7FFFFFFF;
+	object_type_mask = Physics2DDirectSpaceState::TYPE_MASK_COLLISION;
 }
 
-
-Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) {
+Dictionary Physics2DDirectSpaceState::_intersect_ray(const Vector2 &p_from, const Vector2 &p_to, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) {
 
 	RayResult inters;
 	Set<RID> exclude;
-	for(int i=0;i<p_exclude.size();i++)
+	for (int i = 0; i < p_exclude.size(); i++)
 		exclude.insert(p_exclude[i]);
 
-	bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask);
+	bool res = intersect_ray(p_from, p_to, inters, exclude, p_layers, p_object_type_mask);
 
 	if (!res)
 		return Dictionary(true);
 
 	Dictionary d(true);
-	d["position"]=inters.position;
-	d["normal"]=inters.normal;
-	d["collider_id"]=inters.collider_id;
-	d["collider"]=inters.collider;
-	d["shape"]=inters.shape;
-	d["rid"]=inters.rid;
-	d["metadata"]=inters.metadata;
+	d["position"] = inters.position;
+	d["normal"] = inters.normal;
+	d["collider_id"] = inters.collider_id;
+	d["collider"] = inters.collider;
+	d["shape"] = inters.shape;
+	d["rid"] = inters.rid;
+	d["metadata"] = inters.metadata;
 
 	return d;
 }
@@ -259,131 +248,119 @@ Array Physics2DDirectSpaceState::_intersect_shape(const Ref<Physics2DShapeQueryP
 
 	Vector<ShapeResult> sr;
 	sr.resize(p_max_results);
-	int rc = intersect_shape(psq->shape,psq->transform,psq->motion,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask);
+	int rc = intersect_shape(psq->shape, psq->transform, psq->motion, psq->margin, sr.ptr(), sr.size(), psq->exclude, psq->layer_mask, psq->object_type_mask);
 	Array ret;
 	ret.resize(rc);
-	for(int i=0;i<rc;i++) {
+	for (int i = 0; i < rc; i++) {
 
 		Dictionary d;
-		d["rid"]=sr[i].rid;
-		d["collider_id"]=sr[i].collider_id;
-		d["collider"]=sr[i].collider;
-		d["shape"]=sr[i].shape;
-		d["metadata"]=sr[i].metadata;
-		ret[i]=d;
+		d["rid"] = sr[i].rid;
+		d["collider_id"] = sr[i].collider_id;
+		d["collider"] = sr[i].collider;
+		d["shape"] = sr[i].shape;
+		d["metadata"] = sr[i].metadata;
+		ret[i] = d;
 	}
 
 	return ret;
 }
 
-Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq){
+Array Physics2DDirectSpaceState::_cast_motion(const Ref<Physics2DShapeQueryParameters> &psq) {
 
-	float closest_safe,closest_unsafe;
-	bool res = cast_motion(psq->shape,psq->transform,psq->motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	float closest_safe, closest_unsafe;
+	bool res = cast_motion(psq->shape, psq->transform, psq->motion, psq->margin, closest_safe, closest_unsafe, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	if (!res)
 		return Array();
 	Array ret(true);
 	ret.resize(2);
-	ret[0]=closest_safe;
-	ret[1]=closest_unsafe;
+	ret[0] = closest_safe;
+	ret[1] = closest_unsafe;
 	return ret;
-
 }
 
-Array Physics2DDirectSpaceState::_intersect_point(const Vector2& p_point,int p_max_results,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) {
+Array Physics2DDirectSpaceState::_intersect_point(const Vector2 &p_point, int p_max_results, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) {
 
 	Set<RID> exclude;
-	for(int i=0;i<p_exclude.size();i++)
+	for (int i = 0; i < p_exclude.size(); i++)
 		exclude.insert(p_exclude[i]);
 
 	Vector<ShapeResult> ret;
 	ret.resize(p_max_results);
 
-	int rc = intersect_point(p_point,ret.ptr(),ret.size(),exclude,p_layers,p_object_type_mask);
-	if (rc==0)
+	int rc = intersect_point(p_point, ret.ptr(), ret.size(), exclude, p_layers, p_object_type_mask);
+	if (rc == 0)
 		return Array();
 
 	Array r;
 	r.resize(rc);
-	for(int i=0;i<rc;i++) {
+	for (int i = 0; i < rc; i++) {
 
 		Dictionary d;
-		d["rid"]=ret[i].rid;
-		d["collider_id"]=ret[i].collider_id;
-		d["collider"]=ret[i].collider;
-		d["shape"]=ret[i].shape;
-		d["metadata"]=ret[i].metadata;
-		r[i]=d;
+		d["rid"] = ret[i].rid;
+		d["collider_id"] = ret[i].collider_id;
+		d["collider"] = ret[i].collider;
+		d["shape"] = ret[i].shape;
+		d["metadata"] = ret[i].metadata;
+		r[i] = d;
 	}
 	return r;
-
 }
 
-Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results){
+Array Physics2DDirectSpaceState::_collide_shape(const Ref<Physics2DShapeQueryParameters> &psq, int p_max_results) {
 
 	Vector<Vector2> ret;
-	ret.resize(p_max_results*2);
-	int rc=0;
-	bool res = collide_shape(psq->shape,psq->transform,psq->motion,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	ret.resize(p_max_results * 2);
+	int rc = 0;
+	bool res = collide_shape(psq->shape, psq->transform, psq->motion, psq->margin, ret.ptr(), p_max_results, rc, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	if (!res)
 		return Array();
 	Array r;
-	r.resize(rc*2);
-	for(int i=0;i<rc*2;i++)
-		r[i]=ret[i];
+	r.resize(rc * 2);
+	for (int i = 0; i < rc * 2; i++)
+		r[i] = ret[i];
 	return r;
-
 }
-Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq){
+Dictionary Physics2DDirectSpaceState::_get_rest_info(const Ref<Physics2DShapeQueryParameters> &psq) {
 
 	ShapeRestInfo sri;
 
-	bool res = rest_info(psq->shape,psq->transform,psq->motion,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	bool res = rest_info(psq->shape, psq->transform, psq->motion, psq->margin, &sri, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	Dictionary r(true);
 	if (!res)
 		return r;
 
-	r["point"]=sri.point;
-	r["normal"]=sri.normal;
-	r["rid"]=sri.rid;
-	r["collider_id"]=sri.collider_id;
-	r["shape"]=sri.shape;
-	r["linear_velocity"]=sri.linear_velocity;
-	r["metadata"]=sri.metadata;
+	r["point"] = sri.point;
+	r["normal"] = sri.normal;
+	r["rid"] = sri.rid;
+	r["collider_id"] = sri.collider_id;
+	r["shape"] = sri.shape;
+	r["linear_velocity"] = sri.linear_velocity;
+	r["metadata"] = sri.metadata;
 
 	return r;
 }
 
-
-
 Physics2DDirectSpaceState::Physics2DDirectSpaceState() {
-
-
-
 }
 
-
 void Physics2DDirectSpaceState::_bind_methods() {
 
-
-	ObjectTypeDB::bind_method(_MD("intersect_point","point","max_results","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_point,DEFVAL(32),DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION));
-	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&Physics2DDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION));
-	ObjectTypeDB::bind_method(_MD("intersect_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(32));
-	ObjectTypeDB::bind_method(_MD("cast_motion","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_cast_motion);
-	ObjectTypeDB::bind_method(_MD("collide_shape","shape:Physics2DShapeQueryParameters","max_results"),&Physics2DDirectSpaceState::_collide_shape,DEFVAL(32));
-	ObjectTypeDB::bind_method(_MD("get_rest_info","shape:Physics2DShapeQueryParameters"),&Physics2DDirectSpaceState::_get_rest_info);
+	ObjectTypeDB::bind_method(_MD("intersect_point", "point", "max_results", "exclude", "layer_mask", "type_mask"), &Physics2DDirectSpaceState::_intersect_point, DEFVAL(32), DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION));
+	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary", "from", "to", "exclude", "layer_mask", "type_mask"), &Physics2DDirectSpaceState::_intersect_ray, DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION));
+	ObjectTypeDB::bind_method(_MD("intersect_shape", "shape:Physics2DShapeQueryParameters", "max_results"), &Physics2DDirectSpaceState::_intersect_shape, DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("cast_motion", "shape:Physics2DShapeQueryParameters"), &Physics2DDirectSpaceState::_cast_motion);
+	ObjectTypeDB::bind_method(_MD("collide_shape", "shape:Physics2DShapeQueryParameters", "max_results"), &Physics2DDirectSpaceState::_collide_shape, DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("get_rest_info", "shape:Physics2DShapeQueryParameters"), &Physics2DDirectSpaceState::_get_rest_info);
 	//ObjectTypeDB::bind_method(_MD("cast_motion","shape","xform","motion","exclude","umask"),&Physics2DDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
 
-	BIND_CONSTANT( TYPE_MASK_STATIC_BODY );
-	BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY );
-	BIND_CONSTANT( TYPE_MASK_RIGID_BODY );
-	BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY );
-	BIND_CONSTANT( TYPE_MASK_AREA );
-	BIND_CONSTANT( TYPE_MASK_COLLISION );
-
+	BIND_CONSTANT(TYPE_MASK_STATIC_BODY);
+	BIND_CONSTANT(TYPE_MASK_KINEMATIC_BODY);
+	BIND_CONSTANT(TYPE_MASK_RIGID_BODY);
+	BIND_CONSTANT(TYPE_MASK_CHARACTER_BODY);
+	BIND_CONSTANT(TYPE_MASK_AREA);
+	BIND_CONSTANT(TYPE_MASK_COLLISION);
 }
 
-
 int Physics2DShapeQueryResult::get_result_count() const {
 
 	return result.size();
@@ -396,7 +373,7 @@ ObjectID Physics2DShapeQueryResult::get_result_object_id(int p_idx) const {
 
 	return result[p_idx].collider_id;
 }
-Object* Physics2DShapeQueryResult::get_result_object(int p_idx) const {
+Object *Physics2DShapeQueryResult::get_result_object(int p_idx) const {
 
 	return result[p_idx].collider;
 }
@@ -406,19 +383,15 @@ int Physics2DShapeQueryResult::get_result_object_shape(int p_idx) const {
 }
 
 Physics2DShapeQueryResult::Physics2DShapeQueryResult() {
-
-
 }
 
 void Physics2DShapeQueryResult::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("get_result_count"),&Physics2DShapeQueryResult::get_result_count);
-	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&Physics2DShapeQueryResult::get_result_rid);
-	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&Physics2DShapeQueryResult::get_result_object_id);
-	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&Physics2DShapeQueryResult::get_result_object);
-	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&Physics2DShapeQueryResult::get_result_object_shape);
-
-
+	ObjectTypeDB::bind_method(_MD("get_result_count"), &Physics2DShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid", "idx"), &Physics2DShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id", "idx"), &Physics2DShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object", "idx"), &Physics2DShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape", "idx"), &Physics2DShapeQueryResult::get_result_object_shape);
 }
 
 ///////////////////////////////
@@ -427,41 +400,41 @@ void Physics2DShapeQueryResult::_bind_methods() {
 
 	return colliding;
 }*/
-Vector2 Physics2DTestMotionResult::get_motion() const{
+Vector2 Physics2DTestMotionResult::get_motion() const {
 
 	return result.motion;
 }
-Vector2 Physics2DTestMotionResult::get_motion_remainder() const{
+Vector2 Physics2DTestMotionResult::get_motion_remainder() const {
 
 	return result.remainder;
 }
 
-Vector2 Physics2DTestMotionResult::get_collision_point() const{
+Vector2 Physics2DTestMotionResult::get_collision_point() const {
 
 	return result.collision_point;
 }
-Vector2 Physics2DTestMotionResult::get_collision_normal() const{
+Vector2 Physics2DTestMotionResult::get_collision_normal() const {
 
 	return result.collision_normal;
 }
-Vector2 Physics2DTestMotionResult::get_collider_velocity() const{
+Vector2 Physics2DTestMotionResult::get_collider_velocity() const {
 
 	return result.collider_velocity;
 }
-ObjectID Physics2DTestMotionResult::get_collider_id() const{
+ObjectID Physics2DTestMotionResult::get_collider_id() const {
 
 	return result.collider_id;
 }
-RID Physics2DTestMotionResult::get_collider_rid() const{
+RID Physics2DTestMotionResult::get_collider_rid() const {
 
 	return result.collider;
 }
 
-Object* Physics2DTestMotionResult::get_collider() const {
+Object *Physics2DTestMotionResult::get_collider() const {
 	return ObjectDB::get_instance(result.collider_id);
 }
 
-int Physics2DTestMotionResult::get_collider_shape() const{
+int Physics2DTestMotionResult::get_collider_shape() const {
 
 	return result.collider_shape;
 }
@@ -469,278 +442,263 @@ int Physics2DTestMotionResult::get_collider_shape() const{
 void Physics2DTestMotionResult::_bind_methods() {
 
 	//ObjectTypeDB::bind_method(_MD("is_colliding"),&Physics2DTestMotionResult::is_colliding);
-	ObjectTypeDB::bind_method(_MD("get_motion"),&Physics2DTestMotionResult::get_motion);
-	ObjectTypeDB::bind_method(_MD("get_motion_remainder"),&Physics2DTestMotionResult::get_motion_remainder);
-	ObjectTypeDB::bind_method(_MD("get_collision_point"),&Physics2DTestMotionResult::get_collision_point);
-	ObjectTypeDB::bind_method(_MD("get_collision_normal"),&Physics2DTestMotionResult::get_collision_normal);
-	ObjectTypeDB::bind_method(_MD("get_collider_velocity"),&Physics2DTestMotionResult::get_collider_velocity);
-	ObjectTypeDB::bind_method(_MD("get_collider_id"),&Physics2DTestMotionResult::get_collider_id);
-	ObjectTypeDB::bind_method(_MD("get_collider_rid"),&Physics2DTestMotionResult::get_collider_rid);
-	ObjectTypeDB::bind_method(_MD("get_collider"),&Physics2DTestMotionResult::get_collider);
-	ObjectTypeDB::bind_method(_MD("get_collider_shape"),&Physics2DTestMotionResult::get_collider_shape);
-
+	ObjectTypeDB::bind_method(_MD("get_motion"), &Physics2DTestMotionResult::get_motion);
+	ObjectTypeDB::bind_method(_MD("get_motion_remainder"), &Physics2DTestMotionResult::get_motion_remainder);
+	ObjectTypeDB::bind_method(_MD("get_collision_point"), &Physics2DTestMotionResult::get_collision_point);
+	ObjectTypeDB::bind_method(_MD("get_collision_normal"), &Physics2DTestMotionResult::get_collision_normal);
+	ObjectTypeDB::bind_method(_MD("get_collider_velocity"), &Physics2DTestMotionResult::get_collider_velocity);
+	ObjectTypeDB::bind_method(_MD("get_collider_id"), &Physics2DTestMotionResult::get_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_collider_rid"), &Physics2DTestMotionResult::get_collider_rid);
+	ObjectTypeDB::bind_method(_MD("get_collider"), &Physics2DTestMotionResult::get_collider);
+	ObjectTypeDB::bind_method(_MD("get_collider_shape"), &Physics2DTestMotionResult::get_collider_shape);
 }
 
-Physics2DTestMotionResult::Physics2DTestMotionResult(){
+Physics2DTestMotionResult::Physics2DTestMotionResult() {
 
-	colliding=false;
-	result.collider_id=0;
-	result.collider_shape=0;
+	colliding = false;
+	result.collider_id = 0;
+	result.collider_shape = 0;
 }
 
-
 ///////////////////////////////////////
 
+bool Physics2DServer::_body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin, const Ref<Physics2DTestMotionResult> &p_result) {
 
-
-bool Physics2DServer::_body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
-
-	MotionResult *r=NULL;
+	MotionResult *r = NULL;
 	if (p_result.is_valid())
-		r=p_result->get_result_ptr();
-	return body_test_motion(p_body,p_motion,p_margin,r);
+		r = p_result->get_result_ptr();
+	return body_test_motion(p_body, p_motion, p_margin, r);
 }
 
-bool Physics2DServer::_body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
+bool Physics2DServer::_body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin, const Ref<Physics2DTestMotionResult> &p_result) {
 
-	MotionResult *r=NULL;
+	MotionResult *r = NULL;
 	if (p_result.is_valid())
-		r=p_result->get_result_ptr();
-	return body_test_motion_from(p_body,p_from,p_motion,p_margin,r);
+		r = p_result->get_result_ptr();
+	return body_test_motion_from(p_body, p_from, p_motion, p_margin, r);
 }
 
 void Physics2DServer::_bind_methods() {
 
+	ObjectTypeDB::bind_method(_MD("shape_create", "type"), &Physics2DServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data", "shape", "data"), &Physics2DServer::shape_set_data);
 
-	ObjectTypeDB::bind_method(_MD("shape_create","type"),&Physics2DServer::shape_create);
-	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&Physics2DServer::shape_set_data);
-
-	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&Physics2DServer::shape_get_type);
-	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&Physics2DServer::shape_get_data);
+	ObjectTypeDB::bind_method(_MD("shape_get_type", "shape"), &Physics2DServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data", "shape"), &Physics2DServer::shape_get_data);
 
+	ObjectTypeDB::bind_method(_MD("space_create"), &Physics2DServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active", "space", "active"), &Physics2DServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active", "space"), &Physics2DServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param", "space", "param", "value"), &Physics2DServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param", "space", "param"), &Physics2DServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:Physics2DDirectSpaceState", "space"), &Physics2DServer::space_get_direct_state);
 
-	ObjectTypeDB::bind_method(_MD("space_create"),&Physics2DServer::space_create);
-	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&Physics2DServer::space_set_active);
-	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&Physics2DServer::space_is_active);
-	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&Physics2DServer::space_set_param);
-	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&Physics2DServer::space_get_param);
-	ObjectTypeDB::bind_method(_MD("space_get_direct_state:Physics2DDirectSpaceState","space"),&Physics2DServer::space_get_direct_state);
+	ObjectTypeDB::bind_method(_MD("area_create"), &Physics2DServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space", "area", "space"), &Physics2DServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space", "area"), &Physics2DServer::area_get_space);
 
-	ObjectTypeDB::bind_method(_MD("area_create"),&Physics2DServer::area_create);
-	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&Physics2DServer::area_set_space);
-	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&Physics2DServer::area_get_space);
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode", "area", "mode"), &Physics2DServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode", "area"), &Physics2DServer::area_get_space_override_mode);
 
-	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&Physics2DServer::area_set_space_override_mode);
-	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&Physics2DServer::area_get_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_add_shape", "area", "shape", "transform"), &Physics2DServer::area_add_shape, DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape", "area", "shape_idx", "shape"), &Physics2DServer::area_set_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform", "area", "shape_idx", "transform"), &Physics2DServer::area_set_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&Physics2DServer::area_add_shape,DEFVAL(Matrix32()));
-	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&Physics2DServer::area_set_shape);
-	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&Physics2DServer::area_set_shape_transform);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count", "area"), &Physics2DServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape", "area", "shape_idx"), &Physics2DServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform", "area", "shape_idx"), &Physics2DServer::area_get_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&Physics2DServer::area_get_shape_count);
-	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&Physics2DServer::area_get_shape);
-	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&Physics2DServer::area_get_shape_transform);
+	ObjectTypeDB::bind_method(_MD("area_remove_shape", "area", "shape_idx"), &Physics2DServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes", "area"), &Physics2DServer::area_clear_shapes);
 
-	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&Physics2DServer::area_remove_shape);
-	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&Physics2DServer::area_clear_shapes);
+	ObjectTypeDB::bind_method(_MD("area_set_layer_mask", "area", "mask"), &Physics2DServer::area_set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("area_set_collision_mask", "area", "mask"), &Physics2DServer::area_set_collision_mask);
 
-	ObjectTypeDB::bind_method(_MD("area_set_layer_mask","area","mask"),&Physics2DServer::area_set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("area_set_collision_mask","area","mask"),&Physics2DServer::area_set_collision_mask);
+	ObjectTypeDB::bind_method(_MD("area_set_param", "area", "param", "value"), &Physics2DServer::area_set_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform", "area", "transform"), &Physics2DServer::area_set_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&Physics2DServer::area_set_param);
-	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&Physics2DServer::area_set_transform);
+	ObjectTypeDB::bind_method(_MD("area_get_param", "area", "param"), &Physics2DServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform", "area"), &Physics2DServer::area_get_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&Physics2DServer::area_get_param);
-	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&Physics2DServer::area_get_transform);
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID", "area", "id"), &Physics2DServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID", "area"), &Physics2DServer::area_get_object_instance_ID);
 
-	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&Physics2DServer::area_attach_object_instance_ID);
-	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&Physics2DServer::area_get_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback", "area", "receiver", "method"), &Physics2DServer::area_set_monitor_callback);
 
-	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","area","receiver","method"),&Physics2DServer::area_set_monitor_callback);
+	ObjectTypeDB::bind_method(_MD("body_create", "mode", "init_sleeping"), &Physics2DServer::body_create, DEFVAL(BODY_MODE_RIGID), DEFVAL(false));
 
-	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&Physics2DServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+	ObjectTypeDB::bind_method(_MD("body_set_space", "body", "space"), &Physics2DServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space", "body"), &Physics2DServer::body_get_space);
 
-	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&Physics2DServer::body_set_space);
-	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&Physics2DServer::body_get_space);
+	ObjectTypeDB::bind_method(_MD("body_set_mode", "body", "mode"), &Physics2DServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode", "body"), &Physics2DServer::body_get_mode);
 
-	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&Physics2DServer::body_set_mode);
-	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&Physics2DServer::body_get_mode);
+	ObjectTypeDB::bind_method(_MD("body_add_shape", "body", "shape", "transform"), &Physics2DServer::body_add_shape, DEFVAL(Matrix32()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape", "body", "shape_idx", "shape"), &Physics2DServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform", "body", "shape_idx", "transform"), &Physics2DServer::body_set_shape_transform);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_metadata", "body", "shape_idx", "metadata"), &Physics2DServer::body_set_shape_metadata);
 
-	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&Physics2DServer::body_add_shape,DEFVAL(Matrix32()));
-	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&Physics2DServer::body_set_shape);
-	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&Physics2DServer::body_set_shape_transform);
-	ObjectTypeDB::bind_method(_MD("body_set_shape_metadata","body","shape_idx","metadata"),&Physics2DServer::body_set_shape_metadata);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count", "body"), &Physics2DServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape", "body", "shape_idx"), &Physics2DServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform", "body", "shape_idx"), &Physics2DServer::body_get_shape_transform);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_metadata", "body", "shape_idx"), &Physics2DServer::body_get_shape_metadata);
 
-	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&Physics2DServer::body_get_shape_count);
-	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&Physics2DServer::body_get_shape);
-	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&Physics2DServer::body_get_shape_transform);
-	ObjectTypeDB::bind_method(_MD("body_get_shape_metadata","body","shape_idx"),&Physics2DServer::body_get_shape_metadata);
+	ObjectTypeDB::bind_method(_MD("body_remove_shape", "body", "shape_idx"), &Physics2DServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes", "body"), &Physics2DServer::body_clear_shapes);
 
-	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&Physics2DServer::body_remove_shape);
-	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&Physics2DServer::body_clear_shapes);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_as_trigger", "body", "shape_idx", "enable"), &Physics2DServer::body_set_shape_as_trigger);
+	ObjectTypeDB::bind_method(_MD("body_is_shape_set_as_trigger", "body", "shape_idx"), &Physics2DServer::body_is_shape_set_as_trigger);
 
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID", "body", "id"), &Physics2DServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID", "body"), &Physics2DServer::body_get_object_instance_ID);
 
-	ObjectTypeDB::bind_method(_MD("body_set_shape_as_trigger","body","shape_idx","enable"),&Physics2DServer::body_set_shape_as_trigger);
-	ObjectTypeDB::bind_method(_MD("body_is_shape_set_as_trigger","body","shape_idx"),&Physics2DServer::body_is_shape_set_as_trigger);
+	ObjectTypeDB::bind_method(_MD("body_set_continuous_collision_detection_mode", "body", "mode"), &Physics2DServer::body_set_continuous_collision_detection_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_continuous_collision_detection_mode", "body"), &Physics2DServer::body_get_continuous_collision_detection_mode);
 
-	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&Physics2DServer::body_attach_object_instance_ID);
-	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&Physics2DServer::body_get_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_set_layer_mask", "body", "mask"), &Physics2DServer::body_set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("body_get_layer_mask", "body"), &Physics2DServer::body_get_layer_mask);
 
+	ObjectTypeDB::bind_method(_MD("body_set_collision_mask", "body", "mask"), &Physics2DServer::body_set_collision_mask);
+	ObjectTypeDB::bind_method(_MD("body_get_collision_mask", "body"), &Physics2DServer::body_get_collision_mask);
 
-	ObjectTypeDB::bind_method(_MD("body_set_continuous_collision_detection_mode","body","mode"),&Physics2DServer::body_set_continuous_collision_detection_mode);
-	ObjectTypeDB::bind_method(_MD("body_get_continuous_collision_detection_mode","body"),&Physics2DServer::body_get_continuous_collision_detection_mode);
+	ObjectTypeDB::bind_method(_MD("body_set_param", "body", "param", "value"), &Physics2DServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param", "body", "param"), &Physics2DServer::body_get_param);
 
+	ObjectTypeDB::bind_method(_MD("body_set_state", "body", "state", "value"), &Physics2DServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state", "body", "state"), &Physics2DServer::body_get_state);
 
-	ObjectTypeDB::bind_method(_MD("body_set_layer_mask","body","mask"),&Physics2DServer::body_set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("body_get_layer_mask","body"),&Physics2DServer::body_get_layer_mask);
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse", "body", "pos", "impulse"), &Physics2DServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_add_force", "body", "offset", "force"), &Physics2DServer::body_add_force);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity", "body", "axis_velocity"), &Physics2DServer::body_set_axis_velocity);
 
-	ObjectTypeDB::bind_method(_MD("body_set_collision_mask","body","mask"),&Physics2DServer::body_set_collision_mask);
-	ObjectTypeDB::bind_method(_MD("body_get_collision_mask","body"),&Physics2DServer::body_get_collision_mask);
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception", "body", "excepted_body"), &Physics2DServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception", "body", "excepted_body"), &Physics2DServer::body_remove_collision_exception);
+	//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
 
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported", "body", "amount"), &Physics2DServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported", "body"), &Physics2DServer::body_get_max_contacts_reported);
 
-	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&Physics2DServer::body_set_param);
-	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&Physics2DServer::body_get_param);
+	ObjectTypeDB::bind_method(_MD("body_set_one_way_collision_direction", "body", "normal"), &Physics2DServer::body_set_one_way_collision_direction);
+	ObjectTypeDB::bind_method(_MD("body_get_one_way_collision_direction", "body"), &Physics2DServer::body_get_one_way_collision_direction);
 
-	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&Physics2DServer::body_set_state);
-	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&Physics2DServer::body_get_state);
+	ObjectTypeDB::bind_method(_MD("body_set_one_way_collision_max_depth", "body", "depth"), &Physics2DServer::body_set_one_way_collision_max_depth);
+	ObjectTypeDB::bind_method(_MD("body_get_one_way_collision_max_depth", "body"), &Physics2DServer::body_get_one_way_collision_max_depth);
 
-	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&Physics2DServer::body_apply_impulse);
-	ObjectTypeDB::bind_method(_MD("body_add_force","body","offset","force"),&Physics2DServer::body_add_force);
-	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&Physics2DServer::body_set_axis_velocity);
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration", "body", "enable"), &Physics2DServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration", "body"), &Physics2DServer::body_is_omitting_force_integration);
 
-	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&Physics2DServer::body_add_collision_exception);
-	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&Physics2DServer::body_remove_collision_exception);
-//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &Physics2DServer::body_set_force_integration_callback, DEFVAL(Variant()));
 
-	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&Physics2DServer::body_set_max_contacts_reported);
-	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&Physics2DServer::body_get_max_contacts_reported);
-
-	ObjectTypeDB::bind_method(_MD("body_set_one_way_collision_direction","body","normal"),&Physics2DServer::body_set_one_way_collision_direction);
-	ObjectTypeDB::bind_method(_MD("body_get_one_way_collision_direction","body"),&Physics2DServer::body_get_one_way_collision_direction);
-
-	ObjectTypeDB::bind_method(_MD("body_set_one_way_collision_max_depth","body","depth"),&Physics2DServer::body_set_one_way_collision_max_depth);
-	ObjectTypeDB::bind_method(_MD("body_get_one_way_collision_max_depth","body"),&Physics2DServer::body_get_one_way_collision_max_depth);
-
-
-	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&Physics2DServer::body_set_omit_force_integration);
-	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&Physics2DServer::body_is_omitting_force_integration);
-
-	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&Physics2DServer::body_set_force_integration_callback,DEFVAL(Variant()));
-
-	ObjectTypeDB::bind_method(_MD("body_test_motion","body","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant()));
-	ObjectTypeDB::bind_method(_MD("body_test_motion_from","body","from","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion_from,DEFVAL(0.08),DEFVAL(Variant()));
+	ObjectTypeDB::bind_method(_MD("body_test_motion", "body", "motion", "margin", "result:Physics2DTestMotionResult"), &Physics2DServer::_body_test_motion, DEFVAL(0.08), DEFVAL(Variant()));
+	ObjectTypeDB::bind_method(_MD("body_test_motion_from", "body", "from", "motion", "margin", "result:Physics2DTestMotionResult"), &Physics2DServer::_body_test_motion_from, DEFVAL(0.08), DEFVAL(Variant()));
 
 	/* JOINT API */
 
-	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&Physics2DServer::joint_set_param);
-	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&Physics2DServer::joint_get_param);
+	ObjectTypeDB::bind_method(_MD("joint_set_param", "joint", "param", "value"), &Physics2DServer::joint_set_param);
+	ObjectTypeDB::bind_method(_MD("joint_get_param", "joint", "param"), &Physics2DServer::joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("pin_joint_create","anchor","body_a","body_b"),&Physics2DServer::pin_joint_create,DEFVAL(RID()));
-	ObjectTypeDB::bind_method(_MD("groove_joint_create","groove1_a","groove2_a","anchor_b","body_a","body_b"),&Physics2DServer::groove_joint_create,DEFVAL(RID()),DEFVAL(RID()));
-	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create","anchor_a","anchor_b","body_a","body_b"),&Physics2DServer::damped_spring_joint_create,DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("pin_joint_create", "anchor", "body_a", "body_b"), &Physics2DServer::pin_joint_create, DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("groove_joint_create", "groove1_a", "groove2_a", "anchor_b", "body_a", "body_b"), &Physics2DServer::groove_joint_create, DEFVAL(RID()), DEFVAL(RID()));
+	ObjectTypeDB::bind_method(_MD("damped_spring_joint_create", "anchor_a", "anchor_b", "body_a", "body_b"), &Physics2DServer::damped_spring_joint_create, DEFVAL(RID()));
 
-	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param","joint","param","value"),&Physics2DServer::damped_string_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param","joint","param"),&Physics2DServer::damped_string_joint_get_param);
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_set_param", "joint", "param", "value"), &Physics2DServer::damped_string_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("damped_string_joint_get_param", "joint", "param"), &Physics2DServer::damped_string_joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&Physics2DServer::joint_get_type);
+	ObjectTypeDB::bind_method(_MD("joint_get_type", "joint"), &Physics2DServer::joint_get_type);
 
-	ObjectTypeDB::bind_method(_MD("free_rid","rid"),&Physics2DServer::free);
+	ObjectTypeDB::bind_method(_MD("free_rid", "rid"), &Physics2DServer::free);
 
-	ObjectTypeDB::bind_method(_MD("set_active","active"),&Physics2DServer::set_active);
+	ObjectTypeDB::bind_method(_MD("set_active", "active"), &Physics2DServer::set_active);
 
-	ObjectTypeDB::bind_method(_MD("get_process_info","process_info"),&Physics2DServer::get_process_info);
+	ObjectTypeDB::bind_method(_MD("get_process_info", "process_info"), &Physics2DServer::get_process_info);
 
-//	ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init);
-//	ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step);
-//	ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync);
+	//	ObjectTypeDB::bind_method(_MD("init"),&Physics2DServer::init);
+	//	ObjectTypeDB::bind_method(_MD("step"),&Physics2DServer::step);
+	//	ObjectTypeDB::bind_method(_MD("sync"),&Physics2DServer::sync);
 	//ObjectTypeDB::bind_method(_MD("flush_queries"),&Physics2DServer::flush_queries);
 
-	BIND_CONSTANT( SPACE_PARAM_CONTACT_RECYCLE_RADIUS );
-	BIND_CONSTANT( SPACE_PARAM_CONTACT_MAX_SEPARATION );
-	BIND_CONSTANT( SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION );
-	BIND_CONSTANT( SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD );
-	BIND_CONSTANT( SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD );
-	BIND_CONSTANT( SPACE_PARAM_BODY_TIME_TO_SLEEP );
-	BIND_CONSTANT( SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS );
-
-	BIND_CONSTANT( SHAPE_LINE );
-	BIND_CONSTANT( SHAPE_SEGMENT );
-	BIND_CONSTANT( SHAPE_CIRCLE );
-	BIND_CONSTANT( SHAPE_RECTANGLE );
-	BIND_CONSTANT( SHAPE_CAPSULE );
-	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
-	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
-	BIND_CONSTANT( SHAPE_CUSTOM );
-
-	BIND_CONSTANT( AREA_PARAM_GRAVITY );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_DISTANCE_SCALE );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
-	BIND_CONSTANT( AREA_PARAM_LINEAR_DAMP);
-	BIND_CONSTANT( AREA_PARAM_ANGULAR_DAMP);
-	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+	BIND_CONSTANT(SPACE_PARAM_CONTACT_RECYCLE_RADIUS);
+	BIND_CONSTANT(SPACE_PARAM_CONTACT_MAX_SEPARATION);
+	BIND_CONSTANT(SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION);
+	BIND_CONSTANT(SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD);
+	BIND_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD);
+	BIND_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
+	BIND_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
 
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE_REPLACE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE_COMBINE );
+	BIND_CONSTANT(SHAPE_LINE);
+	BIND_CONSTANT(SHAPE_SEGMENT);
+	BIND_CONSTANT(SHAPE_CIRCLE);
+	BIND_CONSTANT(SHAPE_RECTANGLE);
+	BIND_CONSTANT(SHAPE_CAPSULE);
+	BIND_CONSTANT(SHAPE_CONVEX_POLYGON);
+	BIND_CONSTANT(SHAPE_CONCAVE_POLYGON);
+	BIND_CONSTANT(SHAPE_CUSTOM);
 
-	BIND_CONSTANT( BODY_MODE_STATIC );
-	BIND_CONSTANT( BODY_MODE_KINEMATIC );
-	BIND_CONSTANT( BODY_MODE_RIGID );
-	BIND_CONSTANT( BODY_MODE_CHARACTER );
+	BIND_CONSTANT(AREA_PARAM_GRAVITY);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_VECTOR);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_IS_POINT);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_DISTANCE_SCALE);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_POINT_ATTENUATION);
+	BIND_CONSTANT(AREA_PARAM_LINEAR_DAMP);
+	BIND_CONSTANT(AREA_PARAM_ANGULAR_DAMP);
+	BIND_CONSTANT(AREA_PARAM_PRIORITY);
 
-	BIND_CONSTANT( BODY_PARAM_BOUNCE );
-	BIND_CONSTANT( BODY_PARAM_FRICTION );
-	BIND_CONSTANT( BODY_PARAM_MASS );
-	BIND_CONSTANT( BODY_PARAM_INERTIA );
-	BIND_CONSTANT( BODY_PARAM_GRAVITY_SCALE );
-	BIND_CONSTANT( BODY_PARAM_LINEAR_DAMP);
-	BIND_CONSTANT( BODY_PARAM_ANGULAR_DAMP);
-	BIND_CONSTANT( BODY_PARAM_MAX );
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE_REPLACE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE_COMBINE);
 
-	BIND_CONSTANT( BODY_STATE_TRANSFORM );
-	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
-	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
-	BIND_CONSTANT( BODY_STATE_SLEEPING );
-	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
+	BIND_CONSTANT(BODY_MODE_STATIC);
+	BIND_CONSTANT(BODY_MODE_KINEMATIC);
+	BIND_CONSTANT(BODY_MODE_RIGID);
+	BIND_CONSTANT(BODY_MODE_CHARACTER);
 
-	BIND_CONSTANT( JOINT_PIN );
-	BIND_CONSTANT( JOINT_GROOVE );
-	BIND_CONSTANT( JOINT_DAMPED_SPRING );
+	BIND_CONSTANT(BODY_PARAM_BOUNCE);
+	BIND_CONSTANT(BODY_PARAM_FRICTION);
+	BIND_CONSTANT(BODY_PARAM_MASS);
+	BIND_CONSTANT(BODY_PARAM_INERTIA);
+	BIND_CONSTANT(BODY_PARAM_GRAVITY_SCALE);
+	BIND_CONSTANT(BODY_PARAM_LINEAR_DAMP);
+	BIND_CONSTANT(BODY_PARAM_ANGULAR_DAMP);
+	BIND_CONSTANT(BODY_PARAM_MAX);
 
-	BIND_CONSTANT( DAMPED_STRING_REST_LENGTH );
-	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
-	BIND_CONSTANT( DAMPED_STRING_DAMPING );
+	BIND_CONSTANT(BODY_STATE_TRANSFORM);
+	BIND_CONSTANT(BODY_STATE_LINEAR_VELOCITY);
+	BIND_CONSTANT(BODY_STATE_ANGULAR_VELOCITY);
+	BIND_CONSTANT(BODY_STATE_SLEEPING);
+	BIND_CONSTANT(BODY_STATE_CAN_SLEEP);
 
-	BIND_CONSTANT( CCD_MODE_DISABLED );
-	BIND_CONSTANT( CCD_MODE_CAST_RAY );
-	BIND_CONSTANT( CCD_MODE_CAST_SHAPE );
+	BIND_CONSTANT(JOINT_PIN);
+	BIND_CONSTANT(JOINT_GROOVE);
+	BIND_CONSTANT(JOINT_DAMPED_SPRING);
 
-//	BIND_CONSTANT( TYPE_BODY );
-//	BIND_CONSTANT( TYPE_AREA );
+	BIND_CONSTANT(DAMPED_STRING_REST_LENGTH);
+	BIND_CONSTANT(DAMPED_STRING_STIFFNESS);
+	BIND_CONSTANT(DAMPED_STRING_DAMPING);
 
-	BIND_CONSTANT( AREA_BODY_ADDED );
-	BIND_CONSTANT( AREA_BODY_REMOVED );
+	BIND_CONSTANT(CCD_MODE_DISABLED);
+	BIND_CONSTANT(CCD_MODE_CAST_RAY);
+	BIND_CONSTANT(CCD_MODE_CAST_SHAPE);
 
-	BIND_CONSTANT( INFO_ACTIVE_OBJECTS );
-	BIND_CONSTANT( INFO_COLLISION_PAIRS );
-	BIND_CONSTANT( INFO_ISLAND_COUNT );
+	//	BIND_CONSTANT( TYPE_BODY );
+	//	BIND_CONSTANT( TYPE_AREA );
 
+	BIND_CONSTANT(AREA_BODY_ADDED);
+	BIND_CONSTANT(AREA_BODY_REMOVED);
 
+	BIND_CONSTANT(INFO_ACTIVE_OBJECTS);
+	BIND_CONSTANT(INFO_COLLISION_PAIRS);
+	BIND_CONSTANT(INFO_ISLAND_COUNT);
 }
 
-
 Physics2DServer::Physics2DServer() {
 
 	//ERR_FAIL_COND( singleton!=NULL );
-	singleton=this;
+	singleton = this;
 }
 
 Physics2DServer::~Physics2DServer() {
 
-	singleton=NULL;
+	singleton = NULL;
 }
-
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index cd65c01b0..a1406d171 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -37,61 +37,60 @@ class Physics2DDirectSpaceState;
 
 class Physics2DDirectBodyState : public Object {
 
-	OBJ_TYPE( Physics2DDirectBodyState, Object );
+	OBJ_TYPE(Physics2DDirectBodyState, Object);
+
 protected:
 	static void _bind_methods();
-public:
 
-	virtual Vector2 get_total_gravity() const=0; // get gravity vector working on this body space/area
-	virtual float get_total_linear_damp() const=0; // get density of this body space/area
-	virtual float get_total_angular_damp() const=0; // get density of this body space/area
+public:
+	virtual Vector2 get_total_gravity() const = 0; // get gravity vector working on this body space/area
+	virtual float get_total_linear_damp() const = 0; // get density of this body space/area
+	virtual float get_total_angular_damp() const = 0; // get density of this body space/area
 
-	virtual float get_inverse_mass() const=0; // get the mass
-	virtual real_t get_inverse_inertia() const=0; // get density of this body space
+	virtual float get_inverse_mass() const = 0; // get the mass
+	virtual real_t get_inverse_inertia() const = 0; // get density of this body space
 
-	virtual void set_linear_velocity(const Vector2& p_velocity)=0;
-	virtual Vector2 get_linear_velocity() const=0;
+	virtual void set_linear_velocity(const Vector2 &p_velocity) = 0;
+	virtual Vector2 get_linear_velocity() const = 0;
 
-	virtual void set_angular_velocity(real_t p_velocity)=0;
-	virtual real_t get_angular_velocity() const=0;
+	virtual void set_angular_velocity(real_t p_velocity) = 0;
+	virtual real_t get_angular_velocity() const = 0;
 
-	virtual void set_transform(const Matrix32& p_transform)=0;
-	virtual Matrix32 get_transform() const=0;
+	virtual void set_transform(const Matrix32 &p_transform) = 0;
+	virtual Matrix32 get_transform() const = 0;
 
-	virtual void set_sleep_state(bool p_enable)=0;
-	virtual bool is_sleeping() const=0;
+	virtual void set_sleep_state(bool p_enable) = 0;
+	virtual bool is_sleeping() const = 0;
 
-	virtual int get_contact_count() const=0;
+	virtual int get_contact_count() const = 0;
 
-	virtual Vector2 get_contact_local_pos(int p_contact_idx) const=0;
-	virtual Vector2 get_contact_local_normal(int p_contact_idx) const=0;
-	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+	virtual Vector2 get_contact_local_pos(int p_contact_idx) const = 0;
+	virtual Vector2 get_contact_local_normal(int p_contact_idx) const = 0;
+	virtual int get_contact_local_shape(int p_contact_idx) const = 0;
 
-	virtual RID get_contact_collider(int p_contact_idx) const=0;
-	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const=0;
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
-	virtual Object* get_contact_collider_object(int p_contact_idx) const;
-	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
-	virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const=0;
-	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+	virtual RID get_contact_collider(int p_contact_idx) const = 0;
+	virtual Vector2 get_contact_collider_pos(int p_contact_idx) const = 0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const = 0;
+	virtual Object *get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const = 0;
+	virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const = 0;
+	virtual Vector2 get_contact_collider_velocity_at_pos(int p_contact_idx) const = 0;
 
-	virtual real_t get_step() const=0;
+	virtual real_t get_step() const = 0;
 	virtual void integrate_forces();
 
-	virtual Physics2DDirectSpaceState* get_space_state()=0;
+	virtual Physics2DDirectSpaceState *get_space_state() = 0;
 
 	Physics2DDirectBodyState();
 };
 
-
 class Physics2DShapeQueryResult;
 
-
 //used for script
 class Physics2DShapeQueryParameters : public Reference {
 
 	OBJ_TYPE(Physics2DShapeQueryParameters, Reference);
-friend class Physics2DDirectSpaceState;
+	friend class Physics2DDirectSpaceState;
 	RID shape;
 	Matrix32 transform;
 	Vector2 motion;
@@ -99,19 +98,19 @@ friend class Physics2DDirectSpaceState;
 	Set<RID> exclude;
 	uint32_t layer_mask;
 	uint32_t object_type_mask;
+
 protected:
 	static void _bind_methods();
-public:
-
 
-	void set_shape(const RES& p_shape);
-	void set_shape_rid(const RID& p_shape);
+public:
+	void set_shape(const RES &p_shape);
+	void set_shape_rid(const RID &p_shape);
 	RID get_shape_rid() const;
 
-	void set_transform(const Matrix32& p_transform);
+	void set_transform(const Matrix32 &p_transform);
 	Matrix32 get_transform() const;
 
-	void set_motion(const Vector2& p_motion);
+	void set_motion(const Vector2 &p_motion);
 	Vector2 get_motion() const;
 
 	void set_margin(float p_margin);
@@ -123,38 +122,35 @@ public:
 	void set_object_type_mask(int p_object_type_mask);
 	int get_object_type_mask() const;
 
-	void set_exclude(const Vector<RID>& p_exclude);
+	void set_exclude(const Vector<RID> &p_exclude);
 	Vector<RID> get_exclude() const;
 
 	Physics2DShapeQueryParameters();
-
 };
 
-
 class Physics2DDirectSpaceState : public Object {
 
-	OBJ_TYPE( Physics2DDirectSpaceState, Object );
+	OBJ_TYPE(Physics2DDirectSpaceState, Object);
 
-	Dictionary _intersect_ray(const Vector2& p_from, const Vector2& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
+	Dictionary _intersect_ray(const Vector2 &p_from, const Vector2 &p_to, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
 
-	Array _intersect_point(const Vector2& p_point,int p_max_results=32,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32);
+	Array _intersect_point(const Vector2 &p_point, int p_max_results = 32, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	Array _intersect_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query, int p_max_results = 32);
 	Array _cast_motion(const Ref<Physics2DShapeQueryParameters> &p_shape_query);
-	Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query,int p_max_results=32);
+	Array _collide_shape(const Ref<Physics2DShapeQueryParameters> &p_shape_query, int p_max_results = 32);
 	Dictionary _get_rest_info(const Ref<Physics2DShapeQueryParameters> &p_shape_query);
 
 protected:
 	static void _bind_methods();
 
 public:
-
 	enum ObjectTypeMask {
-		TYPE_MASK_STATIC_BODY=1<<0,
-		TYPE_MASK_KINEMATIC_BODY=1<<1,
-		TYPE_MASK_RIGID_BODY=1<<2,
-		TYPE_MASK_CHARACTER_BODY=1<<3,
-		TYPE_MASK_AREA=1<<4,
-		TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
+		TYPE_MASK_STATIC_BODY = 1 << 0,
+		TYPE_MASK_KINEMATIC_BODY = 1 << 1,
+		TYPE_MASK_RIGID_BODY = 1 << 2,
+		TYPE_MASK_CHARACTER_BODY = 1 << 3,
+		TYPE_MASK_AREA = 1 << 4,
+		TYPE_MASK_COLLISION = TYPE_MASK_STATIC_BODY | TYPE_MASK_CHARACTER_BODY | TYPE_MASK_KINEMATIC_BODY | TYPE_MASK_RIGID_BODY
 
 	};
 
@@ -169,7 +165,7 @@ public:
 		Variant metadata;
 	};
 
-	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual bool intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	struct ShapeResult {
 
@@ -178,17 +174,15 @@ public:
 		Object *collider;
 		int shape;
 		Variant metadata;
-
-
 	};
 
-	virtual int intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,bool p_pick_point=false)=0;
+	virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_point = false) = 0;
 
-	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual int intersect_shape(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
-	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual bool cast_motion(const RID &p_shape, const Matrix32 &p_xform, const Vector2 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
-	virtual bool collide_shape(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,Vector2 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual bool collide_shape(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	struct ShapeRestInfo {
 
@@ -199,32 +193,29 @@ public:
 		int shape;
 		Vector2 linear_velocity; //velocity at contact point
 		Variant metadata;
-
 	};
 
-	virtual bool rest_info(RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
-
+	virtual bool rest_info(RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	Physics2DDirectSpaceState();
 };
 
-
 class Physics2DShapeQueryResult : public Reference {
 
-	OBJ_TYPE( Physics2DShapeQueryResult, Reference );
+	OBJ_TYPE(Physics2DShapeQueryResult, Reference);
 
 	Vector<Physics2DDirectSpaceState::ShapeResult> result;
 
-friend class Physics2DDirectSpaceState;
+	friend class Physics2DDirectSpaceState;
 
 protected:
 	static void _bind_methods();
-public:
 
+public:
 	int get_result_count() const;
 	RID get_result_rid(int p_idx) const;
 	ObjectID get_result_object_id(int p_idx) const;
-	Object* get_result_object(int p_idx) const;
+	Object *get_result_object(int p_idx) const;
 	int get_result_object_shape(int p_idx) const;
 
 	Physics2DShapeQueryResult();
@@ -234,19 +225,18 @@ class Physics2DTestMotionResult;
 
 class Physics2DServer : public Object {
 
-	OBJ_TYPE( Physics2DServer, Object );
+	OBJ_TYPE(Physics2DServer, Object);
 
-	static Physics2DServer * singleton;
+	static Physics2DServer *singleton;
 
-	virtual bool _body_test_motion(RID p_body, const Vector2& p_motion, float p_margin=0.08, const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
-	virtual bool _body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin=0.08, const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
+	virtual bool _body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin = 0.08, const Ref<Physics2DTestMotionResult> &p_result = Ref<Physics2DTestMotionResult>());
+	virtual bool _body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin = 0.08, const Ref<Physics2DTestMotionResult> &p_result = Ref<Physics2DTestMotionResult>());
 
 protected:
 	static void _bind_methods();
 
 public:
-
-	static Physics2DServer * get_singleton();
+	static Physics2DServer *get_singleton();
 
 	enum ShapeType {
 		SHAPE_LINE, ///< plane:"plane"
@@ -260,22 +250,22 @@ public:
 		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
 	};
 
-	virtual RID shape_create(ShapeType p_shape)=0;
-	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
-	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
+	virtual RID shape_create(ShapeType p_shape) = 0;
+	virtual void shape_set_data(RID p_shape, const Variant &p_data) = 0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) = 0;
 
-	virtual ShapeType shape_get_type(RID p_shape) const=0;
-	virtual Variant shape_get_data(RID p_shape) const=0;
-	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+	virtual ShapeType shape_get_type(RID p_shape) const = 0;
+	virtual Variant shape_get_data(RID p_shape) const = 0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const = 0;
 
 	//these work well, but should be used from the main thread only
-	virtual bool shape_collide(RID p_shape_A, const Matrix32& p_xform_A,const Vector2& p_motion_A,RID p_shape_B, const Matrix32& p_xform_B, const Vector2& p_motion_B,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
+	virtual bool shape_collide(RID p_shape_A, const Matrix32 &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Matrix32 &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) = 0;
 
 	/* SPACE API */
 
-	virtual RID space_create()=0;
-	virtual void space_set_active(RID p_space,bool p_active)=0;
-	virtual bool space_is_active(RID p_space) const=0;
+	virtual RID space_create() = 0;
+	virtual void space_set_active(RID p_space, bool p_active) = 0;
+	virtual bool space_is_active(RID p_space) const = 0;
 
 	enum SpaceParameter {
 
@@ -288,15 +278,15 @@ public:
 		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
 	};
 
-	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
-	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const = 0;
 
 	// this function only works on fixed process, errors and returns null otherwise
-	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space)=0;
+	virtual Physics2DDirectSpaceState *space_get_direct_state(RID p_space) = 0;
 
-	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0;
-	virtual Vector<Vector2> space_get_contacts(RID p_space) const=0;
-	virtual int space_get_contact_count(RID p_space) const=0;
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) = 0;
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const = 0;
+	virtual int space_get_contact_count(RID p_space) const = 0;
 
 	//missing space parameters
 
@@ -304,8 +294,6 @@ public:
 
 	//missing attenuation? missing better override?
 
-
-
 	enum AreaParameter {
 		AREA_PARAM_GRAVITY,
 		AREA_PARAM_GRAVITY_VECTOR,
@@ -317,11 +305,10 @@ public:
 		AREA_PARAM_PRIORITY
 	};
 
-	virtual RID area_create()=0;
-
-	virtual void area_set_space(RID p_area, RID p_space)=0;
-	virtual RID area_get_space(RID p_area) const=0;
+	virtual RID area_create() = 0;
 
+	virtual void area_set_space(RID p_area, RID p_space) = 0;
+	virtual RID area_get_space(RID p_area) const = 0;
 
 	enum AreaSpaceOverrideMode {
 		AREA_SPACE_OVERRIDE_DISABLED,
@@ -331,37 +318,37 @@ public:
 		AREA_SPACE_OVERRIDE_REPLACE_COMBINE // Discards all previous calculations, then keeps combining
 	};
 
-	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
-	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) = 0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const = 0;
 
-	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
-	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
-	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32& p_transform)=0;
+	virtual void area_add_shape(RID p_area, RID p_shape, const Matrix32 &p_transform = Matrix32()) = 0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) = 0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Matrix32 &p_transform) = 0;
 
-	virtual int area_get_shape_count(RID p_area) const=0;
-	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
-	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+	virtual int area_get_shape_count(RID p_area) const = 0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const = 0;
+	virtual Matrix32 area_get_shape_transform(RID p_area, int p_shape_idx) const = 0;
 
-	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
-	virtual void area_clear_shapes(RID p_area)=0;
+	virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0;
+	virtual void area_clear_shapes(RID p_area) = 0;
 
-	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
-	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0;
 
-	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
-	virtual void area_set_transform(RID p_area, const Matrix32& p_transform)=0;
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) = 0;
+	virtual void area_set_transform(RID p_area, const Matrix32 &p_transform) = 0;
 
-	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
-	virtual Matrix32 area_get_transform(RID p_area) const=0;
+	virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const = 0;
+	virtual Matrix32 area_get_transform(RID p_area) const = 0;
 
-	virtual void area_set_collision_mask(RID p_area,uint32_t p_mask)=0;
-	virtual void area_set_layer_mask(RID p_area,uint32_t p_mask)=0;
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) = 0;
+	virtual void area_set_layer_mask(RID p_area, uint32_t p_mask) = 0;
 
-	virtual void area_set_monitorable(RID p_area,bool p_monitorable)=0;
-	virtual void area_set_pickable(RID p_area,bool p_pickable)=0;
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable) = 0;
+	virtual void area_set_pickable(RID p_area, bool p_pickable) = 0;
 
-	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
-	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0;
+	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0;
 
 	/* BODY API */
 
@@ -375,32 +362,32 @@ public:
 		//BODY_MODE_SOFT ??
 	};
 
-	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+	virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) = 0;
 
-	virtual void body_set_space(RID p_body, RID p_space)=0;
-	virtual RID body_get_space(RID p_body) const=0;
+	virtual void body_set_space(RID p_body, RID p_space) = 0;
+	virtual RID body_get_space(RID p_body) const = 0;
 
-	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
-	virtual BodyMode body_get_mode(RID p_body) const=0;
+	virtual void body_set_mode(RID p_body, BodyMode p_mode) = 0;
+	virtual BodyMode body_get_mode(RID p_body) const = 0;
 
-	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32& p_transform=Matrix32())=0;
-	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
-	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32& p_transform)=0;
-	virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant& p_metadata)=0;
+	virtual void body_add_shape(RID p_body, RID p_shape, const Matrix32 &p_transform = Matrix32()) = 0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) = 0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Matrix32 &p_transform) = 0;
+	virtual void body_set_shape_metadata(RID p_body, int p_shape_idx, const Variant &p_metadata) = 0;
 
-	virtual int body_get_shape_count(RID p_body) const=0;
-	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
-	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
-	virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const=0;
+	virtual int body_get_shape_count(RID p_body) const = 0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0;
+	virtual Matrix32 body_get_shape_transform(RID p_body, int p_shape_idx) const = 0;
+	virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
-	virtual void body_clear_shapes(RID p_body)=0;
+	virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0;
+	virtual void body_clear_shapes(RID p_body) = 0;
 
-	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
-	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0;
 
 	enum CCDMode {
 		CCD_MODE_DISABLED,
@@ -408,30 +395,29 @@ public:
 		CCD_MODE_CAST_SHAPE,
 	};
 
-	virtual void body_set_continuous_collision_detection_mode(RID p_body,CCDMode p_mode)=0;
-	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const=0;
+	virtual void body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) = 0;
+	virtual CCDMode body_get_continuous_collision_detection_mode(RID p_body) const = 0;
 
-	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0;
-	virtual uint32_t body_get_layer_mask(RID p_body) const=0;
+	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask) = 0;
+	virtual uint32_t body_get_layer_mask(RID p_body) const = 0;
 
-	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask)=0;
-	virtual uint32_t body_get_collision_mask(RID p_body) const=0;
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0;
+	virtual uint32_t body_get_collision_mask(RID p_body) const = 0;
 
 	// common body variables
 	enum BodyParameter {
 		BODY_PARAM_BOUNCE,
 		BODY_PARAM_FRICTION,
 		BODY_PARAM_MASS, ///< unused for static, always infinite
-		BODY_PARAM_INERTIA,  // read-only: computed from mass & shapes
+		BODY_PARAM_INERTIA, // read-only: computed from mass & shapes
 		BODY_PARAM_GRAVITY_SCALE,
 		BODY_PARAM_LINEAR_DAMP,
 		BODY_PARAM_ANGULAR_DAMP,
 		BODY_PARAM_MAX,
 	};
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
-	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
-
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0;
 
 	//state
 	enum BodyState {
@@ -442,47 +428,47 @@ public:
 		BODY_STATE_CAN_SLEEP,
 	};
 
-	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
-	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const = 0;
 
 	//do something about it
-	virtual void body_set_applied_force(RID p_body, const Vector2& p_force)=0;
-	virtual Vector2 body_get_applied_force(RID p_body) const=0;
+	virtual void body_set_applied_force(RID p_body, const Vector2 &p_force) = 0;
+	virtual Vector2 body_get_applied_force(RID p_body) const = 0;
 
-	virtual void body_set_applied_torque(RID p_body, float p_torque)=0;
-	virtual float body_get_applied_torque(RID p_body) const=0;
+	virtual void body_set_applied_torque(RID p_body, float p_torque) = 0;
+	virtual float body_get_applied_torque(RID p_body) const = 0;
 
-	virtual void body_add_force(RID p_body, const Vector2& p_offset, const Vector2& p_force)=0;
+	virtual void body_add_force(RID p_body, const Vector2 &p_offset, const Vector2 &p_force) = 0;
 
-	virtual void body_apply_impulse(RID p_body, const Vector2& p_offset, const Vector2& p_impulse)=0;
-	virtual void body_set_axis_velocity(RID p_body, const Vector2& p_axis_velocity)=0;
+	virtual void body_apply_impulse(RID p_body, const Vector2 &p_offset, const Vector2 &p_impulse) = 0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector2 &p_axis_velocity) = 0;
 
 	//fix
-	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
-	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
-	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) = 0;
 
-	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
-	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) = 0;
+	virtual int body_get_max_contacts_reported(RID p_body) const = 0;
 
-	virtual void body_set_one_way_collision_direction(RID p_body,const Vector2& p_direction)=0;
-	virtual Vector2 body_get_one_way_collision_direction(RID p_body) const=0;
+	virtual void body_set_one_way_collision_direction(RID p_body, const Vector2 &p_direction) = 0;
+	virtual Vector2 body_get_one_way_collision_direction(RID p_body) const = 0;
 
-	virtual void body_set_one_way_collision_max_depth(RID p_body,float p_max_depth)=0;
-	virtual float body_get_one_way_collision_max_depth(RID p_body) const=0;
+	virtual void body_set_one_way_collision_max_depth(RID p_body, float p_max_depth) = 0;
+	virtual float body_get_one_way_collision_max_depth(RID p_body) const = 0;
 
 	//missing remove
-	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
-	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) = 0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const = 0;
 
-	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
-	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
 
-	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) = 0;
 
-	virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count)=0;
+	virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Matrix32 &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) = 0;
 
-	virtual void body_set_pickable(RID p_body,bool p_pickable)=0;
+	virtual void body_set_pickable(RID p_body, bool p_pickable) = 0;
 
 	struct MotionResult {
 
@@ -498,8 +484,8 @@ public:
 		Variant collider_metadata;
 	};
 
-	virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0;
-	virtual bool body_test_motion_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0;
+	virtual bool body_test_motion(RID p_body, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0;
+	virtual bool body_test_motion_from(RID p_body, const Matrix32 &p_from, const Vector2 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL) = 0;
 
 	/* JOINT API */
 
@@ -516,29 +502,29 @@ public:
 		JOINT_PARAM_MAX_FORCE,
 	};
 
-	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value)=0;
-	virtual real_t joint_get_param(RID p_joint,JointParam p_param) const=0;
+	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) = 0;
+	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const = 0;
 
-	virtual RID pin_joint_create(const Vector2& p_anchor,RID p_body_a,RID p_body_b=RID())=0;
-	virtual RID groove_joint_create(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, RID p_body_a,RID p_body_b)=0;
-	virtual RID damped_spring_joint_create(const Vector2& p_anchor_a,const Vector2& p_anchor_b,RID p_body_a,RID p_body_b=RID())=0;
+	virtual RID pin_joint_create(const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) = 0;
+	virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) = 0;
+	virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) = 0;
 
 	enum PinJointParam {
 		PIN_JOINT_SOFTNESS
 	};
 
-	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value)=0;
-	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const=0;
+	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) = 0;
+	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const = 0;
 
 	enum DampedStringParam {
 		DAMPED_STRING_REST_LENGTH,
 		DAMPED_STRING_STIFFNESS,
 		DAMPED_STRING_DAMPING
 	};
-	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value)=0;
-	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const=0;
+	virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) = 0;
+	virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const = 0;
 
-	virtual JointType joint_get_type(RID p_joint) const=0;
+	virtual JointType joint_get_type(RID p_joint) const = 0;
 
 	/* QUERY API */
 
@@ -547,18 +533,17 @@ public:
 		AREA_BODY_REMOVED
 	};
 
-
 	/* MISC */
 
-	virtual void free(RID p_rid)=0;
+	virtual void free(RID p_rid) = 0;
 
-	virtual void set_active(bool p_active)=0;
-	virtual void init()=0;
-	virtual void step(float p_step)=0;
-	virtual void sync()=0;
-	virtual void flush_queries()=0;
-	virtual void end_sync()=0;
-	virtual void finish()=0;
+	virtual void set_active(bool p_active) = 0;
+	virtual void init() = 0;
+	virtual void step(float p_step) = 0;
+	virtual void sync() = 0;
+	virtual void flush_queries() = 0;
+	virtual void end_sync() = 0;
+	virtual void finish() = 0;
 
 	enum ProcessInfo {
 
@@ -569,26 +554,25 @@ public:
 		INFO_BROAD_PHASE_TIME
 	};
 
-	virtual int get_process_info(ProcessInfo p_info)=0;
+	virtual int get_process_info(ProcessInfo p_info) = 0;
 
 	Physics2DServer();
 	~Physics2DServer();
 };
 
-
 class Physics2DTestMotionResult : public Reference {
 
-	OBJ_TYPE( Physics2DTestMotionResult, Reference );
+	OBJ_TYPE(Physics2DTestMotionResult, Reference);
 
 	Physics2DServer::MotionResult result;
 	bool colliding;
-friend class Physics2DServer;
+	friend class Physics2DServer;
 
 protected:
 	static void _bind_methods();
-public:
 
-	Physics2DServer::MotionResult* get_result_ptr() const { return const_cast<Physics2DServer::MotionResult*>(&result); }
+public:
+	Physics2DServer::MotionResult *get_result_ptr() const { return const_cast<Physics2DServer::MotionResult *>(&result); }
 
 	//bool is_colliding() const;
 	Vector2 get_motion() const;
@@ -599,26 +583,25 @@ public:
 	Vector2 get_collider_velocity() const;
 	ObjectID get_collider_id() const;
 	RID get_collider_rid() const;
-	Object* get_collider() const;
+	Object *get_collider() const;
 	int get_collider_shape() const;
 
 	Physics2DTestMotionResult();
 };
 
-
-VARIANT_ENUM_CAST( Physics2DServer::ShapeType );
-VARIANT_ENUM_CAST( Physics2DServer::SpaceParameter );
-VARIANT_ENUM_CAST( Physics2DServer::AreaParameter );
-VARIANT_ENUM_CAST( Physics2DServer::AreaSpaceOverrideMode );
-VARIANT_ENUM_CAST( Physics2DServer::BodyMode );
-VARIANT_ENUM_CAST( Physics2DServer::BodyParameter );
-VARIANT_ENUM_CAST( Physics2DServer::BodyState );
-VARIANT_ENUM_CAST( Physics2DServer::CCDMode );
-VARIANT_ENUM_CAST( Physics2DServer::JointParam );
-VARIANT_ENUM_CAST( Physics2DServer::JointType );
-VARIANT_ENUM_CAST( Physics2DServer::DampedStringParam );
+VARIANT_ENUM_CAST(Physics2DServer::ShapeType);
+VARIANT_ENUM_CAST(Physics2DServer::SpaceParameter);
+VARIANT_ENUM_CAST(Physics2DServer::AreaParameter);
+VARIANT_ENUM_CAST(Physics2DServer::AreaSpaceOverrideMode);
+VARIANT_ENUM_CAST(Physics2DServer::BodyMode);
+VARIANT_ENUM_CAST(Physics2DServer::BodyParameter);
+VARIANT_ENUM_CAST(Physics2DServer::BodyState);
+VARIANT_ENUM_CAST(Physics2DServer::CCDMode);
+VARIANT_ENUM_CAST(Physics2DServer::JointParam);
+VARIANT_ENUM_CAST(Physics2DServer::JointType);
+VARIANT_ENUM_CAST(Physics2DServer::DampedStringParam);
 //VARIANT_ENUM_CAST( Physics2DServer::ObjectType );
-VARIANT_ENUM_CAST( Physics2DServer::AreaBodyStatus );
-VARIANT_ENUM_CAST( Physics2DServer::ProcessInfo );
+VARIANT_ENUM_CAST(Physics2DServer::AreaBodyStatus);
+VARIANT_ENUM_CAST(Physics2DServer::ProcessInfo);
 
 #endif
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index f93e3ad32..ba67fcd09 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -28,106 +28,98 @@
 /*************************************************************************/
 #include "physics_server.h"
 #include "print_string.h"
-PhysicsServer * PhysicsServer::singleton=NULL;
-
+PhysicsServer *PhysicsServer::singleton = NULL;
 
 void PhysicsDirectBodyState::integrate_forces() {
 
 	real_t step = get_step();
 	Vector3 lv = get_linear_velocity();
-	lv+=get_total_gravity() * step;
+	lv += get_total_gravity() * step;
 
 	Vector3 av = get_angular_velocity();
 
 	float linear_damp = 1.0 - step * get_total_linear_damp();
 
-	if (linear_damp<0) // reached zero in the given time
-		linear_damp=0;
+	if (linear_damp < 0) // reached zero in the given time
+		linear_damp = 0;
 
 	float angular_damp = 1.0 - step * get_total_angular_damp();
 
-	if (angular_damp<0) // reached zero in the given time
-		angular_damp=0;
+	if (angular_damp < 0) // reached zero in the given time
+		angular_damp = 0;
 
-	lv*=linear_damp;
-	av*=angular_damp;
+	lv *= linear_damp;
+	av *= angular_damp;
 
 	set_linear_velocity(lv);
 	set_angular_velocity(av);
-
-
-
-
 }
 
-Object* PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
+Object *PhysicsDirectBodyState::get_contact_collider_object(int p_contact_idx) const {
 
 	ObjectID objid = get_contact_collider_id(p_contact_idx);
-	Object *obj = ObjectDB::get_instance( objid );
+	Object *obj = ObjectDB::get_instance(objid);
 	return obj;
 }
 
-PhysicsServer * PhysicsServer::get_singleton() {
+PhysicsServer *PhysicsServer::get_singleton() {
 
 	return singleton;
 }
 
 void PhysicsDirectBodyState::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("get_total_gravity"),&PhysicsDirectBodyState::get_total_gravity);
-	ObjectTypeDB::bind_method(_MD("get_total_linear_damp"),&PhysicsDirectBodyState::get_total_linear_damp);
-	ObjectTypeDB::bind_method(_MD("get_total_angular_damp"),&PhysicsDirectBodyState::get_total_angular_damp);
-
-	ObjectTypeDB::bind_method(_MD("get_inverse_mass"),&PhysicsDirectBodyState::get_inverse_mass);
-	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"),&PhysicsDirectBodyState::get_inverse_inertia);
+	ObjectTypeDB::bind_method(_MD("get_total_gravity"), &PhysicsDirectBodyState::get_total_gravity);
+	ObjectTypeDB::bind_method(_MD("get_total_linear_damp"), &PhysicsDirectBodyState::get_total_linear_damp);
+	ObjectTypeDB::bind_method(_MD("get_total_angular_damp"), &PhysicsDirectBodyState::get_total_angular_damp);
 
-	ObjectTypeDB::bind_method(_MD("set_linear_velocity","velocity"),&PhysicsDirectBodyState::set_linear_velocity);
-	ObjectTypeDB::bind_method(_MD("get_linear_velocity"),&PhysicsDirectBodyState::get_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_inverse_mass"), &PhysicsDirectBodyState::get_inverse_mass);
+	ObjectTypeDB::bind_method(_MD("get_inverse_inertia"), &PhysicsDirectBodyState::get_inverse_inertia);
 
-	ObjectTypeDB::bind_method(_MD("set_angular_velocity","velocity"),&PhysicsDirectBodyState::set_angular_velocity);
-	ObjectTypeDB::bind_method(_MD("get_angular_velocity"),&PhysicsDirectBodyState::get_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("set_linear_velocity", "velocity"), &PhysicsDirectBodyState::set_linear_velocity);
+	ObjectTypeDB::bind_method(_MD("get_linear_velocity"), &PhysicsDirectBodyState::get_linear_velocity);
 
-	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsDirectBodyState::set_transform);
-	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsDirectBodyState::get_transform);
+	ObjectTypeDB::bind_method(_MD("set_angular_velocity", "velocity"), &PhysicsDirectBodyState::set_angular_velocity);
+	ObjectTypeDB::bind_method(_MD("get_angular_velocity"), &PhysicsDirectBodyState::get_angular_velocity);
 
-	ObjectTypeDB::bind_method(_MD("add_force","force","pos"),&PhysicsDirectBodyState::add_force);
-	ObjectTypeDB::bind_method(_MD("apply_impulse","pos","j"),&PhysicsDirectBodyState::apply_impulse);
+	ObjectTypeDB::bind_method(_MD("set_transform", "transform"), &PhysicsDirectBodyState::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"), &PhysicsDirectBodyState::get_transform);
 
-	ObjectTypeDB::bind_method(_MD("set_sleep_state","enabled"),&PhysicsDirectBodyState::set_sleep_state);
-	ObjectTypeDB::bind_method(_MD("is_sleeping"),&PhysicsDirectBodyState::is_sleeping);
+	ObjectTypeDB::bind_method(_MD("add_force", "force", "pos"), &PhysicsDirectBodyState::add_force);
+	ObjectTypeDB::bind_method(_MD("apply_impulse", "pos", "j"), &PhysicsDirectBodyState::apply_impulse);
 
-	ObjectTypeDB::bind_method(_MD("get_contact_count"),&PhysicsDirectBodyState::get_contact_count);
+	ObjectTypeDB::bind_method(_MD("set_sleep_state", "enabled"), &PhysicsDirectBodyState::set_sleep_state);
+	ObjectTypeDB::bind_method(_MD("is_sleeping"), &PhysicsDirectBodyState::is_sleeping);
 
-	ObjectTypeDB::bind_method(_MD("get_contact_local_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_local_pos);
-	ObjectTypeDB::bind_method(_MD("get_contact_local_normal","contact_idx"),&PhysicsDirectBodyState::get_contact_local_normal);
-	ObjectTypeDB::bind_method(_MD("get_contact_local_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_local_shape);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider","contact_idx"),&PhysicsDirectBodyState::get_contact_collider);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_pos);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_id","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_id);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_object","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_object);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_shape);
-	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos","contact_idx"),&PhysicsDirectBodyState::get_contact_collider_velocity_at_pos);
-	ObjectTypeDB::bind_method(_MD("get_step"),&PhysicsDirectBodyState::get_step);
-	ObjectTypeDB::bind_method(_MD("integrate_forces"),&PhysicsDirectBodyState::integrate_forces);
-	ObjectTypeDB::bind_method(_MD("get_space_state:PhysicsDirectSpaceState"),&PhysicsDirectBodyState::get_space_state);
+	ObjectTypeDB::bind_method(_MD("get_contact_count"), &PhysicsDirectBodyState::get_contact_count);
 
+	ObjectTypeDB::bind_method(_MD("get_contact_local_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_normal);
+	ObjectTypeDB::bind_method(_MD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_local_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_pos);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_id);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_object", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_object);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_shape", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_shape);
+	ObjectTypeDB::bind_method(_MD("get_contact_collider_velocity_at_pos", "contact_idx"), &PhysicsDirectBodyState::get_contact_collider_velocity_at_pos);
+	ObjectTypeDB::bind_method(_MD("get_step"), &PhysicsDirectBodyState::get_step);
+	ObjectTypeDB::bind_method(_MD("integrate_forces"), &PhysicsDirectBodyState::integrate_forces);
+	ObjectTypeDB::bind_method(_MD("get_space_state:PhysicsDirectSpaceState"), &PhysicsDirectBodyState::get_space_state);
 }
 
 PhysicsDirectBodyState::PhysicsDirectBodyState() {}
 
 ///////////////////////////////////////////////////////
 
-
-
 void PhysicsShapeQueryParameters::set_shape(const RES &p_shape) {
 
 	ERR_FAIL_COND(p_shape.is_null());
-	shape=p_shape->get_rid();
+	shape = p_shape->get_rid();
 }
 
-void PhysicsShapeQueryParameters::set_shape_rid(const RID& p_shape) {
+void PhysicsShapeQueryParameters::set_shape_rid(const RID &p_shape) {
 
-	shape=p_shape;
+	shape = p_shape;
 }
 
 RID PhysicsShapeQueryParameters::get_shape_rid() const {
@@ -135,95 +127,89 @@ RID PhysicsShapeQueryParameters::get_shape_rid() const {
 	return shape;
 }
 
-void PhysicsShapeQueryParameters::set_transform(const Transform& p_transform){
+void PhysicsShapeQueryParameters::set_transform(const Transform &p_transform) {
 
-	transform=p_transform;
+	transform = p_transform;
 }
-Transform PhysicsShapeQueryParameters::get_transform() const{
+Transform PhysicsShapeQueryParameters::get_transform() const {
 
 	return transform;
 }
 
-void PhysicsShapeQueryParameters::set_margin(float p_margin){
+void PhysicsShapeQueryParameters::set_margin(float p_margin) {
 
-	margin=p_margin;
+	margin = p_margin;
 }
 
-float PhysicsShapeQueryParameters::get_margin() const{
+float PhysicsShapeQueryParameters::get_margin() const {
 
 	return margin;
 }
 
-void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask){
+void PhysicsShapeQueryParameters::set_layer_mask(int p_layer_mask) {
 
-	layer_mask=p_layer_mask;
+	layer_mask = p_layer_mask;
 }
-int PhysicsShapeQueryParameters::get_layer_mask() const{
+int PhysicsShapeQueryParameters::get_layer_mask() const {
 
 	return layer_mask;
 }
 
+void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask) {
 
-void PhysicsShapeQueryParameters::set_object_type_mask(int p_object_type_mask){
-
-	object_type_mask=p_object_type_mask;
+	object_type_mask = p_object_type_mask;
 }
-int PhysicsShapeQueryParameters::get_object_type_mask() const{
+int PhysicsShapeQueryParameters::get_object_type_mask() const {
 
 	return object_type_mask;
 }
-void PhysicsShapeQueryParameters::set_exclude(const Vector<RID>& p_exclude) {
+void PhysicsShapeQueryParameters::set_exclude(const Vector<RID> &p_exclude) {
 
 	exclude.clear();
-	for(int i=0;i<p_exclude.size();i++)
+	for (int i = 0; i < p_exclude.size(); i++)
 		exclude.insert(p_exclude[i]);
-
 }
 
-Vector<RID> PhysicsShapeQueryParameters::get_exclude() const{
+Vector<RID> PhysicsShapeQueryParameters::get_exclude() const {
 
 	Vector<RID> ret;
 	ret.resize(exclude.size());
-	int idx=0;
-	for(Set<RID>::Element *E=exclude.front();E;E=E->next()) {
-		ret[idx]=E->get();
+	int idx = 0;
+	for (Set<RID>::Element *E = exclude.front(); E; E = E->next()) {
+		ret[idx] = E->get();
 	}
 	return ret;
 }
 
 void PhysicsShapeQueryParameters::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("set_shape","shape:Shape"),&PhysicsShapeQueryParameters::set_shape);
-	ObjectTypeDB::bind_method(_MD("set_shape_rid","shape"),&PhysicsShapeQueryParameters::set_shape_rid);
-	ObjectTypeDB::bind_method(_MD("get_shape_rid"),&PhysicsShapeQueryParameters::get_shape_rid);
-
-	ObjectTypeDB::bind_method(_MD("set_transform","transform"),&PhysicsShapeQueryParameters::set_transform);
-	ObjectTypeDB::bind_method(_MD("get_transform"),&PhysicsShapeQueryParameters::get_transform);
+	ObjectTypeDB::bind_method(_MD("set_shape", "shape:Shape"), &PhysicsShapeQueryParameters::set_shape);
+	ObjectTypeDB::bind_method(_MD("set_shape_rid", "shape"), &PhysicsShapeQueryParameters::set_shape_rid);
+	ObjectTypeDB::bind_method(_MD("get_shape_rid"), &PhysicsShapeQueryParameters::get_shape_rid);
 
-	ObjectTypeDB::bind_method(_MD("set_margin","margin"),&PhysicsShapeQueryParameters::set_margin);
-	ObjectTypeDB::bind_method(_MD("get_margin"),&PhysicsShapeQueryParameters::get_margin);
+	ObjectTypeDB::bind_method(_MD("set_transform", "transform"), &PhysicsShapeQueryParameters::set_transform);
+	ObjectTypeDB::bind_method(_MD("get_transform"), &PhysicsShapeQueryParameters::get_transform);
 
-	ObjectTypeDB::bind_method(_MD("set_layer_mask","layer_mask"),&PhysicsShapeQueryParameters::set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("get_layer_mask"),&PhysicsShapeQueryParameters::get_layer_mask);
+	ObjectTypeDB::bind_method(_MD("set_margin", "margin"), &PhysicsShapeQueryParameters::set_margin);
+	ObjectTypeDB::bind_method(_MD("get_margin"), &PhysicsShapeQueryParameters::get_margin);
 
-	ObjectTypeDB::bind_method(_MD("set_object_type_mask","object_type_mask"),&PhysicsShapeQueryParameters::set_object_type_mask);
-	ObjectTypeDB::bind_method(_MD("get_object_type_mask"),&PhysicsShapeQueryParameters::get_object_type_mask);
-
-	ObjectTypeDB::bind_method(_MD("set_exclude","exclude"),&PhysicsShapeQueryParameters::set_exclude);
-	ObjectTypeDB::bind_method(_MD("get_exclude"),&PhysicsShapeQueryParameters::get_exclude);
+	ObjectTypeDB::bind_method(_MD("set_layer_mask", "layer_mask"), &PhysicsShapeQueryParameters::set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("get_layer_mask"), &PhysicsShapeQueryParameters::get_layer_mask);
 
+	ObjectTypeDB::bind_method(_MD("set_object_type_mask", "object_type_mask"), &PhysicsShapeQueryParameters::set_object_type_mask);
+	ObjectTypeDB::bind_method(_MD("get_object_type_mask"), &PhysicsShapeQueryParameters::get_object_type_mask);
 
+	ObjectTypeDB::bind_method(_MD("set_exclude", "exclude"), &PhysicsShapeQueryParameters::set_exclude);
+	ObjectTypeDB::bind_method(_MD("get_exclude"), &PhysicsShapeQueryParameters::get_exclude);
 }
 
 PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() {
 
-	margin=0;
-	layer_mask=0x7FFFFFFF;
-	object_type_mask=PhysicsDirectSpaceState::TYPE_MASK_COLLISION;
+	margin = 0;
+	layer_mask = 0x7FFFFFFF;
+	object_type_mask = PhysicsDirectSpaceState::TYPE_MASK_COLLISION;
 }
 
-
-
 /////////////////////////////////////
 
 /*
@@ -256,26 +242,25 @@ Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Tran
 }
 */
 
-
-Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) {
+Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector<RID> &p_exclude, uint32_t p_layers, uint32_t p_object_type_mask) {
 
 	RayResult inters;
 	Set<RID> exclude;
-	for(int i=0;i<p_exclude.size();i++)
+	for (int i = 0; i < p_exclude.size(); i++)
 		exclude.insert(p_exclude[i]);
 
-	bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask);
+	bool res = intersect_ray(p_from, p_to, inters, exclude, p_layers, p_object_type_mask);
 
 	if (!res)
 		return Dictionary(true);
 
 	Dictionary d(true);
-	d["position"]=inters.position;
-	d["normal"]=inters.normal;
-	d["collider_id"]=inters.collider_id;
-	d["collider"]=inters.collider;
-	d["shape"]=inters.shape;
-	d["rid"]=inters.rid;
+	d["position"] = inters.position;
+	d["normal"] = inters.normal;
+	d["collider_id"] = inters.collider_id;
+	d["collider"] = inters.collider;
+	d["shape"] = inters.shape;
+	d["rid"] = inters.rid;
 
 	return d;
 }
@@ -284,101 +269,89 @@ Array PhysicsDirectSpaceState::_intersect_shape(const Ref<PhysicsShapeQueryParam
 
 	Vector<ShapeResult> sr;
 	sr.resize(p_max_results);
-	int rc = intersect_shape(psq->shape,psq->transform,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask);
+	int rc = intersect_shape(psq->shape, psq->transform, psq->margin, sr.ptr(), sr.size(), psq->exclude, psq->layer_mask, psq->object_type_mask);
 	Array ret;
 	ret.resize(rc);
-	for(int i=0;i<rc;i++) {
+	for (int i = 0; i < rc; i++) {
 
 		Dictionary d;
-		d["rid"]=sr[i].rid;
-		d["collider_id"]=sr[i].collider_id;
-		d["collider"]=sr[i].collider;
-		d["shape"]=sr[i].shape;
-		ret[i]=d;
+		d["rid"] = sr[i].rid;
+		d["collider_id"] = sr[i].collider_id;
+		d["collider"] = sr[i].collider;
+		d["shape"] = sr[i].shape;
+		ret[i] = d;
 	}
 
 	return ret;
 }
 
-Array PhysicsDirectSpaceState::_cast_motion(const Ref<PhysicsShapeQueryParameters> &psq,const Vector3& p_motion){
+Array PhysicsDirectSpaceState::_cast_motion(const Ref<PhysicsShapeQueryParameters> &psq, const Vector3 &p_motion) {
 
-	float closest_safe,closest_unsafe;
-	bool res = cast_motion(psq->shape,psq->transform,p_motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	float closest_safe, closest_unsafe;
+	bool res = cast_motion(psq->shape, psq->transform, p_motion, psq->margin, closest_safe, closest_unsafe, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	if (!res)
 		return Array();
 	Array ret(true);
 	ret.resize(2);
-	ret[0]=closest_safe;
-	ret[1]=closest_unsafe;
+	ret[0] = closest_safe;
+	ret[1] = closest_unsafe;
 	return ret;
-
 }
-Array PhysicsDirectSpaceState::_collide_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results){
+Array PhysicsDirectSpaceState::_collide_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results) {
 
 	Vector<Vector3> ret;
-	ret.resize(p_max_results*2);
-	int rc=0;
-	bool res = collide_shape(psq->shape,psq->transform,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	ret.resize(p_max_results * 2);
+	int rc = 0;
+	bool res = collide_shape(psq->shape, psq->transform, psq->margin, ret.ptr(), p_max_results, rc, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	if (!res)
 		return Array();
 	Array r;
-	r.resize(rc*2);
-	for(int i=0;i<rc*2;i++)
-		r[i]=ret[i];
+	r.resize(rc * 2);
+	for (int i = 0; i < rc * 2; i++)
+		r[i] = ret[i];
 	return r;
-
 }
-Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref<PhysicsShapeQueryParameters> &psq){
+Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref<PhysicsShapeQueryParameters> &psq) {
 
 	ShapeRestInfo sri;
 
-	bool res = rest_info(psq->shape,psq->transform,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask);
+	bool res = rest_info(psq->shape, psq->transform, psq->margin, &sri, psq->exclude, psq->layer_mask, psq->object_type_mask);
 	Dictionary r(true);
 	if (!res)
 		return r;
 
-	r["point"]=sri.point;
-	r["normal"]=sri.normal;
-	r["rid"]=sri.rid;
-	r["collider_id"]=sri.collider_id;
-	r["shape"]=sri.shape;
-	r["linear_velocity"]=sri.linear_velocity;
+	r["point"] = sri.point;
+	r["normal"] = sri.normal;
+	r["rid"] = sri.rid;
+	r["collider_id"] = sri.collider_id;
+	r["shape"] = sri.shape;
+	r["linear_velocity"] = sri.linear_velocity;
 
 	return r;
 }
 
-
-
 PhysicsDirectSpaceState::PhysicsDirectSpaceState() {
-
-
-
 }
 
-
 void PhysicsDirectSpaceState::_bind_methods() {
 
+	//	ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
+	//	ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
 
-//	ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
-//	ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
-
-	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION));
-	ObjectTypeDB::bind_method(_MD("intersect_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(32));
-	ObjectTypeDB::bind_method(_MD("cast_motion","shape:PhysicsShapeQueryParameters","motion"),&PhysicsDirectSpaceState::_cast_motion);
-	ObjectTypeDB::bind_method(_MD("collide_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_collide_shape,DEFVAL(32));
-	ObjectTypeDB::bind_method(_MD("get_rest_info","shape:PhysicsShapeQueryParameters"),&PhysicsDirectSpaceState::_get_rest_info);
-
-
-	BIND_CONSTANT( TYPE_MASK_STATIC_BODY );
-	BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY );
-	BIND_CONSTANT( TYPE_MASK_RIGID_BODY );
-	BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY );
-	BIND_CONSTANT( TYPE_MASK_AREA );
-	BIND_CONSTANT( TYPE_MASK_COLLISION );
+	ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary", "from", "to", "exclude", "layer_mask", "type_mask"), &PhysicsDirectSpaceState::_intersect_ray, DEFVAL(Array()), DEFVAL(0x7FFFFFFF), DEFVAL(TYPE_MASK_COLLISION));
+	ObjectTypeDB::bind_method(_MD("intersect_shape", "shape:PhysicsShapeQueryParameters", "max_results"), &PhysicsDirectSpaceState::_intersect_shape, DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("cast_motion", "shape:PhysicsShapeQueryParameters", "motion"), &PhysicsDirectSpaceState::_cast_motion);
+	ObjectTypeDB::bind_method(_MD("collide_shape", "shape:PhysicsShapeQueryParameters", "max_results"), &PhysicsDirectSpaceState::_collide_shape, DEFVAL(32));
+	ObjectTypeDB::bind_method(_MD("get_rest_info", "shape:PhysicsShapeQueryParameters"), &PhysicsDirectSpaceState::_get_rest_info);
 
+	BIND_CONSTANT(TYPE_MASK_STATIC_BODY);
+	BIND_CONSTANT(TYPE_MASK_KINEMATIC_BODY);
+	BIND_CONSTANT(TYPE_MASK_RIGID_BODY);
+	BIND_CONSTANT(TYPE_MASK_CHARACTER_BODY);
+	BIND_CONSTANT(TYPE_MASK_AREA);
+	BIND_CONSTANT(TYPE_MASK_COLLISION);
 }
 
-
 int PhysicsShapeQueryResult::get_result_count() const {
 
 	return result.size();
@@ -391,7 +364,7 @@ ObjectID PhysicsShapeQueryResult::get_result_object_id(int p_idx) const {
 
 	return result[p_idx].collider_id;
 }
-Object* PhysicsShapeQueryResult::get_result_object(int p_idx) const {
+Object *PhysicsShapeQueryResult::get_result_object(int p_idx) const {
 
 	return result[p_idx].collider;
 }
@@ -401,164 +374,151 @@ int PhysicsShapeQueryResult::get_result_object_shape(int p_idx) const {
 }
 
 PhysicsShapeQueryResult::PhysicsShapeQueryResult() {
-
-
 }
 
 void PhysicsShapeQueryResult::_bind_methods() {
 
-	ObjectTypeDB::bind_method(_MD("get_result_count"),&PhysicsShapeQueryResult::get_result_count);
-	ObjectTypeDB::bind_method(_MD("get_result_rid","idx"),&PhysicsShapeQueryResult::get_result_rid);
-	ObjectTypeDB::bind_method(_MD("get_result_object_id","idx"),&PhysicsShapeQueryResult::get_result_object_id);
-	ObjectTypeDB::bind_method(_MD("get_result_object","idx"),&PhysicsShapeQueryResult::get_result_object);
-	ObjectTypeDB::bind_method(_MD("get_result_object_shape","idx"),&PhysicsShapeQueryResult::get_result_object_shape);
-
-
+	ObjectTypeDB::bind_method(_MD("get_result_count"), &PhysicsShapeQueryResult::get_result_count);
+	ObjectTypeDB::bind_method(_MD("get_result_rid", "idx"), &PhysicsShapeQueryResult::get_result_rid);
+	ObjectTypeDB::bind_method(_MD("get_result_object_id", "idx"), &PhysicsShapeQueryResult::get_result_object_id);
+	ObjectTypeDB::bind_method(_MD("get_result_object", "idx"), &PhysicsShapeQueryResult::get_result_object);
+	ObjectTypeDB::bind_method(_MD("get_result_object_shape", "idx"), &PhysicsShapeQueryResult::get_result_object_shape);
 }
 
-
-
-
-
 ///////////////////////////////////////
 
 void PhysicsServer::_bind_methods() {
 
+	ObjectTypeDB::bind_method(_MD("shape_create", "type"), &PhysicsServer::shape_create);
+	ObjectTypeDB::bind_method(_MD("shape_set_data", "shape", "data"), &PhysicsServer::shape_set_data);
 
-	ObjectTypeDB::bind_method(_MD("shape_create","type"),&PhysicsServer::shape_create);
-	ObjectTypeDB::bind_method(_MD("shape_set_data","shape","data"),&PhysicsServer::shape_set_data);
-
-	ObjectTypeDB::bind_method(_MD("shape_get_type","shape"),&PhysicsServer::shape_get_type);
-	ObjectTypeDB::bind_method(_MD("shape_get_data","shape"),&PhysicsServer::shape_get_data);
-
-
-	ObjectTypeDB::bind_method(_MD("space_create"),&PhysicsServer::space_create);
-	ObjectTypeDB::bind_method(_MD("space_set_active","space","active"),&PhysicsServer::space_set_active);
-	ObjectTypeDB::bind_method(_MD("space_is_active","space"),&PhysicsServer::space_is_active);
-	ObjectTypeDB::bind_method(_MD("space_set_param","space","param","value"),&PhysicsServer::space_set_param);
-	ObjectTypeDB::bind_method(_MD("space_get_param","space","param"),&PhysicsServer::space_get_param);
-	ObjectTypeDB::bind_method(_MD("space_get_direct_state:PhysicsDirectSpaceState","space"),&PhysicsServer::space_get_direct_state);
+	ObjectTypeDB::bind_method(_MD("shape_get_type", "shape"), &PhysicsServer::shape_get_type);
+	ObjectTypeDB::bind_method(_MD("shape_get_data", "shape"), &PhysicsServer::shape_get_data);
 
-	ObjectTypeDB::bind_method(_MD("area_create"),&PhysicsServer::area_create);
-	ObjectTypeDB::bind_method(_MD("area_set_space","area","space"),&PhysicsServer::area_set_space);
-	ObjectTypeDB::bind_method(_MD("area_get_space","area"),&PhysicsServer::area_get_space);
+	ObjectTypeDB::bind_method(_MD("space_create"), &PhysicsServer::space_create);
+	ObjectTypeDB::bind_method(_MD("space_set_active", "space", "active"), &PhysicsServer::space_set_active);
+	ObjectTypeDB::bind_method(_MD("space_is_active", "space"), &PhysicsServer::space_is_active);
+	ObjectTypeDB::bind_method(_MD("space_set_param", "space", "param", "value"), &PhysicsServer::space_set_param);
+	ObjectTypeDB::bind_method(_MD("space_get_param", "space", "param"), &PhysicsServer::space_get_param);
+	ObjectTypeDB::bind_method(_MD("space_get_direct_state:PhysicsDirectSpaceState", "space"), &PhysicsServer::space_get_direct_state);
 
-	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode","area","mode"),&PhysicsServer::area_set_space_override_mode);
-	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode","area"),&PhysicsServer::area_get_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_create"), &PhysicsServer::area_create);
+	ObjectTypeDB::bind_method(_MD("area_set_space", "area", "space"), &PhysicsServer::area_set_space);
+	ObjectTypeDB::bind_method(_MD("area_get_space", "area"), &PhysicsServer::area_get_space);
 
-	ObjectTypeDB::bind_method(_MD("area_add_shape","area","shape","transform"),&PhysicsServer::area_add_shape,DEFVAL(Transform()));
-	ObjectTypeDB::bind_method(_MD("area_set_shape","area","shape_idx","shape"),&PhysicsServer::area_set_shape);
-	ObjectTypeDB::bind_method(_MD("area_set_shape_transform","area","shape_idx","transform"),&PhysicsServer::area_set_shape_transform);
+	ObjectTypeDB::bind_method(_MD("area_set_space_override_mode", "area", "mode"), &PhysicsServer::area_set_space_override_mode);
+	ObjectTypeDB::bind_method(_MD("area_get_space_override_mode", "area"), &PhysicsServer::area_get_space_override_mode);
 
-	ObjectTypeDB::bind_method(_MD("area_get_shape_count","area"),&PhysicsServer::area_get_shape_count);
-	ObjectTypeDB::bind_method(_MD("area_get_shape","area","shape_idx"),&PhysicsServer::area_get_shape);
-	ObjectTypeDB::bind_method(_MD("area_get_shape_transform","area","shape_idx"),&PhysicsServer::area_get_shape_transform);
+	ObjectTypeDB::bind_method(_MD("area_add_shape", "area", "shape", "transform"), &PhysicsServer::area_add_shape, DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("area_set_shape", "area", "shape_idx", "shape"), &PhysicsServer::area_set_shape);
+	ObjectTypeDB::bind_method(_MD("area_set_shape_transform", "area", "shape_idx", "transform"), &PhysicsServer::area_set_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_remove_shape","area","shape_idx"),&PhysicsServer::area_remove_shape);
-	ObjectTypeDB::bind_method(_MD("area_clear_shapes","area"),&PhysicsServer::area_clear_shapes);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_count", "area"), &PhysicsServer::area_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("area_get_shape", "area", "shape_idx"), &PhysicsServer::area_get_shape);
+	ObjectTypeDB::bind_method(_MD("area_get_shape_transform", "area", "shape_idx"), &PhysicsServer::area_get_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_set_layer_mask","area","mask"),&PhysicsServer::area_set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("area_set_collision_mask","area","mask"),&PhysicsServer::area_set_collision_mask);
+	ObjectTypeDB::bind_method(_MD("area_remove_shape", "area", "shape_idx"), &PhysicsServer::area_remove_shape);
+	ObjectTypeDB::bind_method(_MD("area_clear_shapes", "area"), &PhysicsServer::area_clear_shapes);
 
-	ObjectTypeDB::bind_method(_MD("area_set_param","area","param","value"),&PhysicsServer::area_set_param);
-	ObjectTypeDB::bind_method(_MD("area_set_transform","area","transform"),&PhysicsServer::area_set_transform);
+	ObjectTypeDB::bind_method(_MD("area_set_layer_mask", "area", "mask"), &PhysicsServer::area_set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("area_set_collision_mask", "area", "mask"), &PhysicsServer::area_set_collision_mask);
 
-	ObjectTypeDB::bind_method(_MD("area_get_param","area","param"),&PhysicsServer::area_get_param);
-	ObjectTypeDB::bind_method(_MD("area_get_transform","area"),&PhysicsServer::area_get_transform);
+	ObjectTypeDB::bind_method(_MD("area_set_param", "area", "param", "value"), &PhysicsServer::area_set_param);
+	ObjectTypeDB::bind_method(_MD("area_set_transform", "area", "transform"), &PhysicsServer::area_set_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID","area","id"),&PhysicsServer::area_attach_object_instance_ID);
-	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID","area"),&PhysicsServer::area_get_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_param", "area", "param"), &PhysicsServer::area_get_param);
+	ObjectTypeDB::bind_method(_MD("area_get_transform", "area"), &PhysicsServer::area_get_transform);
 
-	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","area","receiver","method"),&PhysicsServer::area_set_monitor_callback);
+	ObjectTypeDB::bind_method(_MD("area_attach_object_instance_ID", "area", "id"), &PhysicsServer::area_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("area_get_object_instance_ID", "area"), &PhysicsServer::area_get_object_instance_ID);
 
-	ObjectTypeDB::bind_method(_MD("area_set_ray_pickable","area","enable"),&PhysicsServer::area_set_ray_pickable);
-	ObjectTypeDB::bind_method(_MD("area_is_ray_pickable","area"),&PhysicsServer::area_is_ray_pickable);
+	ObjectTypeDB::bind_method(_MD("area_set_monitor_callback", "area", "receiver", "method"), &PhysicsServer::area_set_monitor_callback);
 
-	ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&PhysicsServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
+	ObjectTypeDB::bind_method(_MD("area_set_ray_pickable", "area", "enable"), &PhysicsServer::area_set_ray_pickable);
+	ObjectTypeDB::bind_method(_MD("area_is_ray_pickable", "area"), &PhysicsServer::area_is_ray_pickable);
 
-	ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&PhysicsServer::body_set_space);
-	ObjectTypeDB::bind_method(_MD("body_get_space","body"),&PhysicsServer::body_get_space);
+	ObjectTypeDB::bind_method(_MD("body_create", "mode", "init_sleeping"), &PhysicsServer::body_create, DEFVAL(BODY_MODE_RIGID), DEFVAL(false));
 
-	ObjectTypeDB::bind_method(_MD("body_set_mode","body","mode"),&PhysicsServer::body_set_mode);
-	ObjectTypeDB::bind_method(_MD("body_get_mode","body"),&PhysicsServer::body_get_mode);
+	ObjectTypeDB::bind_method(_MD("body_set_space", "body", "space"), &PhysicsServer::body_set_space);
+	ObjectTypeDB::bind_method(_MD("body_get_space", "body"), &PhysicsServer::body_get_space);
 
-	ObjectTypeDB::bind_method(_MD("body_set_layer_mask","body","mask"),&PhysicsServer::body_set_layer_mask);
-	ObjectTypeDB::bind_method(_MD("body_get_layer_mask","body"),&PhysicsServer::body_get_layer_mask);
+	ObjectTypeDB::bind_method(_MD("body_set_mode", "body", "mode"), &PhysicsServer::body_set_mode);
+	ObjectTypeDB::bind_method(_MD("body_get_mode", "body"), &PhysicsServer::body_get_mode);
 
-	ObjectTypeDB::bind_method(_MD("body_set_collision_mask","body","mask"),&PhysicsServer::body_set_collision_mask);
-	ObjectTypeDB::bind_method(_MD("body_get_collision_mask","body"),&PhysicsServer::body_get_collision_mask);
+	ObjectTypeDB::bind_method(_MD("body_set_layer_mask", "body", "mask"), &PhysicsServer::body_set_layer_mask);
+	ObjectTypeDB::bind_method(_MD("body_get_layer_mask", "body"), &PhysicsServer::body_get_layer_mask);
 
-	ObjectTypeDB::bind_method(_MD("body_add_shape","body","shape","transform"),&PhysicsServer::body_add_shape,DEFVAL(Transform()));
-	ObjectTypeDB::bind_method(_MD("body_set_shape","body","shape_idx","shape"),&PhysicsServer::body_set_shape);
-	ObjectTypeDB::bind_method(_MD("body_set_shape_transform","body","shape_idx","transform"),&PhysicsServer::body_set_shape_transform);
+	ObjectTypeDB::bind_method(_MD("body_set_collision_mask", "body", "mask"), &PhysicsServer::body_set_collision_mask);
+	ObjectTypeDB::bind_method(_MD("body_get_collision_mask", "body"), &PhysicsServer::body_get_collision_mask);
 
-	ObjectTypeDB::bind_method(_MD("body_get_shape_count","body"),&PhysicsServer::body_get_shape_count);
-	ObjectTypeDB::bind_method(_MD("body_get_shape","body","shape_idx"),&PhysicsServer::body_get_shape);
-	ObjectTypeDB::bind_method(_MD("body_get_shape_transform","body","shape_idx"),&PhysicsServer::body_get_shape_transform);
+	ObjectTypeDB::bind_method(_MD("body_add_shape", "body", "shape", "transform"), &PhysicsServer::body_add_shape, DEFVAL(Transform()));
+	ObjectTypeDB::bind_method(_MD("body_set_shape", "body", "shape_idx", "shape"), &PhysicsServer::body_set_shape);
+	ObjectTypeDB::bind_method(_MD("body_set_shape_transform", "body", "shape_idx", "transform"), &PhysicsServer::body_set_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("body_remove_shape","body","shape_idx"),&PhysicsServer::body_remove_shape);
-	ObjectTypeDB::bind_method(_MD("body_clear_shapes","body"),&PhysicsServer::body_clear_shapes);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_count", "body"), &PhysicsServer::body_get_shape_count);
+	ObjectTypeDB::bind_method(_MD("body_get_shape", "body", "shape_idx"), &PhysicsServer::body_get_shape);
+	ObjectTypeDB::bind_method(_MD("body_get_shape_transform", "body", "shape_idx"), &PhysicsServer::body_get_shape_transform);
 
-	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID","body","id"),&PhysicsServer::body_attach_object_instance_ID);
-	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID","body"),&PhysicsServer::body_get_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_remove_shape", "body", "shape_idx"), &PhysicsServer::body_remove_shape);
+	ObjectTypeDB::bind_method(_MD("body_clear_shapes", "body"), &PhysicsServer::body_clear_shapes);
 
+	ObjectTypeDB::bind_method(_MD("body_attach_object_instance_ID", "body", "id"), &PhysicsServer::body_attach_object_instance_ID);
+	ObjectTypeDB::bind_method(_MD("body_get_object_instance_ID", "body"), &PhysicsServer::body_get_object_instance_ID);
 
-	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection","body","enable"),&PhysicsServer::body_set_enable_continuous_collision_detection);
-	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled","body"),&PhysicsServer::body_is_continuous_collision_detection_enabled);
-
+	ObjectTypeDB::bind_method(_MD("body_set_enable_continuous_collision_detection", "body", "enable"), &PhysicsServer::body_set_enable_continuous_collision_detection);
+	ObjectTypeDB::bind_method(_MD("body_is_continuous_collision_detection_enabled", "body"), &PhysicsServer::body_is_continuous_collision_detection_enabled);
 
 	//ObjectTypeDB::bind_method(_MD("body_set_user_flags","flags""),&PhysicsServer::body_set_shape,DEFVAL(Transform));
 	//ObjectTypeDB::bind_method(_MD("body_get_user_flags","body","shape_idx","shape"),&PhysicsServer::body_get_shape);
 
-	ObjectTypeDB::bind_method(_MD("body_set_param","body","param","value"),&PhysicsServer::body_set_param);
-	ObjectTypeDB::bind_method(_MD("body_get_param","body","param"),&PhysicsServer::body_get_param);
+	ObjectTypeDB::bind_method(_MD("body_set_param", "body", "param", "value"), &PhysicsServer::body_set_param);
+	ObjectTypeDB::bind_method(_MD("body_get_param", "body", "param"), &PhysicsServer::body_get_param);
 
-	ObjectTypeDB::bind_method(_MD("body_set_state","body","state","value"),&PhysicsServer::body_set_state);
-	ObjectTypeDB::bind_method(_MD("body_get_state","body","state"),&PhysicsServer::body_get_state);
+	ObjectTypeDB::bind_method(_MD("body_set_state", "body", "state", "value"), &PhysicsServer::body_set_state);
+	ObjectTypeDB::bind_method(_MD("body_get_state", "body", "state"), &PhysicsServer::body_get_state);
 
-	ObjectTypeDB::bind_method(_MD("body_apply_impulse","body","pos","impulse"),&PhysicsServer::body_apply_impulse);
-	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity","body","axis_velocity"),&PhysicsServer::body_set_axis_velocity);
+	ObjectTypeDB::bind_method(_MD("body_apply_impulse", "body", "pos", "impulse"), &PhysicsServer::body_apply_impulse);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_velocity", "body", "axis_velocity"), &PhysicsServer::body_set_axis_velocity);
 
-	ObjectTypeDB::bind_method(_MD("body_set_axis_lock","body","axis"),&PhysicsServer::body_set_axis_lock);
-	ObjectTypeDB::bind_method(_MD("body_get_axis_lock","body"),&PhysicsServer::body_get_axis_lock);
+	ObjectTypeDB::bind_method(_MD("body_set_axis_lock", "body", "axis"), &PhysicsServer::body_set_axis_lock);
+	ObjectTypeDB::bind_method(_MD("body_get_axis_lock", "body"), &PhysicsServer::body_get_axis_lock);
 
-	ObjectTypeDB::bind_method(_MD("body_add_collision_exception","body","excepted_body"),&PhysicsServer::body_add_collision_exception);
-	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception","body","excepted_body"),&PhysicsServer::body_remove_collision_exception);
-//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+	ObjectTypeDB::bind_method(_MD("body_add_collision_exception", "body", "excepted_body"), &PhysicsServer::body_add_collision_exception);
+	ObjectTypeDB::bind_method(_MD("body_remove_collision_exception", "body", "excepted_body"), &PhysicsServer::body_remove_collision_exception);
+	//	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
 
-	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported","body","amount"),&PhysicsServer::body_set_max_contacts_reported);
-	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported","body"),&PhysicsServer::body_get_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_set_max_contacts_reported", "body", "amount"), &PhysicsServer::body_set_max_contacts_reported);
+	ObjectTypeDB::bind_method(_MD("body_get_max_contacts_reported", "body"), &PhysicsServer::body_get_max_contacts_reported);
 
-	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration","body","enable"),&PhysicsServer::body_set_omit_force_integration);
-	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration","body"),&PhysicsServer::body_is_omitting_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_set_omit_force_integration", "body", "enable"), &PhysicsServer::body_set_omit_force_integration);
+	ObjectTypeDB::bind_method(_MD("body_is_omitting_force_integration", "body"), &PhysicsServer::body_is_omitting_force_integration);
 
-	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&PhysicsServer::body_set_force_integration_callback,DEFVAL(Variant()));
+	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &PhysicsServer::body_set_force_integration_callback, DEFVAL(Variant()));
 
-	ObjectTypeDB::bind_method(_MD("body_set_ray_pickable","body","enable"),&PhysicsServer::body_set_ray_pickable);
-	ObjectTypeDB::bind_method(_MD("body_is_ray_pickable","body"),&PhysicsServer::body_is_ray_pickable);
+	ObjectTypeDB::bind_method(_MD("body_set_ray_pickable", "body", "enable"), &PhysicsServer::body_set_ray_pickable);
+	ObjectTypeDB::bind_method(_MD("body_is_ray_pickable", "body"), &PhysicsServer::body_is_ray_pickable);
 
 	/* JOINT API */
 
-	BIND_CONSTANT( JOINT_PIN );
-	BIND_CONSTANT( JOINT_HINGE );
-	BIND_CONSTANT( JOINT_SLIDER );
-	BIND_CONSTANT( JOINT_CONE_TWIST );
-	BIND_CONSTANT( JOINT_6DOF );
-
-	ObjectTypeDB::bind_method(_MD("joint_create_pin","body_A","local_A","body_B","local_B"),&PhysicsServer::joint_create_pin);
-	ObjectTypeDB::bind_method(_MD("pin_joint_set_param","joint","param","value"),&PhysicsServer::pin_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("pin_joint_get_param","joint","param"),&PhysicsServer::pin_joint_get_param);
+	BIND_CONSTANT(JOINT_PIN);
+	BIND_CONSTANT(JOINT_HINGE);
+	BIND_CONSTANT(JOINT_SLIDER);
+	BIND_CONSTANT(JOINT_CONE_TWIST);
+	BIND_CONSTANT(JOINT_6DOF);
 
-	ObjectTypeDB::bind_method(_MD("pin_joint_set_local_A","joint","local_A"),&PhysicsServer::pin_joint_set_local_A);
-	ObjectTypeDB::bind_method(_MD("pin_joint_get_local_A","joint"),&PhysicsServer::pin_joint_get_local_A);
+	ObjectTypeDB::bind_method(_MD("joint_create_pin", "body_A", "local_A", "body_B", "local_B"), &PhysicsServer::joint_create_pin);
+	ObjectTypeDB::bind_method(_MD("pin_joint_set_param", "joint", "param", "value"), &PhysicsServer::pin_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("pin_joint_get_param", "joint", "param"), &PhysicsServer::pin_joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("pin_joint_set_local_B","joint","local_B"),&PhysicsServer::pin_joint_set_local_B);
-	ObjectTypeDB::bind_method(_MD("pin_joint_get_local_B","joint"),&PhysicsServer::pin_joint_get_local_B);
+	ObjectTypeDB::bind_method(_MD("pin_joint_set_local_A", "joint", "local_A"), &PhysicsServer::pin_joint_set_local_A);
+	ObjectTypeDB::bind_method(_MD("pin_joint_get_local_A", "joint"), &PhysicsServer::pin_joint_get_local_A);
 
-	BIND_CONSTANT(PIN_JOINT_BIAS );
-	BIND_CONSTANT(PIN_JOINT_DAMPING );
-	BIND_CONSTANT(PIN_JOINT_IMPULSE_CLAMP );
+	ObjectTypeDB::bind_method(_MD("pin_joint_set_local_B", "joint", "local_B"), &PhysicsServer::pin_joint_set_local_B);
+	ObjectTypeDB::bind_method(_MD("pin_joint_get_local_B", "joint"), &PhysicsServer::pin_joint_get_local_B);
 
+	BIND_CONSTANT(PIN_JOINT_BIAS);
+	BIND_CONSTANT(PIN_JOINT_DAMPING);
+	BIND_CONSTANT(PIN_JOINT_IMPULSE_CLAMP);
 
 	BIND_CONSTANT(HINGE_JOINT_BIAS);
 	BIND_CONSTANT(HINGE_JOINT_LIMIT_UPPER);
@@ -571,92 +531,88 @@ void PhysicsServer::_bind_methods() {
 	BIND_CONSTANT(HINGE_JOINT_FLAG_USE_LIMIT);
 	BIND_CONSTANT(HINGE_JOINT_FLAG_ENABLE_MOTOR);
 
-	ObjectTypeDB::bind_method(_MD("joint_create_hinge","body_A","hinge_A","body_B","hinge_B"),&PhysicsServer::joint_create_hinge);
+	ObjectTypeDB::bind_method(_MD("joint_create_hinge", "body_A", "hinge_A", "body_B", "hinge_B"), &PhysicsServer::joint_create_hinge);
 
-	ObjectTypeDB::bind_method(_MD("hinge_joint_set_param","joint","param","value"),&PhysicsServer::hinge_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("hinge_joint_get_param","joint","param"),&PhysicsServer::hinge_joint_get_param);
+	ObjectTypeDB::bind_method(_MD("hinge_joint_set_param", "joint", "param", "value"), &PhysicsServer::hinge_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("hinge_joint_get_param", "joint", "param"), &PhysicsServer::hinge_joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("hinge_joint_set_flag","joint","flag","enabled"),&PhysicsServer::hinge_joint_set_flag);
-	ObjectTypeDB::bind_method(_MD("hinge_joint_get_flag","joint","flag"),&PhysicsServer::hinge_joint_get_flag);
+	ObjectTypeDB::bind_method(_MD("hinge_joint_set_flag", "joint", "flag", "enabled"), &PhysicsServer::hinge_joint_set_flag);
+	ObjectTypeDB::bind_method(_MD("hinge_joint_get_flag", "joint", "flag"), &PhysicsServer::hinge_joint_get_flag);
 
-	ObjectTypeDB::bind_method(_MD("joint_create_slider","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_slider);
+	ObjectTypeDB::bind_method(_MD("joint_create_slider", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_slider);
 
-	ObjectTypeDB::bind_method(_MD("slider_joint_set_param","joint","param","value"),&PhysicsServer::slider_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("slider_joint_get_param","joint","param"),&PhysicsServer::slider_joint_get_param);
+	ObjectTypeDB::bind_method(_MD("slider_joint_set_param", "joint", "param", "value"), &PhysicsServer::slider_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("slider_joint_get_param", "joint", "param"), &PhysicsServer::slider_joint_get_param);
 
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_UPPER );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_LOWER );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_DAMPING );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_DAMPING );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING );
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_UPPER);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_LOWER);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_LIMIT_DAMPING);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_MOTION_DAMPING);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING);
 
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_UPPER );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_LOWER );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_DAMPING );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_DAMPING );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION );
-	BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING );
-	BIND_CONSTANT( SLIDER_JOINT_MAX );
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_UPPER);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_LOWER);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_LIMIT_DAMPING);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_MOTION_DAMPING);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION);
+	BIND_CONSTANT(SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING);
+	BIND_CONSTANT(SLIDER_JOINT_MAX);
 
+	ObjectTypeDB::bind_method(_MD("joint_create_cone_twist", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_cone_twist);
 
-	ObjectTypeDB::bind_method(_MD("joint_create_cone_twist","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_cone_twist);
+	ObjectTypeDB::bind_method(_MD("cone_twist_joint_set_param", "joint", "param", "value"), &PhysicsServer::cone_twist_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("cone_twist_joint_get_param", "joint", "param"), &PhysicsServer::cone_twist_joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("cone_twist_joint_set_param","joint","param","value"),&PhysicsServer::cone_twist_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("cone_twist_joint_get_param","joint","param"),&PhysicsServer::cone_twist_joint_get_param);
+	BIND_CONSTANT(CONE_TWIST_JOINT_SWING_SPAN);
+	BIND_CONSTANT(CONE_TWIST_JOINT_TWIST_SPAN);
+	BIND_CONSTANT(CONE_TWIST_JOINT_BIAS);
+	BIND_CONSTANT(CONE_TWIST_JOINT_SOFTNESS);
+	BIND_CONSTANT(CONE_TWIST_JOINT_RELAXATION);
 
-	BIND_CONSTANT( CONE_TWIST_JOINT_SWING_SPAN );
-	BIND_CONSTANT( CONE_TWIST_JOINT_TWIST_SPAN );
-	BIND_CONSTANT( CONE_TWIST_JOINT_BIAS );
-	BIND_CONSTANT( CONE_TWIST_JOINT_SOFTNESS );
-	BIND_CONSTANT( CONE_TWIST_JOINT_RELAXATION );
+	BIND_CONSTANT(G6DOF_JOINT_LINEAR_LOWER_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_LINEAR_UPPER_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS);
+	BIND_CONSTANT(G6DOF_JOINT_LINEAR_RESTITUTION);
+	BIND_CONSTANT(G6DOF_JOINT_LINEAR_DAMPING);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_LOWER_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_UPPER_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_DAMPING);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_RESTITUTION);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_FORCE_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_ERP);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY);
+	BIND_CONSTANT(G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT);
 
+	BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT);
+	BIND_CONSTANT(G6DOF_JOINT_FLAG_ENABLE_MOTOR);
 
-	BIND_CONSTANT( G6DOF_JOINT_LINEAR_LOWER_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_LINEAR_UPPER_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS );
-	BIND_CONSTANT( G6DOF_JOINT_LINEAR_RESTITUTION );
-	BIND_CONSTANT( G6DOF_JOINT_LINEAR_DAMPING );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LOWER_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_UPPER_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_DAMPING );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_RESTITUTION );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_FORCE_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_ERP );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY );
-	BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT );
+	ObjectTypeDB::bind_method(_MD("joint_get_type", "joint"), &PhysicsServer::joint_get_type);
 
+	ObjectTypeDB::bind_method(_MD("joint_set_solver_priority", "joint", "priority"), &PhysicsServer::joint_set_solver_priority);
+	ObjectTypeDB::bind_method(_MD("joint_get_solver_priority", "joint"), &PhysicsServer::joint_get_solver_priority);
 
-	BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT );
-	BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_MOTOR );
+	ObjectTypeDB::bind_method(_MD("joint_create_generic_6dof", "body_A", "local_ref_A", "body_B", "local_ref_B"), &PhysicsServer::joint_create_generic_6dof);
 
-	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&PhysicsServer::joint_get_type);
-
-	ObjectTypeDB::bind_method(_MD("joint_set_solver_priority","joint","priority"),&PhysicsServer::joint_set_solver_priority);
-	ObjectTypeDB::bind_method(_MD("joint_get_solver_priority","joint"),&PhysicsServer::joint_get_solver_priority);
-
-	ObjectTypeDB::bind_method(_MD("joint_create_generic_6dof","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_generic_6dof);
-
-	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_param","joint","axis","param","value"),&PhysicsServer::generic_6dof_joint_set_param);
-	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_param","joint","axis","param"),&PhysicsServer::generic_6dof_joint_get_param);
+	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_param", "joint", "axis", "param", "value"), &PhysicsServer::generic_6dof_joint_set_param);
+	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_param", "joint", "axis", "param"), &PhysicsServer::generic_6dof_joint_get_param);
 
-	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_flag","joint","axis","flag","enable"),&PhysicsServer::generic_6dof_joint_set_flag);
-	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_flag","joint","axis","flag"),&PhysicsServer::generic_6dof_joint_get_flag);
+	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_flag", "joint", "axis", "flag", "enable"), &PhysicsServer::generic_6dof_joint_set_flag);
+	ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_flag", "joint", "axis", "flag"), &PhysicsServer::generic_6dof_joint_get_flag);
 
-
-/*
+	/*
 	ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&PhysicsServer::joint_set_param);
 	ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&PhysicsServer::joint_get_param);
 
@@ -669,63 +625,61 @@ void PhysicsServer::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&PhysicsServer::joint_get_type);
 */
-	ObjectTypeDB::bind_method(_MD("free_rid","rid"),&PhysicsServer::free);
+	ObjectTypeDB::bind_method(_MD("free_rid", "rid"), &PhysicsServer::free);
 
-	ObjectTypeDB::bind_method(_MD("set_active","active"),&PhysicsServer::set_active);
+	ObjectTypeDB::bind_method(_MD("set_active", "active"), &PhysicsServer::set_active);
 
-//	ObjectTypeDB::bind_method(_MD("init"),&PhysicsServer::init);
-//	ObjectTypeDB::bind_method(_MD("step"),&PhysicsServer::step);
-//	ObjectTypeDB::bind_method(_MD("sync"),&PhysicsServer::sync);
+	//	ObjectTypeDB::bind_method(_MD("init"),&PhysicsServer::init);
+	//	ObjectTypeDB::bind_method(_MD("step"),&PhysicsServer::step);
+	//	ObjectTypeDB::bind_method(_MD("sync"),&PhysicsServer::sync);
 	//ObjectTypeDB::bind_method(_MD("flush_queries"),&PhysicsServer::flush_queries);
 
+	ObjectTypeDB::bind_method(_MD("get_process_info", "process_info"), &PhysicsServer::get_process_info);
 
-	ObjectTypeDB::bind_method(_MD("get_process_info","process_info"),&PhysicsServer::get_process_info);
-
-	BIND_CONSTANT( SHAPE_PLANE );
-	BIND_CONSTANT( SHAPE_RAY );
-	BIND_CONSTANT( SHAPE_SPHERE );
-	BIND_CONSTANT( SHAPE_BOX );
-	BIND_CONSTANT( SHAPE_CAPSULE );
-	BIND_CONSTANT( SHAPE_CONVEX_POLYGON );
-	BIND_CONSTANT( SHAPE_CONCAVE_POLYGON );
-	BIND_CONSTANT( SHAPE_HEIGHTMAP );
-	BIND_CONSTANT( SHAPE_CUSTOM );
+	BIND_CONSTANT(SHAPE_PLANE);
+	BIND_CONSTANT(SHAPE_RAY);
+	BIND_CONSTANT(SHAPE_SPHERE);
+	BIND_CONSTANT(SHAPE_BOX);
+	BIND_CONSTANT(SHAPE_CAPSULE);
+	BIND_CONSTANT(SHAPE_CONVEX_POLYGON);
+	BIND_CONSTANT(SHAPE_CONCAVE_POLYGON);
+	BIND_CONSTANT(SHAPE_HEIGHTMAP);
+	BIND_CONSTANT(SHAPE_CUSTOM);
 
+	BIND_CONSTANT(AREA_PARAM_GRAVITY);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_VECTOR);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_IS_POINT);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_DISTANCE_SCALE);
+	BIND_CONSTANT(AREA_PARAM_GRAVITY_POINT_ATTENUATION);
+	BIND_CONSTANT(AREA_PARAM_LINEAR_DAMP);
+	BIND_CONSTANT(AREA_PARAM_ANGULAR_DAMP);
+	BIND_CONSTANT(AREA_PARAM_PRIORITY);
 
-	BIND_CONSTANT( AREA_PARAM_GRAVITY );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_VECTOR );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_IS_POINT );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_DISTANCE_SCALE );
-	BIND_CONSTANT( AREA_PARAM_GRAVITY_POINT_ATTENUATION );
-	BIND_CONSTANT( AREA_PARAM_LINEAR_DAMP );
-	BIND_CONSTANT( AREA_PARAM_ANGULAR_DAMP );
-	BIND_CONSTANT( AREA_PARAM_PRIORITY );
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_DISABLED);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_COMBINE_REPLACE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE);
+	BIND_CONSTANT(AREA_SPACE_OVERRIDE_REPLACE_COMBINE);
 
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_DISABLED );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_COMBINE_REPLACE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE );
-	BIND_CONSTANT( AREA_SPACE_OVERRIDE_REPLACE_COMBINE );
+	BIND_CONSTANT(BODY_MODE_STATIC);
+	BIND_CONSTANT(BODY_MODE_KINEMATIC);
+	BIND_CONSTANT(BODY_MODE_RIGID);
+	BIND_CONSTANT(BODY_MODE_CHARACTER);
 
-	BIND_CONSTANT( BODY_MODE_STATIC );
-	BIND_CONSTANT( BODY_MODE_KINEMATIC );
-	BIND_CONSTANT( BODY_MODE_RIGID );
-	BIND_CONSTANT( BODY_MODE_CHARACTER );
+	BIND_CONSTANT(BODY_PARAM_BOUNCE);
+	BIND_CONSTANT(BODY_PARAM_FRICTION);
+	BIND_CONSTANT(BODY_PARAM_MASS);
+	BIND_CONSTANT(BODY_PARAM_GRAVITY_SCALE);
+	BIND_CONSTANT(BODY_PARAM_ANGULAR_DAMP);
+	BIND_CONSTANT(BODY_PARAM_LINEAR_DAMP);
+	BIND_CONSTANT(BODY_PARAM_MAX);
 
-	BIND_CONSTANT( BODY_PARAM_BOUNCE );
-	BIND_CONSTANT( BODY_PARAM_FRICTION );
-	BIND_CONSTANT( BODY_PARAM_MASS );
-	BIND_CONSTANT( BODY_PARAM_GRAVITY_SCALE );
-	BIND_CONSTANT( BODY_PARAM_ANGULAR_DAMP );
-	BIND_CONSTANT( BODY_PARAM_LINEAR_DAMP );
-	BIND_CONSTANT( BODY_PARAM_MAX );
-
-	BIND_CONSTANT( BODY_STATE_TRANSFORM );
-	BIND_CONSTANT( BODY_STATE_LINEAR_VELOCITY );
-	BIND_CONSTANT( BODY_STATE_ANGULAR_VELOCITY );
-	BIND_CONSTANT( BODY_STATE_SLEEPING );
-	BIND_CONSTANT( BODY_STATE_CAN_SLEEP );
-/*
+	BIND_CONSTANT(BODY_STATE_TRANSFORM);
+	BIND_CONSTANT(BODY_STATE_LINEAR_VELOCITY);
+	BIND_CONSTANT(BODY_STATE_ANGULAR_VELOCITY);
+	BIND_CONSTANT(BODY_STATE_SLEEPING);
+	BIND_CONSTANT(BODY_STATE_CAN_SLEEP);
+	/*
 	BIND_CONSTANT( JOINT_PIN );
 	BIND_CONSTANT( JOINT_GROOVE );
 	BIND_CONSTANT( JOINT_DAMPED_SPRING );
@@ -734,29 +688,24 @@ void PhysicsServer::_bind_methods() {
 	BIND_CONSTANT( DAMPED_STRING_STIFFNESS );
 	BIND_CONSTANT( DAMPED_STRING_DAMPING );
 */
-//	BIND_CONSTANT( TYPE_BODY );
-//	BIND_CONSTANT( TYPE_AREA );
-
-	BIND_CONSTANT( AREA_BODY_ADDED );
-	BIND_CONSTANT( AREA_BODY_REMOVED );
-
-	BIND_CONSTANT( INFO_ACTIVE_OBJECTS );
-	BIND_CONSTANT( INFO_COLLISION_PAIRS );
-	BIND_CONSTANT( INFO_ISLAND_COUNT );
-
+	//	BIND_CONSTANT( TYPE_BODY );
+	//	BIND_CONSTANT( TYPE_AREA );
 
+	BIND_CONSTANT(AREA_BODY_ADDED);
+	BIND_CONSTANT(AREA_BODY_REMOVED);
 
+	BIND_CONSTANT(INFO_ACTIVE_OBJECTS);
+	BIND_CONSTANT(INFO_COLLISION_PAIRS);
+	BIND_CONSTANT(INFO_ISLAND_COUNT);
 }
 
-
 PhysicsServer::PhysicsServer() {
 
-	ERR_FAIL_COND( singleton!=NULL );
-	singleton=this;
+	ERR_FAIL_COND(singleton != NULL);
+	singleton = this;
 }
 
 PhysicsServer::~PhysicsServer() {
 
-	singleton=NULL;
+	singleton = NULL;
 }
-
diff --git a/servers/physics_server.h b/servers/physics_server.h
index a839b734b..8b77cfc3f 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -36,78 +36,78 @@ class PhysicsDirectSpaceState;
 
 class PhysicsDirectBodyState : public Object {
 
-	OBJ_TYPE( PhysicsDirectBodyState, Object );
+	OBJ_TYPE(PhysicsDirectBodyState, Object);
+
 protected:
 	static void _bind_methods();
-public:
 
-	virtual Vector3 get_total_gravity() const=0;
-	virtual float get_total_angular_damp() const=0;
-	virtual float get_total_linear_damp() const=0;
+public:
+	virtual Vector3 get_total_gravity() const = 0;
+	virtual float get_total_angular_damp() const = 0;
+	virtual float get_total_linear_damp() const = 0;
 
-	virtual float get_inverse_mass() const=0; // get the mass
-	virtual Vector3 get_inverse_inertia() const=0; // get density of this body space
-	virtual Matrix3 get_inverse_inertia_tensor() const=0; // get density of this body space
+	virtual float get_inverse_mass() const = 0; // get the mass
+	virtual Vector3 get_inverse_inertia() const = 0; // get density of this body space
+	virtual Matrix3 get_inverse_inertia_tensor() const = 0; // get density of this body space
 
-	virtual void set_linear_velocity(const Vector3& p_velocity)=0;
-	virtual Vector3 get_linear_velocity() const=0;
+	virtual void set_linear_velocity(const Vector3 &p_velocity) = 0;
+	virtual Vector3 get_linear_velocity() const = 0;
 
-	virtual void set_angular_velocity(const Vector3& p_velocity)=0;
-	virtual Vector3 get_angular_velocity() const=0;
+	virtual void set_angular_velocity(const Vector3 &p_velocity) = 0;
+	virtual Vector3 get_angular_velocity() const = 0;
 
-	virtual void set_transform(const Transform& p_transform)=0;
-	virtual Transform get_transform() const=0;
+	virtual void set_transform(const Transform &p_transform) = 0;
+	virtual Transform get_transform() const = 0;
 
-	virtual void add_force(const Vector3& p_force, const Vector3& p_pos)=0;
-	virtual void apply_impulse(const Vector3& p_pos, const Vector3& p_j)=0;
+	virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) = 0;
+	virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) = 0;
 
-	virtual void set_sleep_state(bool p_enable)=0;
-	virtual bool is_sleeping() const=0;
+	virtual void set_sleep_state(bool p_enable) = 0;
+	virtual bool is_sleeping() const = 0;
 
-	virtual int get_contact_count() const=0;
+	virtual int get_contact_count() const = 0;
 
-	virtual Vector3 get_contact_local_pos(int p_contact_idx) const=0;
-	virtual Vector3 get_contact_local_normal(int p_contact_idx) const=0;
-	virtual int get_contact_local_shape(int p_contact_idx) const=0;
+	virtual Vector3 get_contact_local_pos(int p_contact_idx) const = 0;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0;
+	virtual int get_contact_local_shape(int p_contact_idx) const = 0;
 
-	virtual RID get_contact_collider(int p_contact_idx) const=0;
-	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const=0;
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const=0;
-	virtual Object* get_contact_collider_object(int p_contact_idx) const;
-	virtual int get_contact_collider_shape(int p_contact_idx) const=0;
-	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const=0;
+	virtual RID get_contact_collider(int p_contact_idx) const = 0;
+	virtual Vector3 get_contact_collider_pos(int p_contact_idx) const = 0;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const = 0;
+	virtual Object *get_contact_collider_object(int p_contact_idx) const;
+	virtual int get_contact_collider_shape(int p_contact_idx) const = 0;
+	virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const = 0;
 
-	virtual real_t get_step() const=0;
+	virtual real_t get_step() const = 0;
 	virtual void integrate_forces();
 
-	virtual PhysicsDirectSpaceState* get_space_state()=0;
+	virtual PhysicsDirectSpaceState *get_space_state() = 0;
 
 	PhysicsDirectBodyState();
 };
 
-
 class PhysicsShapeQueryResult;
 
 class PhysicsShapeQueryParameters : public Reference {
 
 	OBJ_TYPE(PhysicsShapeQueryParameters, Reference);
-friend class PhysicsDirectSpaceState;
+	friend class PhysicsDirectSpaceState;
 	RID shape;
 	Transform transform;
 	float margin;
 	Set<RID> exclude;
 	uint32_t layer_mask;
 	uint32_t object_type_mask;
+
 protected:
 	static void _bind_methods();
-public:
 
-
-	void set_shape(const RES& p_shape);
-	void set_shape_rid(const RID& p_shape);
+public:
+	void set_shape(const RES &p_shape);
+	void set_shape_rid(const RID &p_shape);
 	RID get_shape_rid() const;
 
-	void set_transform(const Transform& p_transform);
+	void set_transform(const Transform &p_transform);
 	Transform get_transform() const;
 
 	void set_margin(float p_margin);
@@ -119,47 +119,39 @@ public:
 	void set_object_type_mask(int p_object_type_mask);
 	int get_object_type_mask() const;
 
-	void set_exclude(const Vector<RID>& p_exclude);
+	void set_exclude(const Vector<RID> &p_exclude);
 	Vector<RID> get_exclude() const;
 
 	PhysicsShapeQueryParameters();
-
 };
 
-
-
 class PhysicsDirectSpaceState : public Object {
 
-	OBJ_TYPE( PhysicsDirectSpaceState, Object );
+	OBJ_TYPE(PhysicsDirectSpaceState, Object);
 
-//	Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0);
-//	Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0);
+	//	Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0);
+	//	Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_collision_mask=0);
 public:
-
 	enum ObjectTypeMask {
-		TYPE_MASK_STATIC_BODY=1<<0,
-		TYPE_MASK_KINEMATIC_BODY=1<<1,
-		TYPE_MASK_RIGID_BODY=1<<2,
-		TYPE_MASK_CHARACTER_BODY=1<<3,
-		TYPE_MASK_AREA=1<<4,
-		TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
+		TYPE_MASK_STATIC_BODY = 1 << 0,
+		TYPE_MASK_KINEMATIC_BODY = 1 << 1,
+		TYPE_MASK_RIGID_BODY = 1 << 2,
+		TYPE_MASK_CHARACTER_BODY = 1 << 3,
+		TYPE_MASK_AREA = 1 << 4,
+		TYPE_MASK_COLLISION = TYPE_MASK_STATIC_BODY | TYPE_MASK_CHARACTER_BODY | TYPE_MASK_KINEMATIC_BODY | TYPE_MASK_RIGID_BODY
 	};
 
-
 private:
-	Dictionary _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
-	Array _intersect_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32);
-	Array _cast_motion(const Ref<PhysicsShapeQueryParameters> &p_shape_query,const Vector3& p_motion);
-	Array _collide_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32);
+	Dictionary _intersect_ray(const Vector3 &p_from, const Vector3 &p_to, const Vector<RID> &p_exclude = Vector<RID>(), uint32_t p_layers = 0, uint32_t p_object_type_mask = TYPE_MASK_COLLISION);
+	Array _intersect_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query, int p_max_results = 32);
+	Array _cast_motion(const Ref<PhysicsShapeQueryParameters> &p_shape_query, const Vector3 &p_motion);
+	Array _collide_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query, int p_max_results = 32);
 	Dictionary _get_rest_info(const Ref<PhysicsShapeQueryParameters> &p_shape_query);
 
-
 protected:
 	static void _bind_methods();
 
 public:
-
-
 	struct RayResult {
 
 		Vector3 position;
@@ -170,7 +162,7 @@ public:
 		int shape;
 	};
 
-	virtual bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,bool p_pick_ray=false)=0;
+	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, bool p_pick_ray = false) = 0;
 
 	struct ShapeResult {
 
@@ -178,10 +170,9 @@ public:
 		ObjectID collider_id;
 		Object *collider;
 		int shape;
-
 	};
 
-	virtual int intersect_shape(const RID& p_shape, const Transform& p_xform,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual int intersect_shape(const RID &p_shape, const Transform &p_xform, float p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	struct ShapeRestInfo {
 
@@ -191,55 +182,49 @@ public:
 		ObjectID collider_id;
 		int shape;
 		Vector3 linear_velocity; //velocity at contact point
-
 	};
 
-	virtual bool cast_motion(const RID& p_shape, const Transform& p_xform,const Vector3& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION,ShapeRestInfo *r_info=NULL)=0;
-
-	virtual bool collide_shape(RID p_shape, const Transform& p_shape_xform,float p_margin,Vector3 *r_results,int p_result_max,int &r_result_count, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual bool cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, float p_margin, float &p_closest_safe, float &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION, ShapeRestInfo *r_info = NULL) = 0;
 
-	virtual bool rest_info(RID p_shape, const Transform& p_shape_xform,float p_margin,ShapeRestInfo *r_info, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION)=0;
+	virtual bool collide_shape(RID p_shape, const Transform &p_shape_xform, float p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
+	virtual bool rest_info(RID p_shape, const Transform &p_shape_xform, float p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_layer_mask = 0xFFFFFFFF, uint32_t p_object_type_mask = TYPE_MASK_COLLISION) = 0;
 
 	PhysicsDirectSpaceState();
 };
 
-
 class PhysicsShapeQueryResult : public Reference {
 
-	OBJ_TYPE( PhysicsShapeQueryResult, Reference );
+	OBJ_TYPE(PhysicsShapeQueryResult, Reference);
 
 	Vector<PhysicsDirectSpaceState::ShapeResult> result;
 
-friend class PhysicsDirectSpaceState;
+	friend class PhysicsDirectSpaceState;
 
 protected:
 	static void _bind_methods();
-public:
 
+public:
 	int get_result_count() const;
 	RID get_result_rid(int p_idx) const;
 	ObjectID get_result_object_id(int p_idx) const;
-	Object* get_result_object(int p_idx) const;
+	Object *get_result_object(int p_idx) const;
 	int get_result_object_shape(int p_idx) const;
 
 	PhysicsShapeQueryResult();
 };
 
-
 class PhysicsServer : public Object {
 
-	OBJ_TYPE( PhysicsServer, Object );
-
-	static PhysicsServer * singleton;
+	OBJ_TYPE(PhysicsServer, Object);
 
+	static PhysicsServer *singleton;
 
 protected:
 	static void _bind_methods();
 
 public:
-
-	static PhysicsServer * get_singleton();
+	static PhysicsServer *get_singleton();
 
 	enum ShapeType {
 		SHAPE_PLANE, ///< plane:"plane"
@@ -253,20 +238,19 @@ public:
 		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
 	};
 
-	virtual RID shape_create(ShapeType p_shape)=0;
-	virtual void shape_set_data(RID p_shape, const Variant& p_data)=0;
-	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias)=0;
-
-	virtual ShapeType shape_get_type(RID p_shape) const=0;
-	virtual Variant shape_get_data(RID p_shape) const=0;
-	virtual real_t shape_get_custom_solver_bias(RID p_shape) const=0;
+	virtual RID shape_create(ShapeType p_shape) = 0;
+	virtual void shape_set_data(RID p_shape, const Variant &p_data) = 0;
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) = 0;
 
+	virtual ShapeType shape_get_type(RID p_shape) const = 0;
+	virtual Variant shape_get_data(RID p_shape) const = 0;
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const = 0;
 
 	/* SPACE API */
 
-	virtual RID space_create()=0;
-	virtual void space_set_active(RID p_space,bool p_active)=0;
-	virtual bool space_is_active(RID p_space) const=0;
+	virtual RID space_create() = 0;
+	virtual void space_set_active(RID p_space, bool p_active) = 0;
+	virtual bool space_is_active(RID p_space) const = 0;
 
 	enum SpaceParameter {
 
@@ -280,15 +264,15 @@ public:
 		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
 	};
 
-	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value)=0;
-	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const=0;
+	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;
+	virtual real_t space_get_param(RID p_space, SpaceParameter p_param) const = 0;
 
 	// this function only works on fixed process, errors and returns null otherwise
-	virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space)=0;
+	virtual PhysicsDirectSpaceState *space_get_direct_state(RID p_space) = 0;
 
-	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts)=0;
-	virtual Vector<Vector3> space_get_contacts(RID p_space) const=0;
-	virtual int space_get_contact_count(RID p_space) const=0;
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) = 0;
+	virtual Vector<Vector3> space_get_contacts(RID p_space) const = 0;
+	virtual int space_get_contact_count(RID p_space) const = 0;
 
 	//missing space parameters
 
@@ -296,8 +280,6 @@ public:
 
 	//missing attenuation? missing better override?
 
-
-
 	enum AreaParameter {
 		AREA_PARAM_GRAVITY,
 		AREA_PARAM_GRAVITY_VECTOR,
@@ -309,11 +291,10 @@ public:
 		AREA_PARAM_PRIORITY
 	};
 
-	virtual RID area_create()=0;
-
-	virtual void area_set_space(RID p_area, RID p_space)=0;
-	virtual RID area_get_space(RID p_area) const=0;
+	virtual RID area_create() = 0;
 
+	virtual void area_set_space(RID p_area, RID p_space) = 0;
+	virtual RID area_get_space(RID p_area) const = 0;
 
 	enum AreaSpaceOverrideMode {
 		AREA_SPACE_OVERRIDE_DISABLED,
@@ -323,39 +304,39 @@ public:
 		AREA_SPACE_OVERRIDE_REPLACE_COMBINE
 	};
 
-	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode)=0;
-	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const=0;
+	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) = 0;
+	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const = 0;
 
-	virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform())=0;
-	virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape)=0;
-	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform)=0;
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform &p_transform = Transform()) = 0;
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) = 0;
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) = 0;
 
-	virtual int area_get_shape_count(RID p_area) const=0;
-	virtual RID area_get_shape(RID p_area, int p_shape_idx) const=0;
-	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const=0;
+	virtual int area_get_shape_count(RID p_area) const = 0;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const = 0;
+	virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const = 0;
 
-	virtual void area_remove_shape(RID p_area, int p_shape_idx)=0;
-	virtual void area_clear_shapes(RID p_area)=0;
+	virtual void area_remove_shape(RID p_area, int p_shape_idx) = 0;
+	virtual void area_clear_shapes(RID p_area) = 0;
 
-	virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID)=0;
-	virtual ObjectID area_get_object_instance_ID(RID p_area) const=0;
+	virtual void area_attach_object_instance_ID(RID p_area, ObjectID p_ID) = 0;
+	virtual ObjectID area_get_object_instance_ID(RID p_area) const = 0;
 
-	virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value)=0;
-	virtual void area_set_transform(RID p_area, const Transform& p_transform)=0;
+	virtual void area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) = 0;
+	virtual void area_set_transform(RID p_area, const Transform &p_transform) = 0;
 
-	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const=0;
-	virtual Transform area_get_transform(RID p_area) const=0;
+	virtual Variant area_get_param(RID p_parea, AreaParameter p_param) const = 0;
+	virtual Transform area_get_transform(RID p_area) const = 0;
 
-	virtual void area_set_collision_mask(RID p_area,uint32_t p_mask)=0;
-	virtual void area_set_layer_mask(RID p_area,uint32_t p_mask)=0;
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) = 0;
+	virtual void area_set_layer_mask(RID p_area, uint32_t p_mask) = 0;
 
-	virtual void area_set_monitorable(RID p_area,bool p_monitorable)=0;
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable) = 0;
 
-	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
-	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
+	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0;
+	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) = 0;
 
-	virtual void area_set_ray_pickable(RID p_area,bool p_enable)=0;
-	virtual bool area_is_ray_pickable(RID p_area) const=0;
+	virtual void area_set_ray_pickable(RID p_area, bool p_enable) = 0;
+	virtual bool area_is_ray_pickable(RID p_area) const = 0;
 
 	/* BODY API */
 
@@ -369,42 +350,42 @@ public:
 		BODY_MODE_CHARACTER
 	};
 
-	virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false)=0;
+	virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) = 0;
 
-	virtual void body_set_space(RID p_body, RID p_space)=0;
-	virtual RID body_get_space(RID p_body) const=0;
+	virtual void body_set_space(RID p_body, RID p_space) = 0;
+	virtual RID body_get_space(RID p_body) const = 0;
 
-	virtual void body_set_mode(RID p_body, BodyMode p_mode)=0;
-	virtual BodyMode body_get_mode(RID p_body) const=0;
+	virtual void body_set_mode(RID p_body, BodyMode p_mode) = 0;
+	virtual BodyMode body_get_mode(RID p_body) const = 0;
 
-	virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform())=0;
-	virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape)=0;
-	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform)=0;
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform &p_transform = Transform()) = 0;
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) = 0;
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform &p_transform) = 0;
 
-	virtual int body_get_shape_count(RID p_body) const=0;
-	virtual RID body_get_shape(RID p_body, int p_shape_idx) const=0;
-	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const=0;
+	virtual int body_get_shape_count(RID p_body) const = 0;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const = 0;
+	virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable)=0;
-	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const=0;
+	virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx, bool p_enable) = 0;
+	virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const = 0;
 
-	virtual void body_remove_shape(RID p_body, int p_shape_idx)=0;
-	virtual void body_clear_shapes(RID p_body)=0;
+	virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0;
+	virtual void body_clear_shapes(RID p_body) = 0;
 
-	virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID)=0;
-	virtual uint32_t body_get_object_instance_ID(RID p_body) const=0;
+	virtual void body_attach_object_instance_ID(RID p_body, uint32_t p_ID) = 0;
+	virtual uint32_t body_get_object_instance_ID(RID p_body) const = 0;
 
-	virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable)=0;
-	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const=0;
+	virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) = 0;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const = 0;
 
-	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask)=0;
-	virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const=0;
+	virtual void body_set_layer_mask(RID p_body, uint32_t p_mask) = 0;
+	virtual uint32_t body_get_layer_mask(RID p_body, uint32_t p_mask) const = 0;
 
-	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask)=0;
-	virtual uint32_t body_get_collision_mask(RID p_body, uint32_t p_mask) const=0;
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) = 0;
+	virtual uint32_t body_get_collision_mask(RID p_body, uint32_t p_mask) const = 0;
 
-	virtual void body_set_user_flags(RID p_body, uint32_t p_flags)=0;
-	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const=0;
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) = 0;
+	virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const = 0;
 
 	// common body variables
 	enum BodyParameter {
@@ -417,9 +398,8 @@ public:
 		BODY_PARAM_MAX,
 	};
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value)=0;
-	virtual float body_get_param(RID p_body, BodyParameter p_param) const=0;
-
+	virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value) = 0;
+	virtual float body_get_param(RID p_body, BodyParameter p_param) const = 0;
 
 	//state
 	enum BodyState {
@@ -430,18 +410,18 @@ public:
 		BODY_STATE_CAN_SLEEP
 	};
 
-	virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant)=0;
-	virtual Variant body_get_state(RID p_body, BodyState p_state) const=0;
+	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) = 0;
+	virtual Variant body_get_state(RID p_body, BodyState p_state) const = 0;
 
 	//do something about it
-	virtual void body_set_applied_force(RID p_body, const Vector3& p_force)=0;
-	virtual Vector3 body_get_applied_force(RID p_body) const=0;
+	virtual void body_set_applied_force(RID p_body, const Vector3 &p_force) = 0;
+	virtual Vector3 body_get_applied_force(RID p_body) const = 0;
 
-	virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque)=0;
-	virtual Vector3 body_get_applied_torque(RID p_body) const=0;
+	virtual void body_set_applied_torque(RID p_body, const Vector3 &p_torque) = 0;
+	virtual Vector3 body_get_applied_torque(RID p_body) const = 0;
 
-	virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse)=0;
-	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity)=0;
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) = 0;
+	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) = 0;
 
 	enum BodyAxisLock {
 		BODY_AXIS_LOCK_DISABLED,
@@ -450,29 +430,28 @@ public:
 		BODY_AXIS_LOCK_Z,
 	};
 
-	virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock)=0;
-	virtual BodyAxisLock body_get_axis_lock(RID p_body) const=0;
+	virtual void body_set_axis_lock(RID p_body, BodyAxisLock p_lock) = 0;
+	virtual BodyAxisLock body_get_axis_lock(RID p_body) const = 0;
 
 	//fix
-	virtual void body_add_collision_exception(RID p_body, RID p_body_b)=0;
-	virtual void body_remove_collision_exception(RID p_body, RID p_body_b)=0;
-	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions)=0;
+	virtual void body_add_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void body_remove_collision_exception(RID p_body, RID p_body_b) = 0;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) = 0;
 
-	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts)=0;
-	virtual int body_get_max_contacts_reported(RID p_body) const=0;
+	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) = 0;
+	virtual int body_get_max_contacts_reported(RID p_body) const = 0;
 
 	//missing remove
-	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold)=0;
-	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const=0;
-
-	virtual void body_set_omit_force_integration(RID p_body,bool p_omit)=0;
-	virtual bool body_is_omitting_force_integration(RID p_body) const=0;
+	virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) = 0;
+	virtual float body_get_contacts_reported_depth_treshold(RID p_body) const = 0;
 
-	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
+	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
+	virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
 
-	virtual void body_set_ray_pickable(RID p_body,bool p_enable)=0;
-	virtual bool body_is_ray_pickable(RID p_body) const=0;
+	virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) = 0;
 
+	virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;
+	virtual bool body_is_ray_pickable(RID p_body) const = 0;
 
 	/* JOINT API */
 
@@ -486,13 +465,12 @@ public:
 
 	};
 
-	virtual JointType joint_get_type(RID p_joint) const=0;
-
-	virtual void joint_set_solver_priority(RID p_joint,int p_priority)=0;
-	virtual int joint_get_solver_priority(RID p_joint) const=0;
+	virtual JointType joint_get_type(RID p_joint) const = 0;
 
+	virtual void joint_set_solver_priority(RID p_joint, int p_priority) = 0;
+	virtual int joint_get_solver_priority(RID p_joint) const = 0;
 
-	virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B)=0;
+	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) = 0;
 
 	enum PinJointParam {
 		PIN_JOINT_BIAS,
@@ -500,14 +478,14 @@ public:
 		PIN_JOINT_IMPULSE_CLAMP
 	};
 
-	virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value)=0;
-	virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const=0;
+	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value) = 0;
+	virtual float pin_joint_get_param(RID p_joint, PinJointParam p_param) const = 0;
 
-	virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A)=0;
-	virtual Vector3 pin_joint_get_local_A(RID p_joint) const=0;
+	virtual void pin_joint_set_local_A(RID p_joint, const Vector3 &p_A) = 0;
+	virtual Vector3 pin_joint_get_local_A(RID p_joint) const = 0;
 
-	virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B)=0;
-	virtual Vector3 pin_joint_get_local_B(RID p_joint) const=0;
+	virtual void pin_joint_set_local_B(RID p_joint, const Vector3 &p_B) = 0;
+	virtual Vector3 pin_joint_get_local_B(RID p_joint) const = 0;
 
 	enum HingeJointParam {
 
@@ -528,15 +506,14 @@ public:
 		HINGE_JOINT_FLAG_MAX
 	};
 
-	virtual RID joint_create_hinge(RID p_body_A,const Transform& p_hinge_A,RID p_body_B,const Transform& p_hinge_B)=0;
-	virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B)=0;
-
+	virtual RID joint_create_hinge(RID p_body_A, const Transform &p_hinge_A, RID p_body_B, const Transform &p_hinge_B) = 0;
+	virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) = 0;
 
-	virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value)=0;
-	virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const=0;
+	virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, float p_value) = 0;
+	virtual float hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const = 0;
 
-	virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value)=0;
-	virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const=0;
+	virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) = 0;
+	virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const = 0;
 
 	enum SliderJointParam {
 		SLIDER_JOINT_LINEAR_LIMIT_UPPER,
@@ -564,13 +541,12 @@ public:
 		SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING,
 		SLIDER_JOINT_MAX
 
-
 	};
 
-	virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
+	virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A
 
-	virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value)=0;
-	virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const=0;
+	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, float p_value) = 0;
+	virtual float slider_joint_get_param(RID p_joint, SliderJointParam p_param) const = 0;
 
 	enum ConeTwistJointParam {
 		CONE_TWIST_JOINT_SWING_SPAN,
@@ -581,12 +557,10 @@ public:
 		CONE_TWIST_MAX
 	};
 
+	virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A
 
-	virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
-
-	virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value)=0;
-	virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const=0;
-
+	virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, float p_value) = 0;
+	virtual float cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const = 0;
 
 	enum G6DOFJointAxisParam {
 		G6DOF_JOINT_LINEAR_LOWER_LIMIT,
@@ -614,16 +588,13 @@ public:
 		G6DOF_JOINT_FLAG_MAX
 	};
 
+	virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) = 0; //reference frame is A
 
+	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, float p_value) = 0;
+	virtual float generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) = 0;
 
-	virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
-
-	virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value)=0;
-	virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param)=0;
-
-	virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable)=0;
-	virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag)=0;
-
+	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) = 0;
+	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) = 0;
 
 #if 0
 	enum JointType {
@@ -663,17 +634,16 @@ public:
 		AREA_BODY_REMOVED
 	};
 
-
 	/* MISC */
 
-	virtual void free(RID p_rid)=0;
+	virtual void free(RID p_rid) = 0;
 
-	virtual void set_active(bool p_active)=0;
-	virtual void init()=0;
-	virtual void step(float p_step)=0;
-	virtual void sync()=0;
-	virtual void flush_queries()=0;
-	virtual void finish()=0;
+	virtual void set_active(bool p_active) = 0;
+	virtual void init() = 0;
+	virtual void step(float p_step) = 0;
+	virtual void sync() = 0;
+	virtual void flush_queries() = 0;
+	virtual void finish() = 0;
 
 	enum ProcessInfo {
 
@@ -682,30 +652,30 @@ public:
 		INFO_ISLAND_COUNT
 	};
 
-	virtual int get_process_info(ProcessInfo p_info)=0;
+	virtual int get_process_info(ProcessInfo p_info) = 0;
 
 	PhysicsServer();
 	~PhysicsServer();
 };
 
-VARIANT_ENUM_CAST( PhysicsServer::ShapeType );
-VARIANT_ENUM_CAST( PhysicsServer::SpaceParameter );
-VARIANT_ENUM_CAST( PhysicsServer::AreaParameter );
-VARIANT_ENUM_CAST( PhysicsServer::AreaSpaceOverrideMode );
-VARIANT_ENUM_CAST( PhysicsServer::BodyMode );
-VARIANT_ENUM_CAST( PhysicsServer::BodyParameter );
-VARIANT_ENUM_CAST( PhysicsServer::BodyState );
-VARIANT_ENUM_CAST( PhysicsServer::BodyAxisLock );
-VARIANT_ENUM_CAST( PhysicsServer::PinJointParam );
-VARIANT_ENUM_CAST( PhysicsServer::JointType );
-VARIANT_ENUM_CAST( PhysicsServer::HingeJointParam );
-VARIANT_ENUM_CAST( PhysicsServer::HingeJointFlag );
-VARIANT_ENUM_CAST( PhysicsServer::SliderJointParam );
-VARIANT_ENUM_CAST( PhysicsServer::ConeTwistJointParam );
-VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisParam );
-VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisFlag);
+VARIANT_ENUM_CAST(PhysicsServer::ShapeType);
+VARIANT_ENUM_CAST(PhysicsServer::SpaceParameter);
+VARIANT_ENUM_CAST(PhysicsServer::AreaParameter);
+VARIANT_ENUM_CAST(PhysicsServer::AreaSpaceOverrideMode);
+VARIANT_ENUM_CAST(PhysicsServer::BodyMode);
+VARIANT_ENUM_CAST(PhysicsServer::BodyParameter);
+VARIANT_ENUM_CAST(PhysicsServer::BodyState);
+VARIANT_ENUM_CAST(PhysicsServer::BodyAxisLock);
+VARIANT_ENUM_CAST(PhysicsServer::PinJointParam);
+VARIANT_ENUM_CAST(PhysicsServer::JointType);
+VARIANT_ENUM_CAST(PhysicsServer::HingeJointParam);
+VARIANT_ENUM_CAST(PhysicsServer::HingeJointFlag);
+VARIANT_ENUM_CAST(PhysicsServer::SliderJointParam);
+VARIANT_ENUM_CAST(PhysicsServer::ConeTwistJointParam);
+VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisParam);
+VARIANT_ENUM_CAST(PhysicsServer::G6DOFJointAxisFlag);
 //VARIANT_ENUM_CAST( PhysicsServer::ObjectType );
-VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus );
-VARIANT_ENUM_CAST( PhysicsServer::ProcessInfo );
+VARIANT_ENUM_CAST(PhysicsServer::AreaBodyStatus);
+VARIANT_ENUM_CAST(PhysicsServer::ProcessInfo);
 
 #endif