A Whole New World (clang-format edition)

I can show you the code Pretty, with proper whitespace Tell me, coder, now when did You last write readable code? I can open your eyes Make you see your bad indent Force you to respect the style The core devs agreed upon A whole new world A new fantastic code format A de facto standard With some sugar Enforced with clang-format A whole new world A dazzling style we all dreamed of And when we read it through It's crystal clear That now we're in a whole new world of code
author: Rémi Verschelde <rverschelde@gmail.com> 2017-03-05 16:44:50 +0100
committer: Rémi Verschelde <rverschelde@gmail.com> 2017-03-05 16:44:50 +0100
commit: 5dbf1809c6e3e905b94b8764e99491e608122261 (patch)
tree: 5e5a5360db15d86d59ec8c6e4f7eb511388c5a9a /servers/physics
parent: 45438e9918d421b244bfd7776a30e67dc7f2d3e3 (diff)
43 files changed, 4016 insertions, 5035 deletions
diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index 3aa0816b06..d1040baa65 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(real_t p_step) {
 
 	if (!area->test_collision_mask(body)) {
@@ -37,63 +36,55 @@ bool AreaPairSW::setup(real_t 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(real_t 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(real_t p_step) {
 
 	if (!area_a->test_collision_mask(area_b)) {
@@ -111,51 +100,45 @@ bool Area2PairSW::setup(real_t p_step) {
 	}
 
 	//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 = 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(real_t 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 637976a095..8fc7e7efaa 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(real_t p_step);
 	void solve(real_t 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(real_t p_step);
 	void solve(real_t 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 8aed07d5e5..dfb5d191bc 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,39 @@ 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) {
-
-	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;
+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;
 	}
-
-
 }
 
 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 +151,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 +174,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 +204,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 4e6f1c5a51..2c0cd8dbcd 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;
 	real_t 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(real_t p_gravity) { gravity=p_gravity; }
+	_FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; }
 	_FORCE_INLINE_ real_t 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(real_t scale) { gravity_distance_scale=scale; }
+	_FORCE_INLINE_ void set_gravity_distance_scale(real_t scale) { gravity_distance_scale = scale; }
 	_FORCE_INLINE_ real_t get_gravity_distance_scale() const { return gravity_distance_scale; }
 
-	_FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation=p_point_attenuation; }
+	_FORCE_INLINE_ void set_point_attenuation(real_t p_point_attenuation) { point_attenuation = p_point_attenuation; }
 	_FORCE_INLINE_ real_t get_point_attenuation() const { return point_attenuation; }
 
-	_FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp=p_linear_damp; }
+	_FORCE_INLINE_ void set_linear_damp(real_t p_linear_damp) { linear_damp = p_linear_damp; }
 	_FORCE_INLINE_ real_t get_linear_damp() const { return linear_damp; }
 
-	_FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp=p_angular_damp; }
+	_FORCE_INLINE_ void set_angular_damp(real_t p_angular_damp) { angular_damp = p_angular_damp; }
 	_FORCE_INLINE_ real_t 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 7fb3def387..555d5f15c5 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) ) {
-
-				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;
+				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;
 		}
 	}
 
@@ -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;
-		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];
 			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;
-			real_t depth = axis.dot( c.normal );
+			real_t 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;
-		real_t depth = axis.dot( c.normal );
+		real_t 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(real_t 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(real_t 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(real_t p_step,BodySW *p_A, int p_shape_A,const Transf
 
 	//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);
 
-	real_t newlen = hitpos.distance_to(from)-(max-min)*0.01;
-	p_A->set_linear_velocity((mnormal*newlen)/p_step);
+	real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+	p_A->set_linear_velocity((mnormal * newlen) / p_step);
 
 	return true;
 }
 
-
 bool BodyPairSW::setup(real_t 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(real_t 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();
 
 	real_t bias = (real_t)0.3;
 
 	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-A->get_center_of_mass();
-		c.rB = global_B-B->get_center_of_mass()-offset_B;
+		c.rA = global_A - A->get_center_of_mass();
+		c.rB = global_B - B->get_center_of_mass() - offset_B;
 
-		// contact query reporting...
+// 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(real_t 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(real_t 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 {
 			real_t approach = -0.1 * (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+A->get_center_of_mass(), -j_vec );
-		B->apply_impulse( c.rB+B->get_center_of_mass(), j_vec );
-		c.acc_bias_impulse=0;
+		A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec);
+		B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec);
+		c.acc_bias_impulse = 0;
 		Vector3 jb_vec = c.normal * c.acc_bias_impulse;
-		A->apply_bias_impulse( c.rA+A->get_center_of_mass(), -jb_vec );
-		B->apply_bias_impulse( c.rB+B->get_center_of_mass(), jb_vec );
+		A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb_vec);
+		B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb_vec);
 
-		c.bounce = MAX(A->get_bounce(),B->get_bounce());
+		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(real_t 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 + A->get_center_of_mass(), -jb);
+			B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb);
 
-			A->apply_bias_impulse(c.rA+A->get_center_of_mass(),-jb);
-			B->apply_bias_impulse(c.rB+B->get_center_of_mass(), jb);
-
-			c.active=true;
+			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+A->get_center_of_mass(),-j);
-			B->apply_impulse(c.rB+B->get_center_of_mass(), j);
+			A->apply_impulse(c.rA + A->get_center_of_mass(), -j);
+			B->apply_impulse(c.rB + B->get_center_of_mass(), j);
 
-			c.active=true;
+			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(real_t 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(real_t 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 + A->get_center_of_mass(), -jt);
+			B->apply_impulse(c.rB + B->get_center_of_mass(), jt);
 
-			A->apply_impulse( c.rA+A->get_center_of_mass(), -jt );
-			B->apply_impulse( c.rB+B->get_center_of_mass(), jt );
-
-			c.active=true;
-
+			c.active = true;
 		}
-
-
 	}
-
 }
 
-
-
-
-
-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;
-
+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;
 }
 
-
 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 f282a56b9e..fa426adafd 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; // Offset in world orientation with respect to center of mass
+		Vector3 rA, rB; // Offset in world orientation with respect to center of mass
 	};
 
 	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
@@ -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(real_t 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(real_t 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(real_t p_step);
 	void solve(real_t 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 7fcd767268..a4fc694f67 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -27,18 +27,17 @@
 /* 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_transform_dependant() {
-	
+
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
 
@@ -47,30 +46,29 @@ void BodySW::_update_transform_dependant() {
 	Basis tbt = tb.transposed();
 	tb.scale(_inv_inertia);
 	_inv_inertia_tensor = tb * tbt;
-
 }
 
 void BodySW::update_inertias() {
 
 	//update shapes and motions
 
-	switch(mode) {
+	switch (mode) {
 
 		case PhysicsServer::BODY_MODE_RIGID: {
 
 			//update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
-			real_t total_area=0;
+			real_t 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_area(i);
+				total_area += get_shape_area(i);
 			}
 
 			// We have to recompute the center of mass
 			center_of_mass_local.zero();
 
-			for (int i=0; i<get_shape_count(); i++) {
-				real_t area=get_shape_area(i);
+			for (int i = 0; i < get_shape_count(); i++) {
+				real_t area = get_shape_area(i);
 
 				real_t mass = area * this->mass / total_area;
 
@@ -84,25 +82,23 @@ void BodySW::update_inertias() {
 			Basis inertia_tensor;
 			inertia_tensor.set_zero();
 
-			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);
 
-				real_t area=get_shape_area(i);
+				real_t area = get_shape_area(i);
 
 				real_t mass = area * this->mass / total_area;
 
-				Basis shape_inertia_tensor=shape->get_moment_of_inertia(mass).to_diagonal_matrix();
-				Transform shape_transform=get_shape_transform(i);
+				Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
+				Transform shape_transform = get_shape_transform(i);
 				Basis shape_basis = shape_transform.basis.orthonormalized();
 
 				// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
 				shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
 
 				Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
-				inertia_tensor += shape_inertia_tensor + (Basis()*shape_origin.dot(shape_origin)-shape_origin.outer(shape_origin))*mass;
-
-
+				inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
 			}
 
 			// Compute the principal axes of inertia
@@ -110,9 +106,9 @@ void BodySW::update_inertias() {
 			_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
 
 			if (mass)
-				_inv_mass=1.0/mass;
+				_inv_mass = 1.0 / mass;
 			else
-				_inv_mass=0;
+				_inv_mass = 0;
 
 		} break;
 
@@ -120,42 +116,39 @@ void BodySW::update_inertias() {
 		case PhysicsServer::BODY_MODE_STATIC: {
 
 			_inv_inertia_tensor.set_zero();
-			_inv_mass=0;
+			_inv_mass = 0;
 		} break;
 		case PhysicsServer::BODY_MODE_CHARACTER: {
 
 			_inv_inertia_tensor.set_zero();
-			_inv_mass=1.0/mass;
+			_inv_mass = 1.0 / mass;
 
 		} break;
 	}
 
-
 	//_update_shapes();
 
 	_update_transform_dependant();
 }
 
-
-
 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;
 
@@ -168,43 +161,41 @@ void BodySW::set_active(bool p_active) {
 */
 }
 
-
-
 void BodySW::set_param(PhysicsServer::BodyParameter p_param, real_t 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: {}
 	}
 }
 
 real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
 
-	switch(p_param) {
+	switch (p_param) {
 		case PhysicsServer::BODY_PARAM_BOUNCE: {
 
 			return bounce;
@@ -228,7 +219,7 @@ real_t BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
 			return angular_damp;
 		} break;
 
-		default:{}
+		default: {}
 	}
 
 	return 0;
@@ -236,35 +227,35 @@ real_t 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;
 	}
@@ -274,7 +265,6 @@ void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
 	if (get_space())
 		_update_queries();
 	*/
-
 }
 PhysicsServer::BodyMode BodySW::get_mode() const {
 
@@ -286,35 +276,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();
 
@@ -325,7 +313,7 @@ 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: {
@@ -333,38 +321,37 @@ void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_varian
 			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;
@@ -385,8 +372,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()) {
 
@@ -396,7 +382,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);
@@ -413,19 +398,17 @@ void BodySW::set_space(SpaceSW *p_space){
 			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();
 		}
@@ -439,8 +422,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();
@@ -450,192 +432,179 @@ 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
-		Basis rot=new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
+		Basis rot = new_transform.basis.orthonormalized().transposed() * get_transform().basis.orthonormalized();
 		Vector3 axis;
 		real_t 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;
 
 	real_t 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;
-		Basis rot( ang_vel_axis, ang_vel*p_step );
+		Basis rot(ang_vel_axis, ang_vel * p_step);
 		Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
 		transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
 		transform.basis = rot * transform.basis;
 		transform.orthonormalize();
 	}
 
-	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());
@@ -684,18 +653,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())
@@ -706,109 +675,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;
+	mass = 1;
 	//_inv_inertia=Transform();
-	_inv_mass=1;
-	bounce=0;
-	friction=1;
-	omit_force_integration=false;
+	_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;
+	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() {
@@ -817,9 +773,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 2383d2d688..4b1af6fca5 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;
@@ -78,7 +76,6 @@ class BodySW : public CollisionObjectSW {
 	real_t area_angular_damp;
 	real_t area_linear_damp;
 
-
 	SelfList<BodySW> active_list;
 	SelfList<BodySW> inertia_update_list;
 	SelfList<BodySW> direct_state_query_list;
@@ -96,23 +93,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;
 		real_t depth;
@@ -136,7 +135,6 @@ class BodySW : public CollisionObjectSW {
 
 	ForceIntegrationCallback *fi_callback;
 
-
 	uint64_t island_step;
 	BodySW *island_next;
 	BodySW *island_list_next;
@@ -145,16 +143,14 @@ class BodySW : public CollisionObjectSW {
 
 	_FORCE_INLINE_ void _update_transform_dependant();
 
-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));
@@ -163,77 +159,80 @@ 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, real_t 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, real_t 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_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; }
 	_FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
-	_FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3& p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); } 
+	_FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); }
 
-	_FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }
+	_FORCE_INLINE_ 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-center_of_mass).cross(p_j) );
+		angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j));
 	}
 
-	_FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+	_FORCE_INLINE_ void apply_torque_impulse(const Vector3 &p_j) {
 
 		angular_velocity += _inv_inertia_tensor.xform(p_j);
 	}
 
-	_FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) {
+	_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-center_of_mass).cross(p_j) );
+		biased_angular_velocity += _inv_inertia_tensor.xform((p_pos - center_of_mass).cross(p_j));
 	}
 
-	_FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3& p_j) {
+	_FORCE_INLINE_ void apply_bias_torque_impulse(const Vector3 &p_j) {
 
 		biased_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);
@@ -243,7 +242,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);
 	}
@@ -254,16 +253,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);
@@ -277,33 +276,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-center_of_mass);
+		return linear_velocity + angular_velocity.cross(rel_pos - center_of_mass);
 	}
 
-	_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const {
+	_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3 &p_pos, const Vector3 &p_normal) const {
 
-		 Vector3 r0 = p_pos - get_transform().origin - center_of_mass;
+		Vector3 r0 = p_pos - get_transform().origin - center_of_mass;
 
-		 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();
@@ -313,117 +311,133 @@ public:
 
 	BodySW();
 	~BodySW();
-
 };
 
-
 //add contact inline
 
-void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, real_t 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, real_t 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 {
 
-		real_t least_depth=1e20;
-		int least_deep=-1;
-		for(int i=0;i<c_max;i++) {
+		real_t 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 {
 
-	GDCLASS( PhysicsDirectBodyStateSW, PhysicsDirectBodyState );
+	GDCLASS(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 real_t get_total_angular_damp() const {  return body->area_angular_damp;  } // get density of this body space/area
-	virtual real_t 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 real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area
+	virtual real_t get_total_linear_damp() const { return body->area_linear_damp; } // get density of this body space/area
 
 	virtual Vector3 get_center_of_mass() const { return body->get_center_of_mass(); }
 	virtual Basis get_principal_inertia_axes() const { return body->get_principal_inertia_axes(); }
 
-	virtual real_t 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 Basis get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor();   } // get density of this body space
+	virtual real_t 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 Basis 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 apply_torque_impulse(const Vector3& p_j) { body->apply_torque_impulse(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 apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); }
 
-	virtual void set_sleep_state(bool p_enable) {  body->set_active(!p_enable);  }
-	virtual bool is_sleeping() const {  return !body->is_active();  }
+	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 f1c22caae3..ca9bb40842 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 Rect3& p_aabb) {
+void BroadPhaseBasic::move(ID p_id, const Rect3 &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 Rect3 aabb=E->get().aabb;
-		if (aabb.intersects_segment(p_from,p_to)) {
+		const Rect3 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 Rect3& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) {
+int BroadPhaseBasic::cull_aabb(const Rect3 &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 Rect3 aabb=E->get().aabb;
+		const Rect3 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 Rect3& 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 9f07e896c4..2824af6b68 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 Rect3& p_aabb);
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const Rect3 &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 Rect3& 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 Rect3 &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 89581997a2..cb64077d9a 100644
--- a/servers/physics/broad_phase_octree.cpp
+++ b/servers/physics/broad_phase_octree.cpp
@@ -31,85 +31,77 @@
 
 BroadPhaseSW::ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) {
 
-	ID oid = octree.create(p_object,Rect3(),p_subindex,false,1<<p_object->get_type(),0);
+	ID oid = octree.create(p_object, Rect3(), p_subindex, false, 1 << p_object->get_type(), 0);
 	return oid;
 }
 
-void BroadPhaseOctree::move(ID p_id, const Rect3& p_aabb){
+void BroadPhaseOctree::move(ID p_id, const Rect3 &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 Rect3& 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 Rect3 &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 29f1123edf..f9a8bd17ed 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 Rect3& p_aabb);
+	virtual ID create(CollisionObjectSW *p_object_, int p_subindex = 0);
+	virtual void move(ID p_id, const Rect3 &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 Rect3& 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 Rect3 &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 2a897dea2b..f9a19e558b 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 20b3efc7fb..df6ea1cc73 100644
--- a/servers/physics/broad_phase_sw.h
+++ b/servers/physics/broad_phase_sw.h
@@ -34,40 +34,37 @@
 
 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 Rect3& 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 Rect3 &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 Rect3& 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 Rect3 &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 88aa737579..36704b6eb8 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,54 +124,50 @@ 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..
-		Rect3 shape_aabb=s.shape->get_aabb();
+		Rect3 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);
 
 		Vector3 scale = xform.get_basis().get_scale();
-		s.area_cache=s.shape->get_area()*scale.x*scale.y*scale.z;
+		s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
 
-		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..
-		Rect3 shape_aabb=s.shape->get_aabb();
+		Rect3 shape_aabb = s.shape->get_aabb();
 		Transform xform = transform * s.xform;
-		shape_aabb=xform.xform(shape_aabb);
-		shape_aabb=shape_aabb.merge(Rect3( 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(Rect3(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) {
@@ -185,25 +176,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() {
@@ -214,11 +203,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 c46a7f4a5f..9a7626d583 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;
@@ -65,7 +65,7 @@ private:
 		ShapeSW *shape;
 		bool trigger;
 
-		Shape() { trigger=false; }
+		Shape() { trigger = false; }
 	};
 
 	Vector<Shape> shapes;
@@ -77,84 +77,80 @@ 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 Rect3& 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 Rect3 &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
 	_FORCE_INLINE_ const real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }
 
 	_FORCE_INLINE_ Transform get_transform() const { return transform; }
 	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
-	_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 9d3e1db47b..003e6b3257 100644
--- a/servers/physics/collision_solver_sat.cpp
+++ b/servers/physics/collision_solver_sat.cpp
@@ -40,7 +40,7 @@ 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))
@@ -49,69 +49,61 @@ struct _CollectorCallback {
 		*/
 
 		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 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 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)) < CMP_EPSILON) {
 
 		// should handle somehow..
 		//ERR_PRINT("TODO FIX");
@@ -122,117 +114,110 @@ 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
-		real_t 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]) };
+		real_t 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<real_t> 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) < 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++) {
 
 		real_t d = plane_B.distance_to(clipbuf_src[i]);
 		/*
@@ -240,40 +225,37 @@ static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_
 			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,
 		}
 	};
 
@@ -289,28 +271,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;
@@ -325,46 +304,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
 		}
 
@@ -372,68 +350,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++) {
@@ -445,29 +420,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 *******/
@@ -475,92 +447,84 @@ public:
 /****** SAT TESTS *******/
 /****** SAT TESTS *******/
 
+typedef void (*CollisionFunc)(const ShapeSW *, const Transform &, const ShapeSW *, const Transform &, _CollectorCallback *p_callback, real_t, real_t);
 
-typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback,real_t,real_t);
-
-
-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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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;
@@ -571,38 +535,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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t p_margin_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);
+	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);
 
 	if (!separator.test_previous_axis())
 		return;
@@ -616,146 +577,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,real_t p_margin_a,real_t p_margin_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, real_t p_margin_a, real_t p_margin_b) {
 
-	const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a);
-	const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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;
 			}
 		}
@@ -768,110 +710,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 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-		Vector3 support_a=p_transform_a.xform( Vector3(
+		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
-		) );
+				(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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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;
 			}
 		}
@@ -879,58 +814,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++) {
-
-
-		Vector3 capsule_axis = p_transform_b.basis.get_axis(2)*(capsule_B->get_height()*0.5);
+	for (int i = 0; i < 2; i++) {
 
-		Vector3 sphere_pos = p_transform_b.origin + ((i==0)?capsule_axis:-capsule_axis);
+		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 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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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();
@@ -941,97 +869,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 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-			Vector3 support_a=p_transform_a.xform( Vector3(
+			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
-			) );
+					(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];
-
-					for(int e=0;e<edge_count;e++) {
+					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 p1=p_transform_b.xform(vertices[edges[e].a]);
-						Vector3 p2=p_transform_b.xform(vertices[edges[e].b]);
-						Vector3 n = (p2-p1);
+					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);
 
-						if (!separator.test_axis( (point-p2).cross(n).cross(n).normalized() ))
+						if (!separator.test_axis((point - p2).cross(n).cross(n).normalized()))
 							return;
 					}
 				}
@@ -1040,130 +963,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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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 cnormal_a = p_transform_a.basis.xform_inv(ab_vec);
 
-			Vector3 support_a=p_transform_a.xform( Vector3(
+			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
-			) );
+					(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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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;
@@ -1180,49 +1092,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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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;
@@ -1236,127 +1144,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 sphere_pos = p_transform_a.origin + ((i==0)?capsule_axis:-capsule_axis);
+		Vector3 capsule_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5);
 
-		for (int j=0;j<edge_count;j++) {
+		Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
+		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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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;
@@ -1380,107 +1274,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.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++) {
+	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.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++) {
-
-			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 i = 0; i < edge_count_A; i++) {
 
-			for(int j=0;j<vertex_count_B;j++) {
+			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 e3=p_transform_b.xform(vertices_B[j]);
+			for (int j = 0; j < vertex_count_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++) {
+		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);
+			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 j=0;j<vertex_count_A;j++) {
+			for (int j = 0; j < vertex_count_A; j++) {
 
-				Vector3 e3=p_transform_a.xform(vertices_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, real_t p_margin_a, real_t 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,real_t p_margin_a,real_t 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();
 
@@ -1491,207 +1375,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.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++) {
+		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);
+			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 j=0;j<3;j++) {
+			for (int j = 0; j < 3; j++) {
 
-				Vector3 e3=vertex[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++) {
-
-			Vector3 e1=vertex[i];
-			Vector3 e2=vertex[(i+1)%3];
-			Vector3 n = (e2-e1);
+		for (int i = 0; i < 3; i++) {
 
-			for(int j=0;j<vertex_count;j++) {
+			Vector3 e1 = vertex[i];
+			Vector3 e2 = vertex[(i + 1) % 3];
+			Vector3 n = (e2 - e1);
 
-				Vector3 e3=p_transform_a.xform(vertices[j]);
+			for (int j = 0; j < vertex_count; 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,real_t p_margin_a,real_t 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_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},
+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, real_t p_margin_a, real_t 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_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_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;
-
-	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;
-	real_t margin_A=p_margin_a;
-	real_t margin_B=p_margin_b;
+	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;
+	real_t margin_A = p_margin_a;
+	real_t 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 15fe7dc34a..67ffb0b068 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,real_t p_margin_a=0,real_t 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, real_t p_margin_a = 0, real_t 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 f0ddde3a76..0f6e964359 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;
 	real_t margin_A;
 	real_t 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,real_t p_margin_A,real_t 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, real_t p_margin_A, real_t 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.aabb_tests=0;
+	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;
 
 	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
 
 	Rect3 local_aabb;
-	for(int i=0;i<3;i++) {
-
-	     Vector3 axis( p_transform_B.basis.get_axis(i) );
-	     real_t axis_scale = 1.0/axis.length();
-	     axis*=axis_scale;
+	for (int i = 0; i < 3; i++) {
 
-	     real_t 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));
+		real_t axis_scale = 1.0 / axis.length();
+		axis *= axis_scale;
 
+		real_t 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, real_t p_margin_A, real_t 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,real_t p_margin_A,real_t 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;
 	real_t 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 Rect3& 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 Rect3 &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!=Rect3();
+		bool use_cc_hint = p_concave_hint != Rect3();
 		Rect3 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;
 		}
 
 		Rect3 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) );
-		     real_t axis_scale = ((real_t)1.0)/axis.length();
-		     axis*=axis_scale;
+			Vector3 axis(p_transform_B.basis.get_axis(i));
+			real_t axis_scale = ((real_t)1.0) / axis.length();
+			axis *= axis_scale;
 
-		     real_t smin,smax;
+			real_t 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;
-
+			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 b8d37a8a96..b0f18dc0ac 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,real_t p_margin_A=0,real_t 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, real_t p_margin_A = 0, real_t 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,real_t p_margin_A=0,real_t 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 Rect3& 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, real_t p_margin_A = 0, real_t 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 Rect3 &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 f5bbe4bbd0..2cd0e1a420 100644
--- a/servers/physics/constraint_sw.h
+++ b/servers/physics/constraint_sw.h
@@ -40,35 +40,37 @@ class ConstraintSW : public RID_Data {
 	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(real_t p_step)=0;
-	virtual void solve(real_t p_step)=0;
+	virtual bool setup(real_t p_step) = 0;
+	virtual void solve(real_t p_step) = 0;
 
 	virtual ~ConstraintSW() {}
 };
diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp
index 06c2c2382c..f65e6768ab 100644
--- a/servers/physics/gjk_epa.cpp
+++ b/servers/physics/gjk_epa.cpp
@@ -884,33 +884,30 @@ bool Penetration(	const ShapeSW*	shape0,
 
 /* clang-format on */
 
-
-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;
 	}
diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h
index 58cf8f50ce..ae5db733b5 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 d78bcbd16d..8cab81de2c 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) {
-
-  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 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);
+	}
 }
 
-
-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(real_t p_step) {
+bool ConeTwistJointSW::setup(real_t p_step) {
 	m_appliedImpulse = real_t(0.);
 
 	//set bias, sign, clear accumulator
@@ -107,109 +103,97 @@ bool	ConeTwistJointSW::setup(real_t 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_principal_inertia_axes().transposed(),
-				B->get_principal_inertia_axes().transposed(),
-				pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-				pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-				normal[i],
-				A->get_inv_inertia(),
-				A->get_inv_mass(),
-				B->get_inv_inertia(),
-				B->get_inv_mass()));
+												A->get_principal_inertia_axes().transposed(),
+												B->get_principal_inertia_axes().transposed(),
+												pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+												pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+												normal[i],
+												A->get_inv_inertia(),
+												A->get_inv_mass(),
+												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) );
+	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) );
+	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(real_t 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, real_t 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;
 	}
 }
 
-real_t ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
+real_t 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 eb7a5cd1b1..c122c22258 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(real_t p_step);
 	virtual void solve(real_t 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, real_t p_value);
 	real_t 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 bd07f3122a..1e07bc73fb 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -34,118 +34,90 @@ See corresponding header file for licensing info.
 
 #include "generic_6dof_joint_sw.h"
 
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-
 //////////////////////////// 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;
 
+	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
 
-    Vector3 motorImp = clippedMotorImpulse * axis;
-
-
-    body0->apply_torque_impulse(motorImp);
-    if (body1) body1->apply_torque_impulse(-motorImp);
-
-    return clippedMotorImpulse;
+	Vector3 motorImp = clippedMotorImpulse * axis;
 
+	body0->apply_torque_impulse(motorImp);
+	if (body1) body1->apply_torque_impulse(-motorImp);
 
+	return clippedMotorImpulse;
 }
 
 //////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
@@ -153,120 +125,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;
+		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) {
 
-    Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
-    Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
-    Vector3 vel = vel1 - vel2;
+	///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;
 
-    real_t rel_vel = axis_normal_on_a.dot(vel);
+	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()
-{
-	Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
+void Generic6DOFJointSW::calculateAngleInfo() {
+	Basis relative_frame = m_calculatedTransformA.basis.inverse() * m_calculatedTransformB.basis;
 
 	m_calculatedAxisAngleDiff = relative_frame.get_euler();
 
-
-
 	// 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);
@@ -275,7 +223,6 @@ void Generic6DOFJointSW::calculateAngleInfo()
 	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
 	/*
 	if(m_debugDrawer)
 	{
@@ -288,280 +235,250 @@ void Generic6DOFJointSW::calculateAngleInfo()
 		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_principal_inertia_axes().transposed(),
-	B->get_principal_inertia_axes().transposed(),
-	pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-	pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-	normalWorld,
-	A->get_inv_inertia(),
-	A->get_inv_mass(),
-	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_principal_inertia_axes().transposed(),
+										 B->get_principal_inertia_axes().transposed(),
+										 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+										 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+										 normalWorld,
+										 A->get_inv_inertia(),
+										 A->get_inv_mass(),
+										 B->get_inv_inertia(),
+										 B->get_inv_mass()));
 }
 
 void Generic6DOFJointSW::buildAngularJacobian(
-    JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
-{
-    memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
-				      A->get_principal_inertia_axes().transposed(),
-				      B->get_principal_inertia_axes().transposed(),
-				      A->get_inv_inertia(),
-				      B->get_inv_inertia()));
-
+		JacobianEntrySW &jacAngular, const Vector3 &jointAxisW) {
+	memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
+										  A->get_principal_inertia_axes().transposed(),
+										  B->get_principal_inertia_axes().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(real_t 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();
-
-//  const Vector3& pivotAInW = m_calculatedTransformA.origin;
-//  const Vector3& pivotBInW = m_calculatedTransformB.origin;
+	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;
 	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, real_t p_value) {
 
-void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, real_t 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;
 	}
 }
 
-real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
-	ERR_FAIL_INDEX_V(p_axis,3,0);
-	switch(p_param) {
+real_t 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];
@@ -635,31 +552,29 @@ real_t Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJo
 	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 207ae85a45..87245c6ffe 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;
-
-    //!@}
-
-    //! 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;
-    }
+	//! 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;
+	}
 
 	//! 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 {
+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];
 
-};
+	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);
 
+		enable_limit[0] = true;
+		enable_limit[1] = true;
+		enable_limit[2] = true;
+	}
 
-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];
-
-    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);
-
-	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;
-
-	m_limitSoftness = other.m_limitSoftness ;
-	m_damping = other.m_damping;
-	m_restitution = other.m_restitution;
-    }
-
-    //! Test limit
+	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;
+	}
+
+	//! 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);
+	Generic6DOFJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA);
 
-    virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
-
-    virtual bool setup(real_t p_step);
-    virtual void solve(real_t p_step);
+	virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
 
+	virtual bool setup(real_t p_step);
+	virtual void solve(real_t 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);
-
-    void	setLinearLowerLimit(const Vector3& linearLower)
-    {
-	m_linearLimits.m_lowerLimit = linearLower;
-    }
-
-    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	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;
-    }
+	bool testAngularLimitMotor(int axis_index);
+
+	void setLinearLowerLimit(const Vector3 &linearLower) {
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
+
+	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 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;
-    }
-
-    //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;
+	G6DOFRotationalLimitMotorSW *getRotationalLimitMotor(int index) {
+		return &m_angularLimits[index];
+	}
+
+	//! 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();
+	}
 
+	const BodySW *getRigidBodyA() const {
+		return A;
+	}
+	const BodySW *getRigidBodyB() const {
+		return B;
 	}
-	return m_angularLimits[limitIndex-3].isLimited();
-    }
-
-    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, real_t p_value);
-    real_t 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, real_t p_value);
+	real_t 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 9617eb8794..eaa57af873 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) {
-
-  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 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);
+	}
 }
 
-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=Basis( rbAxisA1.x,rbAxisA2.x,axisInA.x,
-									rbAxisA1.y,rbAxisA2.y,axisInA.y,
-									rbAxisA1.z,rbAxisA2.z,axisInA.z );
+	m_rbAFrame.basis = Basis(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=Basis( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
-									rbAxisB1.y,rbAxisB2.y,-axisInB.y,
-									rbAxisB1.z,rbAxisB2.z,-axisInB.z );
+	m_rbBFrame.basis = Basis(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(real_t 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_principal_inertia_axes().transposed(),
-				B->get_principal_inertia_axes().transposed(),
-				pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-				pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-				normal[i],
-				A->get_inv_inertia(),
-				A->get_inv_mass(),
-				B->get_inv_inertia(),
-				B->get_inv_mass()) );
+												A->get_principal_inertia_axes().transposed(),
+												B->get_principal_inertia_axes().transposed(),
+												pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+												pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+												normal[i],
+												A->get_inv_inertia(),
+												A->get_inv_mass(),
+												B->get_inv_inertia(),
+												B->get_inv_mass()));
 		}
 	}
 
@@ -192,31 +181,30 @@ bool HingeJointSW::setup(real_t 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_principal_inertia_axes().transposed(),
-		B->get_principal_inertia_axes().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_principal_inertia_axes().transposed(),
-		B->get_principal_inertia_axes().transposed(),
-		A->get_inv_inertia(),
-		B->get_inv_inertia()));
+	memnew_placement(&m_jacAng[0], JacobianEntrySW(jointAxis0,
+										   A->get_principal_inertia_axes().transposed(),
+										   B->get_principal_inertia_axes().transposed(),
+										   A->get_inv_inertia(),
+										   B->get_inv_inertia()));
 
-	memnew_placement(&m_jacAng[2],	JacobianEntrySW(hingeAxisWorld,
-		A->get_principal_inertia_axes().transposed(),
-		B->get_principal_inertia_axes().transposed(),
-		A->get_inv_inertia(),
-		B->get_inv_inertia()));
+	memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
+										   A->get_principal_inertia_axes().transposed(),
+										   B->get_principal_inertia_axes().transposed(),
+										   A->get_inv_inertia(),
+										   B->get_inv_inertia()));
 
+	memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
+										   A->get_principal_inertia_axes().transposed(),
+										   B->get_principal_inertia_axes().transposed(),
+										   A->get_inv_inertia(),
+										   B->get_inv_inertia()));
 
 	// Compute limit information
 	real_t hingeAngle = get_hinge_angle();
@@ -228,26 +216,21 @@ bool HingeJointSW::setup(real_t p_step) {
 	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 (m_useLimit && m_lowerLimit <= m_upperLimit) {
 		//if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		if (hingeAngle <= m_lowerLimit)
-		{
+		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_correction = m_upperLimit - hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;
@@ -255,9 +238,9 @@ bool HingeJointSW::setup(real_t 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(real_t 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(real_t 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(real_t 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(real_t 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, real_t 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;
 	}
 }
 
-real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
+real_t HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const {
 
 	switch (p_param) {
 
@@ -446,25 +415,23 @@ real_t 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 8469fd1ca0..013d9afdbf 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(real_t 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 cd85162ba5..b0b31ed797 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 Basis& world2A,
-		const Basis& 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 Basis &world2A,
+			const Basis &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 Basis& world2A,
-		const Basis& 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 Basis &world2A,
+			const Basis &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 Basis& 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 Basis &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 b545ae630b..e01514f4b6 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -38,41 +38,37 @@ bool PinJointSW::setup(real_t 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_principal_inertia_axes().transposed(),
-			B->get_principal_inertia_axes().transposed(),
-			A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
-			B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
-			normal,
-			A->get_inv_inertia(),
-			A->get_inv_mass(),
-			B->get_inv_inertia(),
-			B->get_inv_mass()));
+		memnew_placement(&m_jac[i], JacobianEntrySW(
+											A->get_principal_inertia_axes().transposed(),
+											B->get_principal_inertia_axes().transposed(),
+											A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+											B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
+											normal,
+											A->get_inv_inertia(),
+											A->get_inv_mass(),
+											B->get_inv_inertia(),
+											B->get_inv_mass()));
 		normal[i] = 0;
 	}
 
 	return true;
 }
 
-void PinJointSW::solve(real_t p_step){
+void PinJointSW::solve(real_t 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();
 
-	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(real_t 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(real_t 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,real_t p_value) {
+void PinJointSW::set_param(PhysicsServer::PinJointParam p_param, real_t 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;
 	}
 }
 
-real_t PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
+real_t 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 @@ real_t 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 b72b21219d..9500d4b46d 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(real_t p_step);
 	virtual void solve(real_t p_step);
 
-	void set_param(PhysicsServer::PinJointParam p_param,real_t p_value);
+	void set_param(PhysicsServer::PinJointParam p_param, real_t p_value);
 	real_t 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 fc728ed0ba..b8a6c1ecaf 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(real_t p_step)
-{
+bool SliderJointSW::setup(real_t 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(real_t 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_principal_inertia_axes().transposed(),
-			B->get_principal_inertia_axes().transposed(),
-			m_relPosA - A->get_center_of_mass(),
-			m_relPosB - B->get_center_of_mass(),
-			normalWorld,
-			A->get_inv_inertia(),
-			A->get_inv_mass(),
-			B->get_inv_inertia(),
-			B->get_inv_mass()
-			));
+											   A->get_principal_inertia_axes().transposed(),
+											   B->get_principal_inertia_axes().transposed(),
+											   m_relPosA - A->get_center_of_mass(),
+											   m_relPosB - B->get_center_of_mass(),
+											   normalWorld,
+											   A->get_inv_inertia(),
+											   A->get_inv_mass(),
+											   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_principal_inertia_axes().transposed(),
-	    B->get_principal_inertia_axes().transposed(),
-	    A->get_inv_inertia(),
-	    B->get_inv_inertia()
-			));
+		memnew_placement(&m_jacAng[i], JacobianEntrySW(
+											   normalWorld,
+											   A->get_principal_inertia_axes().transposed(),
+											   B->get_principal_inertia_axes().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(real_t 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, real_t 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;
 	}
-
 }
 
 real_t 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 @@ real_t 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 d01038df59..faf36bfe9e 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(real_t 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 c87c86b599..0f637faf79 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 67e3b27852..37be0a8a1c 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, real_t 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);
 };
 
 real_t 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,21 +732,21 @@ 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_apply_torque_impulse(RID p_body, const Vector3& p_impulse) {
+void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -814,40 +755,34 @@ void PhysicsServerSW::body_apply_torque_impulse(RID p_body, const Vector3& p_imp
 	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);
@@ -855,7 +790,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) {
@@ -865,7 +799,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) {
@@ -873,27 +806,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, real_t p_treshold) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
-
 };
 
 real_t 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);
@@ -904,7 +835,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();
 };
 
@@ -918,355 +849,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, real_t p_value){
+void PhysicsServerSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t 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);
 }
-real_t PhysicsServerSW::pin_joint_get_param(RID p_joint,PinJointParam p_param) const{
+real_t 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, real_t p_value){
+void PhysicsServerSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t 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);
 }
-real_t PhysicsServerSW::hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const{
+real_t 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, real_t p_value){
+void PhysicsServerSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t 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);
 }
-real_t PhysicsServerSW::slider_joint_get_param(RID p_joint,SliderJointParam p_param) const{
+real_t 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, real_t p_value) {
+void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t 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);
 }
-real_t PhysicsServerSW::cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const {
+real_t 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, real_t p_value){
+void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t 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);
 }
-real_t PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param){
+real_t 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) {
 
@@ -1384,8 +1293,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);
 		}
 
@@ -1405,8 +1314,7 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		body->set_space(NULL);
 
-
-		while( body->get_shape_count() ) {
+		while (body->get_shape_count()) {
 
 			body->remove_shape(0);
 		}
@@ -1431,7 +1339,7 @@ void PhysicsServerSW::free(RID p_rid) {
 
 		area->set_space(NULL);
 
-		while( area->get_shape_count() ) {
+		while (area->get_shape_count()) {
 
 			area->remove_shape(0);
 		}
@@ -1442,7 +1350,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);
 		}
@@ -1457,7 +1365,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);
 		}
@@ -1469,51 +1377,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(real_t 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(){
 
 };
 
@@ -1522,21 +1424,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",
@@ -1544,44 +1445,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: {
 
@@ -1594,64 +1490,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
-		real_t min_depth=1e20;
-		int min_depth_idx=0;
-		for(int i=0;i<cbk->amount;i++) {
-
-			real_t 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;
+		real_t min_depth = 1e20;
+		int min_depth_idx = 0;
+		for (int i = 0; i < cbk->amount; i++) {
+
+			real_t 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;
 			}
-
 		}
 
 		real_t 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 6e20474350..cb5a339ee8 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 {
 
-	GDCLASS( PhysicsServerSW, PhysicsServer );
+	GDCLASS(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;
 
@@ -64,7 +62,6 @@ friend class PhysicsDirectSpaceStateSW;
 
 	//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,20 +179,20 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value);
 	virtual real_t 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_apply_torque_impulse(RID p_body, const Vector3& p_impulse);
-	virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity);
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse);
+	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse);
+	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity);
 
-	virtual void body_set_axis_lock(RID p_body,BodyAxisLock p_lock);
+	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);
@@ -206,61 +202,60 @@ public:
 	virtual void body_set_contacts_reported_depth_treshold(RID p_body, real_t p_treshold);
 	virtual real_t 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, real_t p_value);
-	virtual real_t pin_joint_get_param(RID p_joint,PinJointParam p_param) const;
+	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 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, real_t p_value);
-	virtual real_t 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, real_t p_value);
+	virtual real_t 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, real_t p_value);
-	virtual real_t slider_joint_get_param(RID p_joint,SliderJointParam p_param) const;
+	virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value);
+	virtual real_t 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, real_t p_value);
-	virtual real_t 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, real_t p_value);
+	virtual real_t 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, real_t p_value);
-	virtual real_t 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, real_t p_value);
+	virtual real_t 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
@@ -290,8 +285,6 @@ public:
 
 	PhysicsServerSW();
 	~PhysicsServerSW();
-
 };
 
 #endif
-
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp
index dec1c75d9f..4ce716c70a 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 Rect3& 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 Rect3 &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(real_t p_mass) const {
 	return Vector3(); //wtf
 }
 
-void PlaneShapeSW::_setup(const Plane& p_plane)  {
+void PlaneShapeSW::_setup(const Plane &p_plane) {
 
-	plane=p_plane;
-	configure(Rect3(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));
+	plane = p_plane;
+	configure(Rect3(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,38 @@ real_t 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);
-	} else 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);
+	} else 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 +188,15 @@ Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const {
 	return Vector3();
 }
 
-void RayShapeSW::_setup(real_t p_length)  {
+void RayShapeSW::_setup(real_t p_length) {
 
-	length=p_length;
-	configure(Rect3(Vector3(0,0,0),Vector3(0.1,0.1,length)));
+	length = p_length;
+	configure(Rect3(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 +204,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 +216,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 {
 
-	real_t d = p_normal.dot( p_transform.origin );
+	real_t d = p_normal.dot(p_transform.origin);
 
 	// figure out scale at point
 	Vector3 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;
 }
 
-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(real_t p_mass) const {
 
 	real_t 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(Rect3( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));
-
+	radius = p_radius;
+	configure(Rect3(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 +268,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);
 
 	real_t length = local_normal.abs().dot(half_extents);
-	real_t distance = p_normal.dot( p_transform.origin );
+	real_t 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;
-		real_t dot = p_normal.dot( axis );
-		if ( Math::abs( dot ) >  _FACE_IS_VALID_SUPPORT_TRESHOLD ) {
+		axis[i] = 1.0;
+		real_t 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 real_t sign[4][2]={
+			static const real_t 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 +374,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 {
-
-	Rect3 aabb(-half_extents,half_extents*2.0);
+bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
 
-	return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);
+	Rect3 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(real_t p_mass) const {
 
-	real_t lx=half_extents.x;
-	real_t ly=half_extents.y;
-	real_t 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) );
+	real_t lx = half_extents.x;
+	real_t ly = half_extents.y;
+	real_t 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(Rect3(-half_extents,half_extents*2));
+void BoxShapeSW::_setup(const Vector3 &p_half_extents) {
 
+	half_extents = p_half_extents.abs();
 
+	configure(Rect3(-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 +415,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();
 	real_t 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);
 
-	real_t distance = p_normal.dot( p_transform.origin );
+	real_t distance = p_normal.dot(p_transform.origin);
 	real_t 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;
 
 	real_t 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;
 
 	real_t 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 {
 
 		real_t 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();
+	real_t min_d = 1e20;
 
-	Vector3 norm=(p_end-p_begin).normalized();
-	real_t 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) {
-		real_t d=norm.dot(auxres);
-		if (d<min_d) {
-			min_d=d;
-			res=auxres;
-			n=auxn;
-			collision=true;
+		real_t 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) {
-		real_t d=norm.dot(auxres);
-		if (d<min_d) {
-			min_d=d;
-			res=auxres;
-			n=auxn;
-			collision=true;
+		real_t 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) {
-		real_t d=norm.dot(auxres);
+		real_t 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 +544,90 @@ bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_e
 Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t 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(Rect3(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));
-
+	height = p_height;
+	radius = p_radius;
+	configure(Rect3(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++) {
 
-		real_t d=p_normal.dot( p_transform.xform( vrts[i] ) );
+		real_t 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;
 	real_t 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++) {
 
-		real_t d=n.dot(vrts[i]);
+		real_t 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 +642,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++) {
 
-		real_t d=p_normal.dot(vertices[i]);
+		real_t 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 +670,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++) {
 
+		real_t 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)) {
 
-		real_t 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;
 	real_t 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)) {
 				real_t 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(real_t 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);
 	Rect3 _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 +774,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]);
+		real_t d = p_normal.dot(v);
 
-		Vector3 v=p_transform.xform(vertex[i]);
-		real_t 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;
 	real_t support_max;
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		real_t d=p_normal.dot(vertex[i]);
+		real_t 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;
 	real_t support_max;
 
-	for (int i=0;i<3;i++) {
+	for (int i = 0; i < 3; i++) {
 
-		real_t d=n.dot(vertex[i]);
+		real_t 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
-		real_t dot=(vertex[i]-vertex[nx]).normalized().dot(n);
-		dot=ABS(dot);
+		// check if edge is valid as a support
+		real_t 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,183 +882,162 @@ bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,
 Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
 	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
-
 }
 
-FaceShapeSW::FaceShapeSW()  {
+FaceShapeSW::FaceShapeSW() {
 
 	configure(Rect3());
-
 }
 
-
-
 PoolVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
 
-
 	PoolVector<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;
 	}
-	PoolVector<Vector3>::Read r=vertices.read();
-	const Vector3 *vptr=r.ptr();
+	PoolVector<Vector3>::Read r = vertices.read();
+	const Vector3 *vptr = r.ptr();
 
-	for (int i=0;i<count;i++) {
+	for (int i = 0; i < count; i++) {
 
-		real_t 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;
+		real_t 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();
 
-	PoolVector<Vector3>::Read r=vertices.read();
-	const Vector3 *vptr=r.ptr();
+	PoolVector<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;
 	real_t support_max;
 
-	for (int i=0;i<count;i++) {
+	for (int i = 0; i < count; i++) {
 
-		real_t d=n.dot(vptr[i]);
+		real_t 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)) {
-
-
-			real_t d=p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
+					p_params->from,
+					p_params->to,
+					vertices[0],
+					vertices[1],
+					vertices[2],
+					&res)) {
+
+			real_t 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
-	PoolVector<Face>::Read fr=faces.read();
-	PoolVector<Vector3>::Read vr=vertices.read();
-	PoolVector<BVH>::Read br=bvh.read();
-
+	PoolVector<Face>::Read fr = faces.read();
+	PoolVector<Vector3>::Read vr = vertices.read();
+	PoolVector<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 {
 
@@ -1144,81 +1045,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 Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const {
+void ConcavePolygonShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const {
 
 	// make matrix local to concave
-	if (faces.size()==0)
+	if (faces.size() == 0)
 		return;
 
-	Rect3 local_aabb=p_local_aabb;
+	Rect3 local_aabb = p_local_aabb;
 
 	// unlock data
-	PoolVector<Face>::Read fr=faces.read();
-	PoolVector<Vector3>::Read vr=vertices.read();
-	PoolVector<BVH>::Read br=bvh.read();
+	PoolVector<Face>::Read fr = faces.read();
+	PoolVector<Vector3>::Read vr = vertices.read();
+	PoolVector<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(real_t 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 {
 
 	Rect3 aabb;
@@ -1228,26 +1124,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;
 	}
 };
 
@@ -1260,107 +1155,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;
 	}
 
 	Rect3 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);
 	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;
-
+	int idx = p_idx;
 
-	p_bvh_array[idx].aabb=p_bvh_tree->aabb;
-	p_bvh_array[idx].face_index=p_bvh_tree->face_index;
+	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(PoolVector<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(Rect3());
 		return;
 	}
-	ERR_FAIL_COND(src_face_count%3);
-	src_face_count/=3;
+	ERR_FAIL_COND(src_face_count % 3);
+	src_face_count /= 3;
 
 	PoolVector<Vector3>::Read r = p_faces.read();
-	const Vector3 * facesr= r.ptr();
+	const Vector3 *facesr = r.ptr();
 
 #if 0
 	Map<Vector3,int> point_map;
@@ -1476,67 +1366,62 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) {
 
 #else
 	PoolVector<_VolumeSW_BVH_Element> bvh_array;
-	bvh_array.resize( src_face_count );
+	bvh_array.resize(src_face_count);
 
 	PoolVector<_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);
 	PoolVector<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);
 
 	PoolVector<Vector3>::Write vw = vertices.write();
-	Vector3 *verticesw=vw.ptr();
+	Vector3 *verticesw = vw.ptr();
 
 	Rect3 _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=PoolVector<Face>::Write();
-	vw=PoolVector<Vector3>::Write();
+	w = PoolVector<Face>::Write();
+	vw = PoolVector<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);
 
 	PoolVector<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);
 }
@@ -1547,12 +1432,8 @@ Variant ConcavePolygonShapeSW::get_data() const {
 }
 
 ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
-
-
 }
 
-
-
 /* HEIGHT MAP SHAPE */
 
 PoolVector<real_t> HeightMapShapeSW::get_heights() const {
@@ -1572,114 +1453,94 @@ real_t 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 Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const {
-
-
-
+void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const {
 }
 
-
 Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t 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(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {
 
-void HeightMapShapeSW::_setup(PoolVector<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;
-
-	PoolVector<real_t>::Read r = heights. read();
+	PoolVector<real_t>::Read r = heights.read();
 
 	Rect3 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++) {
 
-			real_t h = r[i*width+j];
+			real_t 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"];
-	real_t cell_size=d["cell_size"];
-	PoolVector<real_t> 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"];
+	real_t cell_size = d["cell_size"];
+	PoolVector<real_t> 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 55daa5856d..442cbc39eb 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"
@@ -46,16 +46,14 @@ SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_creat
 
 class ShapeSW;
 
-class ShapeOwnerSW :  public RID_Data {
+class ShapeOwnerSW : public RID_Data {
 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 : public RID_Data {
 
 	RID self;
@@ -63,60 +61,58 @@ class ShapeSW : public RID_Data {
 	bool configured;
 	real_t custom_bias;
 
-	Map<ShapeOwnerSW*,int> owners;
+	Map<ShapeOwnerSW *, int> owners;
+
 protected:
+	void configure(const Rect3 &p_aabb);
 
-	void configure(const Rect3& p_aabb);
 public:
-
 	enum {
-		MAX_SUPPORTS=8
+		MAX_SUPPORTS = 8
 	};
 
-	virtual real_t get_area() const { return aabb.get_area();}
+	virtual real_t get_area() const { return aabb.get_area(); }
 
-	_FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
-	_FORCE_INLINE_ RID get_self() const {return  self; }
+	_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_ Rect3 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(real_t 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(real_t 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 Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
 
 	ConcaveShapeSW() {}
 };
@@ -125,22 +121,22 @@ 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 real_t get_area() const { return Math_INF; }
 	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(real_t 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();
@@ -151,21 +147,21 @@ class RayShapeSW : public ShapeSW {
 	real_t length;
 
 	void _setup(real_t p_length);
-public:
 
+public:
 	real_t get_length() const;
 
 	virtual real_t get_area() const { return 0.0; }
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	virtual Vector3 get_support(const Vector3& p_normal) const;
-	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(real_t 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();
@@ -176,22 +172,22 @@ class SphereShapeSW : public ShapeSW {
 	real_t radius;
 
 	void _setup(real_t p_radius);
-public:
 
+public:
 	real_t get_radius() const;
 
-	virtual real_t get_area() const { return 4.0/3.0 * Math_PI * radius * radius * radius; }
+	virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius; }
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	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(real_t 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();
@@ -200,22 +196,22 @@ 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 real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; } 
+	virtual real_t get_area() const { return 8 * half_extents.x * half_extents.y * half_extents.z; }
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
 
-	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(real_t 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();
@@ -226,25 +222,24 @@ 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 real_t get_area() { return 4.0/3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
+	virtual real_t get_area() { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
 
 	virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
 
-	virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
-	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(real_t 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();
@@ -254,28 +249,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(real_t 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;
 
@@ -326,39 +319,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(PoolVector<Vector3> p_faces);
-public:
 
+public:
 	PoolVector<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 Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const;
+	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t 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 {
 
 	PoolVector<real_t> heights;
@@ -369,9 +358,9 @@ struct HeightMapShapeSW : public ConcaveShapeSW {
 	//void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
 	//void _cull(int p_idx,_CullParams *p_params) const;
 
-	void _setup(PoolVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size);
-public:
+	void _setup(PoolVector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size);
 
+public:
 	PoolVector<real_t> get_heights() const;
 	int get_width() const;
 	int get_depth() const;
@@ -379,19 +368,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 Rect3& p_local_aabb,Callback p_callback,void* p_userdata) const;
+	virtual void cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const;
 
 	virtual Vector3 get_moment_of_inertia(real_t 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
@@ -402,22 +390,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(real_t 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;
@@ -425,56 +412,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(real_t 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(Rect3()); }
+	MotionShapeSW() { configure(Rect3()); }
 };
 
-
-
-
 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 0bc11041de..603c6fa3c4 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 "global_config.h"
 #include "space_sw.h"
 #include "collision_solver_sw.h"
+#include "global_config.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,293 +85,268 @@ 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, real_t 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,real_t 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);
 
 	Rect3 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, real_t p_margin, real_t &p_closest_safe, real_t &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,real_t p_margin,real_t &p_closest_safe,real_t &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);
 
 	Rect3 aabb = p_xform.xform(shape->get_aabb());
-	aabb=aabb.merge(Rect3(aabb.pos+p_motion,aabb.size)); //motion
-	aabb=aabb.grow(p_margin);
+	aabb = aabb.merge(Rect3(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);
 
-	real_t best_safe=1;
-	real_t best_unsafe=1;
+	real_t best_safe = 1;
+	real_t 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
-		real_t low=0;
-		real_t hi=1;
-		Vector3 mnormal=p_motion.normalized();
+		real_t low = 0;
+		real_t 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..
 
-			real_t ofs = (low+hi)*0.5;
+			real_t 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,real_t 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, real_t 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);
 
 	Rect3 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;
-
-	PhysicsServerSW::CollCbkData *cbkptr=NULL;
-	if (p_result_max>0) {
-		cbkptr=&cbk;
-		cbkres=PhysicsServerSW::_shape_col_cbk;
+	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;
 	}
 
+	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;
@@ -391,173 +358,147 @@ struct _RestCallbackData {
 	real_t 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;
 	real_t 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,real_t 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, real_t 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);
 
 	Rect3 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);
 }
@@ -569,112 +510,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;
@@ -690,12 +622,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 {
@@ -710,41 +642,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/3d/sleep_threshold_linear",0.1);
-	body_angular_velocity_sleep_threshold=GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI) );
-	body_time_to_sleep=GLOBAL_DEF("physics/3d/time_before_sleep",0.5);
-	body_angular_velocity_damp_ratio=10;
-
+	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);
+	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI));
+	body_time_to_sleep = GLOBAL_DEF("physics/3d/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 208831914f..06538265bb 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 "global_config.h"
-
+#include "hash_map.h"
+#include "typedefs.h"
 
 class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
 
-	GDCLASS( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState );
-public:
+	GDCLASS(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,real_t 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,real_t p_margin,real_t &p_closest_safe,real_t &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,real_t 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,real_t 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, real_t 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, real_t p_margin, real_t &p_closest_safe, real_t &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, real_t 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, real_t 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 RID_Data {
 
 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 0bd5a874ea..c7b1be7a9b 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,real_t p_delta) {
+void StepSW::_setup_island(ConstraintSW *p_island, real_t 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,real_t p_delta){
+void StepSW::_solve_island(ConstraintSW *p_island, int p_iterations, real_t 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,real_t p_delta) {
-
+void StepSW::_check_suspend(BodySW *p_island, real_t 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,real_t p_delta,int p_iterations) {
+void StepSW::step(SpaceSW *p_space, real_t 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));
 	/* 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 7048a76937..54f5fe9857 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,real_t p_delta);
-	void _solve_island(ConstraintSW *p_island,int p_iterations,real_t p_delta);
-	void _check_suspend(BodySW *p_island,real_t p_delta);
-public:
+	void _populate_island(BodySW *p_body, BodySW **p_island, ConstraintSW **p_constraint_island);
+	void _setup_island(ConstraintSW *p_island, real_t p_delta);
+	void _solve_island(ConstraintSW *p_island, int p_iterations, real_t p_delta);
+	void _check_suspend(BodySW *p_island, real_t p_delta);
 
-	void step(SpaceSW* p_space,real_t p_delta,int p_iterations);
+public:
+	void step(SpaceSW *p_space, real_t p_delta, int p_iterations);
 	StepSW();
 };
author	Rémi Verschelde <rverschelde@gmail.com>	2017-03-05 16:44:50 +0100
committer	Rémi Verschelde <rverschelde@gmail.com>	2017-03-05 16:44:50 +0100
commit	5dbf1809c6e3e905b94b8764e99491e608122261 (patch)
tree	5e5a5360db15d86d59ec8c6e4f7eb511388c5a9a /servers/physics
parent	45438e9918d421b244bfd7776a30e67dc7f2d3e3 (diff)