text: /*
  * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
  *
  * This software is provided 'as-is', without any express or implied
  * warranty.  In no event will the authors be held liable for any damages
  * arising from the use of this software.
  * Permission is granted to anyone to use this software for any purpose,
  * including commercial applications, and to alter it and redistribute it
  * freely, subject to the following restrictions:
  * 1. The origin of this software must not be misrepresented; you must not
  * claim that you wrote the original software. If you use this software
  * in a product, an acknowledgment in the product documentation would be
  * appreciated but is not required.
  * 2. Altered source versions must be plainly marked as such, and must not be
  * misrepresented as being the original software.
  * 3. This notice may not be removed or altered from any source distribution.
  */
  
  #include "b2ContactSolver.h"
  #include "b2Contact.h"
  #include "../b2Body.h"
  #include "../b2World.h"
  #include "../../Common/b2StackAllocator.h"
  
  b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
  {
  	m_step = step;
  	m_allocator = allocator;
  
  	m_constraintCount = 0;
  	for (int32 i = 0; i < contactCount; ++i)
  	{
  		b2Assert(contacts[i]->IsSolid());
  		m_constraintCount += contacts[i]->GetManifoldCount();
  	}
  
  	m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
  
  	int32 count = 0;
  	for (int32 i = 0; i < contactCount; ++i)
  	{
  		b2Contact* contact = contacts[i];
  
  		b2Body* b1 = contact->m_shape1->GetBody();
  		b2Body* b2 = contact->m_shape2->GetBody();
  		int32 manifoldCount = contact->GetManifoldCount();
  		b2Manifold* manifolds = contact->GetManifolds();
  		float32 friction = contact->m_friction;
  		float32 restitution = contact->m_restitution;
  
  		b2Vec2 v1 = b1->m_linearVelocity;
  		b2Vec2 v2 = b2->m_linearVelocity;
  		float32 w1 = b1->m_angularVelocity;
  		float32 w2 = b2->m_angularVelocity;
  
  		for (int32 j = 0; j < manifoldCount; ++j)
  		{
  			b2Manifold* manifold = manifolds + j;
  
  			b2Assert(manifold->pointCount > 0);
  
  			const b2Vec2 normal = manifold->normal;
  
  			b2Assert(count < m_constraintCount);
  			b2ContactConstraint* c = m_constraints + count;
  			c->body1 = b1;
  			c->body2 = b2;
  			c->manifold = manifold;
  			c->normal = normal;
  			c->pointCount = manifold->pointCount;
  			c->friction = friction;
  			c->restitution = restitution;
  
  			for (int32 k = 0; k < c->pointCount; ++k)
  			{
  				b2ManifoldPoint* cp = manifold->points + k;
  				b2ContactConstraintPoint* ccp = c->points + k;
  
  				ccp->normalImpulse = cp->normalImpulse;
  				ccp->tangentImpulse = cp->tangentImpulse;
  				ccp->separation = cp->separation;
  				ccp->positionImpulse = 0.0f;
  
  				ccp->localAnchor1 = cp->localPoint1;
  				ccp->localAnchor2 = cp->localPoint2;
  				ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
  				ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
  
  				float32 rn1 = b2Cross(ccp->r1, normal);
  				float32 rn2 = b2Cross(ccp->r2, normal);
  				rn1 *= rn1;
  				rn2 *= rn2;
  
  				float32 kNormal = b1->m_invMass + b2->m_invMass + b1->m_invI * rn1 + b2->m_invI * rn2;
  
  				b2Assert(kNormal > B2_FLT_EPSILON);
  				ccp->normalMass = 1.0f / kNormal;
  
  				float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
  				kEqualized += b1->m_mass * b1->m_invI * rn1 + b2->m_mass * b2->m_invI * rn2;
  
  				b2Assert(kEqualized > B2_FLT_EPSILON);
  				ccp->equalizedMass = 1.0f / kEqualized;
  
  				b2Vec2 tangent = b2Cross(normal, 1.0f);
  
  				float32 rt1 = b2Cross(ccp->r1, tangent);
  				float32 rt2 = b2Cross(ccp->r2, tangent);
  				rt1 *= rt1;
  				rt2 *= rt2;
  
  				float32 kTangent = b1->m_invMass + b2->m_invMass + b1->m_invI * rt1 + b2->m_invI * rt2;
  
  				b2Assert(kTangent > B2_FLT_EPSILON);
  				ccp->tangentMass = 1.0f /  kTangent;
  
  				// Setup a velocity bias for restitution.
  				ccp->velocityBias = 0.0f;
  				if (ccp->separation > 0.0f)
  				{
  					ccp->velocityBias = -60.0f * ccp->separation; // TODO_ERIN b2TimeStep
  				}
  
  				float32 vRel = b2Dot(c->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
  				if (vRel < -b2_velocityThreshold)
  				{
  					ccp->velocityBias += -c->restitution * vRel;
  				}
  			}
  
  			++count;
  		}
  	}
  
  	b2Assert(count == m_constraintCount);
  }
  
  b2ContactSolver::~b2ContactSolver()
  {
  	m_allocator->Free(m_constraints);
  }
  
  void b2ContactSolver::InitVelocityConstraints(const b2TimeStep& step)
  {
  	// Warm start.
  	for (int32 i = 0; i < m_constraintCount; ++i)
  	{
  		b2ContactConstraint* c = m_constraints + i;
  
  		b2Body* b1 = c->body1;
  		b2Body* b2 = c->body2;
  		float32 invMass1 = b1->m_invMass;
  		float32 invI1 = b1->m_invI;
  		float32 invMass2 = b2->m_invMass;
  		float32 invI2 = b2->m_invI;
  		b2Vec2 normal = c->normal;
  		b2Vec2 tangent = b2Cross(normal, 1.0f);
  
  		if (step.warmStarting)
  		{
  			for (int32 j = 0; j < c->pointCount; ++j)
  			{
  				b2ContactConstraintPoint* ccp = c->points + j;
  				ccp->normalImpulse *= step.dtRatio;
  				ccp->tangentImpulse *= step.dtRatio;
  				b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
  				b1->m_angularVelocity -= invI1 * b2Cross(ccp->r1, P);
  				b1->m_linearVelocity -= invMass1 * P;
  				b2->m_angularVelocity += invI2 * b2Cross(ccp->r2, P);
  				b2->m_linearVelocity += invMass2 * P;
  			}
  		}
  		else
  		{
  			for (int32 j = 0; j < c->pointCount; ++j)
  			{
  				b2ContactConstraintPoint* ccp = c->points + j;
  				ccp->normalImpulse = 0.0f;
  				ccp->tangentImpulse = 0.0f;
  			}
  		}
  	}
  }
  
  void b2ContactSolver::SolveVelocityConstraints()
  {
  	for (int32 i = 0; i < m_constraintCount; ++i)
  	{
  		b2ContactConstraint* c = m_constraints + i;
  		b2Body* b1 = c->body1;
  		b2Body* b2 = c->body2;
  		float32 w1 = b1->m_angularVelocity;
  		float32 w2 = b2->m_angularVelocity;
  		b2Vec2 v1 = b1->m_linearVelocity;
  		b2Vec2 v2 = b2->m_linearVelocity;
  		float32 invMass1 = b1->m_invMass;
  		float32 invI1 = b1->m_invI;
  		float32 invMass2 = b2->m_invMass;
  		float32 invI2 = b2->m_invI;
  		b2Vec2 normal = c->normal;
  		b2Vec2 tangent = b2Cross(normal, 1.0f);
  		float32 friction = c->friction;
  //#define DEFERRED_UPDATE
  #ifdef DEFERRED_UPDATE
  		b2Vec2 b1_linearVelocity = b1->m_linearVelocity;
  		float32 b1_angularVelocity = b1->m_angularVelocity;
  		b2Vec2 b2_linearVelocity = b2->m_linearVelocity;
  		float32 b2_angularVelocity = b2->m_angularVelocity;
  #endif
  		// Solve normal constraints
  		for (int32 j = 0; j < c->pointCount; ++j)
  		{
  			b2ContactConstraintPoint* ccp = c->points + j;
  
  			// Relative velocity at contact
  			b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
  
  			// Compute normal impulse
  			float32 vn = b2Dot(dv, normal);
  			float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
  
  			// b2Clamp the accumulated impulse
  			float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
  			lambda = newImpulse - ccp->normalImpulse;
  
  			// Apply contact impulse
  			b2Vec2 P = lambda * normal;
  #ifdef DEFERRED_UPDATE
  			b1_linearVelocity -= invMass1 * P;
  			b1_angularVelocity -= invI1 * b2Cross(r1, P);
  
  			b2_linearVelocity += invMass2 * P;
  			b2_angularVelocity += invI2 * b2Cross(r2, P);
  #else
  			v1 -= invMass1 * P;
  			w1 -= invI1 * b2Cross(ccp->r1, P);
  
  			v2 += invMass2 * P;
  			w2 += invI2 * b2Cross(ccp->r2, P);
  #endif
  			ccp->normalImpulse = newImpulse;
  		}
  
  #ifdef DEFERRED_UPDATE
  		b1->m_linearVelocity = b1_linearVelocity;
  		b1->m_angularVelocity = b1_angularVelocity;
  		b2->m_linearVelocity = b2_linearVelocity;
  		b2->m_angularVelocity = b2_angularVelocity;
  #endif
  		// Solve tangent constraints
  		for (int32 j = 0; j < c->pointCount; ++j)
  		{
  			b2ContactConstraintPoint* ccp = c->points + j;
  
  			// Relative velocity at contact
  			b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
  
  			// Compute tangent force
  			float32 vt = b2Dot(dv, tangent);
  			float32 lambda = ccp->tangentMass * (-vt);
  
  			// b2Clamp the accumulated force
  			float32 maxFriction = friction * ccp->normalImpulse;
  			float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
  			lambda = newImpulse - ccp->tangentImpulse;
  
  			// Apply contact impulse
  			b2Vec2 P = lambda * tangent;
  
  			v1 -= invMass1 * P;
  			w1 -= invI1 * b2Cross(ccp->r1, P);
  
  			v2 += invMass2 * P;
  			w2 += invI2 * b2Cross(ccp->r2, P);
  
  			ccp->tangentImpulse = newImpulse;
  		}
  
  		b1->m_linearVelocity = v1;
  		b1->m_angularVelocity = w1;
  		b2->m_linearVelocity = v2;
  		b2->m_angularVelocity = w2;
  	}
  }
  
  void b2ContactSolver::FinalizeVelocityConstraints()
  {
  	for (int32 i = 0; i < m_constraintCount; ++i)
  	{
  		b2ContactConstraint* c = m_constraints + i;
  		b2Manifold* m = c->manifold;
  
  		for (int32 j = 0; j < c->pointCount; ++j)
  		{
  			m->points[j].normalImpulse = c->points[j].normalImpulse;
  			m->points[j].tangentImpulse = c->points[j].tangentImpulse;
  		}
  	}
  }
  
  bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
  {
  	float32 minSeparation = 0.0f;
  
  	for (int32 i = 0; i < m_constraintCount; ++i)
  	{
  		b2ContactConstraint* c = m_constraints + i;
  		b2Body* b1 = c->body1;
  		b2Body* b2 = c->body2;
  		float32 invMass1 = b1->m_mass * b1->m_invMass;
  		float32 invI1 = b1->m_mass * b1->m_invI;
  		float32 invMass2 = b2->m_mass * b2->m_invMass;
  		float32 invI2 = b2->m_mass * b2->m_invI;
  		
  		b2Vec2 normal = c->normal;
  
  		// Solver normal constraints
  		for (int32 j = 0; j < c->pointCount; ++j)
  		{
  			b2ContactConstraintPoint* ccp = c->points + j;
  
  			b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
  			b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
  
  			b2Vec2 p1 = b1->m_sweep.c + r1;
  			b2Vec2 p2 = b2->m_sweep.c + r2;
  			b2Vec2 dp = p2 - p1;
  
  			// Approximate the current separation.
  			float32 separation = b2Dot(dp, normal) + ccp->separation;
  
  			// Track max constraint error.
  			minSeparation = b2Min(minSeparation, separation);
  
  			// Prevent large corrections and allow slop.
  			float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
  
  			// Compute normal impulse
  			float32 dImpulse = -ccp->equalizedMass * C;
  
  			// b2Clamp the accumulated impulse
  			float32 impulse0 = ccp->positionImpulse;
  			ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f);
  			dImpulse = ccp->positionImpulse - impulse0;
  
  			b2Vec2 impulse = dImpulse * normal;
  
  			b1->m_sweep.c -= invMass1 * impulse;
  			b1->m_sweep.a -= invI1 * b2Cross(r1, impulse);
  			b1->SynchronizeTransform();
  
  			b2->m_sweep.c += invMass2 * impulse;
  			b2->m_sweep.a += invI2 * b2Cross(r2, impulse);
  			b2->SynchronizeTransform();
  		}
  	}
  
  	// We can't expect minSpeparation >= -b2_linearSlop because we don't
  	// push the separation above -b2_linearSlop.
  	return minSeparation >= -1.5f * b2_linearSlop;
  }
  