text: /*************************************************************************/
  /*  body_pair_2d_sw.cpp                                                  */
  /*************************************************************************/
  /*                       This file is part of:                           */
  /*                           GODOT ENGINE                                */
  /*                    http://www.godotengine.org                         */
  /*************************************************************************/
  /* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                 */
  /*                                                                       */
  /* Permission is hereby granted, free of charge, to any person obtaining */
  /* a copy of this software and associated documentation files (the       */
  /* "Software"), to deal in the Software without restriction, including   */
  /* without limitation the rights to use, copy, modify, merge, publish,   */
  /* distribute, sublicense, and/or sell copies of the Software, and to    */
  /* permit persons to whom the Software is furnished to do so, subject to */
  /* the following conditions:                                             */
  /*                                                                       */
  /* The above copyright notice and this permission notice shall be        */
  /* included in all copies or substantial portions of the Software.       */
  /*                                                                       */
  /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
  /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
  /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
  /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
  /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
  /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
  /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
  /*************************************************************************/
  #include "body_pair_2d_sw.h"
  #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) {
  
  	BodyPair2DSW *self = (BodyPair2DSW *)p_self;
  
  	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) {
  
  	// 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);
  
  	int new_index = contact_count;
  
  	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();
  
  	// 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++) {
  
  		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) ) {
  
  			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;
  		}
  	}
  
  	// figure out if the contact amount must be reduced to fit the new contact
  
  	if (new_index == MAX_CONTACTS) {
  
  		// remove the contact with the minimum depth
  
  		int least_deep=-1;
  		real_t min_depth=1e10;
  
  
  		for (int i=0;i<=contact_count;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 axis = global_A - global_B;
  			float depth = axis.dot( c.normal );
  
  
  			if (depth<min_depth) {
  
  				min_depth=depth;
  				least_deep=i;
  			}
  		}
  
  		ERR_FAIL_COND(least_deep==-1);
  
  		if (least_deep < contact_count) { //replace the last deep contact by the new one
  
  			contacts[least_deep]=contact;
  		}
  
  		return;
  	}
  
  	contacts[new_index]=contact;
  
  	if (new_index==contact_count) {
  
  		contact_count++;
  	}
  
  }
  
  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;
  
  	for (int i=0;i<contact_count;i++) {
  
  		Contact& c = contacts[i];
  
  		bool erase=false;
  		if (c.reused==false) {
  			//was left behind in previous frame
  			erase=true;
  		} else {
  			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 axis = global_A - global_B;
  			float depth = axis.dot( c.normal );
  
  
  
  			if (depth < -max_separation || (global_B + c.normal * depth - global_A).length_squared() > max_separation2) {
  				erase=true;
  			}
  		}
  
  		if (erase) {
  			// contact no longer needed, remove
  
  
  			if ((i+1) < contact_count) {
  				// swap with the last one
  				SWAP( contacts[i], contacts[ contact_count-1 ] );
  
  			}
  
  			i--;
  			contact_count--;
  		}
  	}
  }
  
  
  bool BodyPair2DSW::_test_ccd(float p_step,Body2DSW *p_A, int p_shape_A,const Matrix32& p_xform_A,Body2DSW *p_B, int p_shape_B,const Matrix32& p_xform_B,bool p_swap_result) {
  
  
  
  	Vector2 motion = p_A->get_linear_velocity()*p_step;
  	real_t mlen = motion.length();
  	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
  
  	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;
  	}
  
  	//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
  	int a;
  	Vector2 s[2];
  	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(),s,a);
  	Vector2 from = p_xform_A.xform(s[0]);
  	Vector2 to = from + motion;
  
  	Matrix32 from_inv = p_xform_B.affine_inverse();
  
  	Vector2 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
  	Vector2 local_to = from_inv.xform(to);
  
  	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;
  	Vector2 contact_B = hitpos;
  
  	//create a contact
  
  	if (p_swap_result)
  		_contact_added_callback(contact_B,contact_A);
  	else
  		_contact_added_callback(contact_A,contact_B);
  
  
  	return true;
  }
  
  bool BodyPair2DSW::setup(float p_step) {
  
  
  	//cannot collide
  	if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_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)) {
  		collided=false;
  		return false;
  	}
  
  	//use local A coordinates to avoid numerical issues on collision detection
  	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
  
  	_validate_contacts();
  
  	Vector2 offset_A = A->get_transform().get_origin();
  	Matrix32 xform_Au = A->get_transform().untranslated();
  	Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A);
  
  	Matrix32 xform_Bu = B->get_transform();
  	xform_Bu.elements[2]-=A->get_transform().get_origin();
  	Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B);
  
  	Shape2DSW *shape_A_ptr=A->get_shape(shape_A);
  	Shape2DSW *shape_B_ptr=B->get_shape(shape_B);
  
  	Vector2 motion_A,motion_B;
  
  	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();
  	} 
  	//faster to set than to check..
  
  	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 (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)
  			return false;
  
  	}
  
  	real_t max_penetration = space->get_contact_max_allowed_penetration();
  
  	float bias = 0.3f;
  	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
  
  		if (shape_A_ptr->get_custom_bias()==0)
  			bias=shape_B_ptr->get_custom_bias();
  		else if (shape_B_ptr->get_custom_bias()==0)
  			bias=shape_A_ptr->get_custom_bias();
  		else
  			bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5;
  	}
  
  
  	cc=0;
  
  
  	real_t inv_dt = 1.0/p_step;
  	for (int i = 0; i < contact_count; 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;
  			continue;
  		}
  
  		c.active=true;
  
  		int gather_A = A->can_report_contacts();
  		int gather_B = B->can_report_contacts();
  
  		c.rA = global_A;
  		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;
  
  			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());
  			}
  			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());
  			}
  		}
  
  		// Precompute normal mass, tangent mass, and bias.
  		real_t rnA = c.rA.dot(c.normal);
  		real_t rnB = c.rB.dot(c.normal);
  		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
  		kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB);
  		c.mass_normal = 1.0f / kNormal;
  
  		Vector2 tangent = c.normal.tangent();
  		real_t rtA = c.rA.dot(tangent);
  		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.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
  		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);
  			B->apply_impulse(c.rB, P);
  		}
  
  #endif
  	}
  
  	return true;
  }
  
  void BodyPair2DSW::solve(float p_step) {
  
  	if (!collided)
  		return;
  
  	for (int i = 0; i < contact_count; ++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 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 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 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);
  		B->apply_bias_impulse(c.rB, jb);
  
  		real_t bounce=MAX(A->get_bounce(),B->get_bounce());
  		real_t jn = -(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 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 );
  
  		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) {
  
  	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;
  
  }
  
  
  BodyPair2DSW::~BodyPair2DSW() {
  
  	A->remove_constraint(this);
  	B->remove_constraint(this);
  
  }
  