Index: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 5781)
+++ code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 8351)
@@ -13,4 +13,50 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
+
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+
+
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+	m_contactManifold(*contactManifold)
+{
+
+}
+
+btContactConstraint::~btContactConstraint()
+{
+
+}
+
+void	btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+	m_contactManifold = *contactManifold;
+}
+
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+
+}
+
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+
+}
+
+void	btContactConstraint::buildJacobian()
+{
+
+}
+
+
+
 
 
@@ -86,344 +132,3 @@
 
 
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollision(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
 
-	const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
-   	const btVector3& normal = contactPoint.m_normalWorldOnB;
-
-	//constant over all iterations
-	btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
-	
-	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-	btVector3 vel = vel1 - vel2;
-	btScalar rel_vel;
-	rel_vel = normal.dot(vel);
-	
-	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
-	// btScalar damping = solverInfo.m_damping ;
-	btScalar Kerp = solverInfo.m_erp;
-	btScalar Kcor = Kerp *Kfps;
-
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-	btScalar distance = cpd->m_penetration;
-	btScalar positionalError = Kcor *-distance;
-	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
-	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
-	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
-	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-	
-	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-	btScalar sum = oldNormalImpulse + normalImpulse;
-	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-	return normalImpulse;
-}
-
-
-btScalar resolveSingleFriction(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	(void)solverInfo;
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-
-	btScalar combinedFriction = cpd->m_friction;
-	
-	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-	
-	if (cpd->m_appliedImpulse>btScalar(0.))
-	//friction
-	{
-		//apply friction in the 2 tangential directions
-		
-		// 1st tangent
-		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-	
-		btScalar j1,j2;
-
-		{
-				
-			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
-			// calculate j that moves us to zero relative velocity
-			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
-			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
-			cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
-			btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
-			btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
-			j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
-
-		}
-		{
-			// 2nd tangent
-
-			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-			
-			// calculate j that moves us to zero relative velocity
-			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
-			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
-			cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
-			btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
-			btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
-			j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
-		}
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
-		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
-		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
-	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-
-	} 
-	return cpd->m_appliedImpulse;
-}
-
-
-btScalar resolveSingleFrictionOriginal(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionOriginal(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	(void)solverInfo;
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-
-	btScalar combinedFriction = cpd->m_friction;
-	
-	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-	//if (contactPoint.m_appliedImpulse>btScalar(0.))
-	//friction
-	{
-		//apply friction in the 2 tangential directions
-		
-		{
-			// 1st tangent
-			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-			
-			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
-			// calculate j that moves us to zero relative velocity
-			btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
-			btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
-			btSetMin(total, limit);
-			btSetMax(total, -limit);
-			j = total - cpd->m_accumulatedTangentImpulse0;
-			cpd->m_accumulatedTangentImpulse0 = total;
-			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
-			body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
-		}
-
-				
-		{
-			// 2nd tangent
-			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-
-			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-			
-			// calculate j that moves us to zero relative velocity
-			btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
-			btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
-			btSetMin(total, limit);
-			btSetMax(total, -limit);
-			j = total - cpd->m_accumulatedTangentImpulse1;
-			cpd->m_accumulatedTangentImpulse1 = total;
-			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
-			body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
-		}
-	} 
-	return cpd->m_appliedImpulse;
-}
-
-
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollisionCombined(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-   	const btVector3& normal = contactPoint.m_normalWorldOnB;
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-	btVector3 vel = vel1 - vel2;
-	btScalar rel_vel;
-	rel_vel = normal.dot(vel);
-	
-	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
-	//btScalar damping = solverInfo.m_damping ;
-	btScalar Kerp = solverInfo.m_erp;
-	btScalar Kcor = Kerp *Kfps;
-
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-	btScalar distance = cpd->m_penetration;
-	btScalar positionalError = Kcor *-distance;
-	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
-	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
-	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
-	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-	
-	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-	btScalar sum = oldNormalImpulse + normalImpulse;
-	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-	{
-		//friction
-		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-	  
-		rel_vel = normal.dot(vel);
-
-
-		btVector3 lat_vel = vel - normal * rel_vel;
-		btScalar lat_rel_vel = lat_vel.length();
-
-		btScalar combinedFriction = cpd->m_friction;
-
-		if (cpd->m_appliedImpulse > 0)
-		if (lat_rel_vel > SIMD_EPSILON)
-		{
-			lat_vel /= lat_rel_vel;
-			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
-			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
-			btScalar friction_impulse = lat_rel_vel /
-				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
-
-			btSetMin(friction_impulse, normal_impulse);
-			btSetMax(friction_impulse, -normal_impulse);
-			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
-		}
-	}
-
-
-
-	return normalImpulse;
-}
-
-btScalar resolveSingleFrictionEmpty(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionEmpty(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-	(void)contactPoint;
-	(void)body1;
-	(void)body2;
-	(void)solverInfo;
-
-
-	return btScalar(0.);
-}
-
