1 files changed, 147 insertions, 182 deletions

diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index c05d61b65..ee94a7ace 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -30,54 +30,52 @@
 #include "collision_solver_2d_sw.h"
 #include "space_2d_sw.h"
 
-
 #define POSITION_CORRECTION
 #define ACCUMULATE_IMPULSES
 
-void BodyPair2DSW::_add_contact(const Vector2& p_point_A,const Vector2& p_point_B,void *p_self) {
+void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
 
 	BodyPair2DSW *self = (BodyPair2DSW *)p_self;
 
-	self->_contact_added_callback(p_point_A,p_point_B);
-
+	self->_contact_added_callback(p_point_A, p_point_B);
 }
 
-void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector2& p_point_B) {
+void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
 
 	// check if we already have the contact
 
 	Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
-	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B-offset_B);
+	Vector2 local_B = B->get_inv_transform().basis_xform(p_point_B - offset_B);
 
 	int new_index = contact_count;
 
-	ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) );
+	ERR_FAIL_COND(new_index >= (MAX_CONTACTS + 1));
 
 	Contact contact;
 
-	contact.acc_normal_impulse=0;
-	contact.acc_bias_impulse=0;
-	contact.acc_tangent_impulse=0;
-	contact.local_A=local_A;
-	contact.local_B=local_B;
-	contact.reused=true;
-	contact.normal=(p_point_A-p_point_B).normalized();
+	contact.acc_normal_impulse = 0;
+	contact.acc_bias_impulse = 0;
+	contact.acc_tangent_impulse = 0;
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.reused = true;
+	contact.normal = (p_point_A - p_point_B).normalized();
 
 	// attempt to determine if the contact will be reused
 
 	real_t recycle_radius_2 = space->get_contact_recycle_radius() * space->get_contact_recycle_radius();
 
-	for (int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 		if (
-		c.local_A.distance_squared_to( local_A ) < (recycle_radius_2) &&
-		c.local_B.distance_squared_to( local_B ) < (recycle_radius_2) ) {
+				c.local_A.distance_squared_to(local_A) < (recycle_radius_2) &&
+				c.local_B.distance_squared_to(local_B) < (recycle_radius_2)) {
 
-			contact.acc_normal_impulse=c.acc_normal_impulse;
-			contact.acc_tangent_impulse=c.acc_tangent_impulse;
-			contact.acc_bias_impulse=c.acc_bias_impulse;
-			new_index=i;
+			contact.acc_normal_impulse = c.acc_normal_impulse;
+			contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			contact.acc_bias_impulse = c.acc_bias_impulse;
+			new_index = i;
 			break;
 		}
 	}
@@ -88,44 +86,41 @@ void BodyPair2DSW::_contact_added_callback(const Vector2& p_point_A,const Vector
 
 		// remove the contact with the minimum depth
 
-		int least_deep=-1;
-		real_t min_depth=1e10;
+		int least_deep = -1;
+		real_t min_depth = 1e10;
 
+		for (int i = 0; i <= contact_count; i++) {
 
-		for (int i=0;i<=contact_count;i++) {
-
-			Contact& c = (i==contact_count)?contact:contacts[i];
+			Contact &c = (i == contact_count) ? contact : contacts[i];
 			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
 
 			Vector2 axis = global_A - global_B;
-			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 BodyPair2DSW::_validate_contacts() {
@@ -133,39 +128,35 @@ void BodyPair2DSW::_validate_contacts() {
 	//make sure to erase contacts that are no longer valid
 
 	real_t max_separation = space->get_contact_max_separation();
-	real_t max_separation2 = max_separation*max_separation;
+	real_t max_separation2 = max_separation * max_separation;
 
-	for (int i=0;i<contact_count;i++) {
+	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 
-		bool erase=false;
-		if (c.reused==false) {
+		bool erase = false;
+		if (c.reused == false) {
 			//was left behind in previous frame
-			erase=true;
+			erase = true;
 		} else {
-			c.reused=false;
+			c.reused = false;
 
 			Vector2 global_A = A->get_transform().basis_xform(c.local_A);
-			Vector2 global_B = B->get_transform().basis_xform(c.local_B)+offset_B;
+			Vector2 global_B = B->get_transform().basis_xform(c.local_B) + offset_B;
 			Vector2 axis = global_A - global_B;
-			real_t depth = axis.dot( c.normal );
-
-
+			real_t depth = axis.dot(c.normal);
 
 			if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
-				erase=true;
+				erase = true;
 			}
 		}
 
 		if (erase) {
 			// contact no longer needed, remove
 
-
-			if ((i+1) < contact_count) {
+			if ((i + 1) < contact_count) {
 				// swap with the last one
-				SWAP( contacts[i], contacts[ contact_count-1 ] );
-
+				SWAP(contacts[i], contacts[contact_count - 1]);
 			}
 
 			i--;
@@ -174,21 +165,18 @@ void BodyPair2DSW::_validate_contacts() {
 	}
 }
 
+bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) {
 
-bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Transform2D& p_xform_A,Body2DSW *p_B, int p_shape_B,const Transform2D& p_xform_B,bool p_swap_result) {
-
-
-
-	Vector2 motion = p_A->get_linear_velocity()*p_step;
+	Vector2 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
-	if (mlen<CMP_EPSILON)
+	if (mlen < CMP_EPSILON)
 		return false;
 
 	Vector2 mnormal = motion / mlen;
 
-	real_t min,max;
-	p_A->get_shape(p_shape_A)->project_rangev(mnormal,p_xform_A,min,max);
-	bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+	real_t min, max;
+	p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
+	bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
 
 	if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
 		return false;
@@ -198,22 +186,21 @@ bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Tr
 	//support is the worst case collision point, so real collision happened before
 	int a;
 	Vector2 s[2];
-	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
+	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
 	Vector2 from = p_xform_A.xform(s[0]);
 	Vector2 to = from + motion;
 
 	Transform2D from_inv = p_xform_B.affine_inverse();
 
-	Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+	Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
 	Vector2 local_to = from_inv.xform(to);
 
-	Vector2 rpos,rnorm;
-	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm))
+	Vector2 rpos, rnorm;
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm))
 		return false;
 
 	//ray hit something
 
-
 	Vector2 hitpos = p_xform_B.xform(rpos);
 
 	Vector2 contact_A = to;
@@ -222,20 +209,18 @@ bool BodyPair2DSW::_test_ccd(real_t p_step,Body2DSW *p_A, int p_shape_A,const Tr
 	//create a contact
 
 	if (p_swap_result)
-		_contact_added_callback(contact_B,contact_A);
+		_contact_added_callback(contact_B, contact_A);
 	else
-		_contact_added_callback(contact_A,contact_B);
-
+		_contact_added_callback(contact_A, contact_B);
 
 	return true;
 }
 
 bool BodyPair2DSW::setup(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()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
-		collided=false;
+	if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+		collided = false;
 		return false;
 	}
 
@@ -249,44 +234,43 @@ bool BodyPair2DSW::setup(real_t p_step) {
 	Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
 
 	Transform2D xform_Bu = B->get_transform();
-	xform_Bu.elements[2]-=A->get_transform().get_origin();
+	xform_Bu.elements[2] -= A->get_transform().get_origin();
 	Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
-	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
-	Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
+	Shape2DSW *shape_A_ptr = A->get_shape(shape_A);
+	Shape2DSW *shape_B_ptr = B->get_shape(shape_B);
 
-	Vector2 motion_A,motion_B;
+	Vector2 motion_A, motion_B;
 
-	if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
-		motion_A=A->get_motion();
+	if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_A = A->get_motion();
 	}
-	if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) {
-		motion_B=B->get_motion();
+	if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_SHAPE) {
+		motion_B = B->get_motion();
 	}
 	//faster to set than to check..
 
 	//bool prev_collided=collided;
 
-	collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis);
+	collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
 	if (!collided) {
 
 		//test ccd (currently just a raycast)
 
-		if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
-			if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B))
-				collided=true;
+		if (A->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B))
+				collided = true;
 		}
 
-		if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) {
-			if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true))
-				collided=true;
+		if (B->get_continuous_collision_detection_mode() == Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode() > Physics2DServer::BODY_MODE_KINEMATIC) {
+			if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true))
+				collided = true;
 		}
 
 		if (!collided) {
-			oneway_disabled=false;
+			oneway_disabled = false;
 			return false;
 		}
-
 	}
 
 	if (oneway_disabled)
@@ -297,45 +281,45 @@ bool BodyPair2DSW::setup(real_t p_step) {
 
 		if (A->is_using_one_way_collision()) {
 			Vector2 direction = A->get_one_way_collision_direction();
-			bool valid=false;
-			if (B->get_linear_velocity().dot(direction)>=0){
-				for(int i=0;i<contact_count;i++) {
-					Contact& c = contacts[i];
+			bool valid = false;
+			if (B->get_linear_velocity().dot(direction) >= 0) {
+				for (int i = 0; i < contact_count; i++) {
+					Contact &c = contacts[i];
 					if (!c.reused)
 						continue;
-					if (c.normal.dot(direction)<0)
+					if (c.normal.dot(direction) < 0)
 						continue;
 
-					valid=true;
+					valid = true;
 					break;
 				}
 			}
 
 			if (!valid) {
-				collided=false;
-				oneway_disabled=true;
+				collided = false;
+				oneway_disabled = true;
 				return false;
 			}
 		}
 
 		if (B->is_using_one_way_collision()) {
 			Vector2 direction = B->get_one_way_collision_direction();
-			bool valid=false;
-			if (A->get_linear_velocity().dot(direction)>=0){
-				for(int i=0;i<contact_count;i++) {
-					Contact& c = contacts[i];
+			bool valid = false;
+			if (A->get_linear_velocity().dot(direction) >= 0) {
+				for (int i = 0; i < contact_count; i++) {
+					Contact &c = contacts[i];
 					if (!c.reused)
 						continue;
-					if (c.normal.dot(direction)<0)
+					if (c.normal.dot(direction) < 0)
 						continue;
 
-					valid=true;
+					valid = true;
 					break;
 				}
 			}
 			if (!valid) {
-				collided=false;
-				oneway_disabled=true;
+				collided = false;
+				oneway_disabled = true;
 				return false;
 			}
 		}
@@ -346,72 +330,69 @@ bool BodyPair2DSW::setup(real_t p_step) {
 	real_t bias = 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;
 	}
 
+	cc = 0;
 
-	cc=0;
-
+	real_t inv_dt = 1.0 / p_step;
 
-	real_t inv_dt = 1.0/p_step;
-
-	bool do_process=false;
+	bool do_process = false;
 
 	for (int i = 0; i < contact_count; i++) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 
 		Vector2 global_A = xform_Au.xform(c.local_A);
 		Vector2 global_B = xform_Bu.xform(c.local_B);
 
 		real_t depth = c.normal.dot(global_A - global_B);
 
-		if (depth<=0 || !c.reused) {
-			c.active=false;
+		if (depth <= 0 || !c.reused) {
+			c.active = false;
 			continue;
 		}
 
-		c.active=true;
+		c.active = true;
 #ifdef DEBUG_ENABLED
 		if (space->is_debugging_contacts()) {
-			space->add_debug_contact(global_A+offset_A);
-			space->add_debug_contact(global_B+offset_A);
+			space->add_debug_contact(global_A + offset_A);
+			space->add_debug_contact(global_B + offset_A);
 		}
 #endif
 		int gather_A = A->can_report_contacts();
 		int gather_B = B->can_report_contacts();
 
 		c.rA = global_A;
-		c.rB = global_B-offset_B;
+		c.rB = global_B - offset_B;
 
 		if (gather_A | gather_B) {
 
 			//Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
 
-			global_A+=offset_A;
-			global_B+=offset_A;
+			global_A += offset_A;
+			global_B += offset_A;
 
 			if (gather_A) {
-				Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
-				A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity());
+				Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
+				A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
 			}
 			if (gather_B) {
 
-				Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-				B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity());
+				Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+				B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
 			}
 		}
 
-		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) {
-			c.active=false;
-			collided=false;
+		if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC)) {
+			c.active = false;
+			collided = false;
 			continue;
-
 		}
 
 		// Precompute normal mass, tangent mass, and bias.
@@ -426,39 +407,33 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		real_t rtB = c.rB.dot(tangent);
 		real_t kTangent = A->get_inv_mass() + B->get_inv_mass();
 		kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB);
-		c.mass_tangent = 1.0f /  kTangent;
-
-
+		c.mass_tangent = 1.0f / kTangent;
 
 		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
-		c.depth=depth;
-		//c.acc_bias_impulse=0;
-
+		c.depth = depth;
+//c.acc_bias_impulse=0;
 
 #ifdef ACCUMULATE_IMPULSES
 		{
 			// Apply normal + friction impulse
 			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
 
-
-			A->apply_impulse(c.rA,-P);
+			A->apply_impulse(c.rA, -P);
 			B->apply_impulse(c.rB, P);
 		}
 
 #endif
 
-
-		c.bounce=MAX(A->get_bounce(),B->get_bounce());
+		c.bounce = MAX(A->get_bounce(), B->get_bounce());
 		if (c.bounce) {
 
-			Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-			Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
 			Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 			c.bounce = c.bounce * dv.dot(c.normal);
 		}
 
-		do_process=true;
-
+		do_process = true;
 	}
 
 	return do_process;
@@ -471,81 +446,71 @@ void BodyPair2DSW::solve(real_t p_step) {
 
 	for (int i = 0; i < contact_count; ++i) {
 
-		Contact& c = contacts[i];
+		Contact &c = contacts[i];
 		cc++;
 
 		if (!c.active)
 			continue;
 
-
 		// Relative velocity at contact
 
-		Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x );
-		Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x );
+		Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+		Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
 		Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
 
-		Vector2 crbA( -A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x );
-		Vector2 crbB( -B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x );
+		Vector2 crbA(-A->get_biased_angular_velocity() * c.rA.y, A->get_biased_angular_velocity() * c.rA.x);
+		Vector2 crbB(-B->get_biased_angular_velocity() * c.rB.y, B->get_biased_angular_velocity() * c.rB.x);
 		Vector2 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA;
 
-
 		real_t vn = dv.dot(c.normal);
 		real_t vbn = dbv.dot(c.normal);
 		Vector2 tangent = c.normal.tangent();
 		real_t vt = dv.dot(tangent);
 
-
-		real_t jbn = (c.bias - vbn)*c.mass_normal;
+		real_t jbn = (c.bias - vbn) * c.mass_normal;
 		real_t jbnOld = c.acc_bias_impulse;
 		c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
 
 		Vector2 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-		A->apply_bias_impulse(c.rA,-jb);
+		A->apply_bias_impulse(c.rA, -jb);
 		B->apply_bias_impulse(c.rB, jb);
 
-		real_t jn = -(c.bounce + vn)*c.mass_normal;
+		real_t jn = -(c.bounce + vn) * c.mass_normal;
 		real_t jnOld = c.acc_normal_impulse;
 		c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
 
-
 		real_t friction = A->get_friction() * B->get_friction();
 
-		real_t jtMax = friction*c.acc_normal_impulse;
-		real_t jt = -vt*c.mass_tangent;
+		real_t jtMax = friction * c.acc_normal_impulse;
+		real_t jt = -vt * c.mass_tangent;
 		real_t jtOld = c.acc_tangent_impulse;
 		c.acc_tangent_impulse = CLAMP(jtOld + jt, -jtMax, jtMax);
 
-		Vector2 j =c.normal * (c.acc_normal_impulse - jnOld) + tangent * ( c.acc_tangent_impulse - jtOld );
+		Vector2 j = c.normal * (c.acc_normal_impulse - jnOld) + tangent * (c.acc_tangent_impulse - jtOld);
 
-		A->apply_impulse(c.rA,-j);
+		A->apply_impulse(c.rA, -j);
 		B->apply_impulse(c.rB, j);
-
-
 	}
 }
 
+BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B)
+	: Constraint2DSW(_arr, 2) {
 
-BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A,Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr,2) {
-
-	A=p_A;
-	B=p_B;
-	shape_A=p_shape_A;
-	shape_B=p_shape_B;
-	space=A->get_space();
-	A->add_constraint(this,0);
-	B->add_constraint(this,1);
-	contact_count=0;
-	collided=false;
-	oneway_disabled=false;
-
+	A = p_A;
+	B = p_B;
+	shape_A = p_shape_A;
+	shape_B = p_shape_B;
+	space = A->get_space();
+	A->add_constraint(this, 0);
+	B->add_constraint(this, 1);
+	contact_count = 0;
+	collided = false;
+	oneway_disabled = false;
 }
 
-
 BodyPair2DSW::~BodyPair2DSW() {
 
-
 	A->remove_constraint(this);
 	B->remove_constraint(this);
-
 }