void dComplentaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
	dJacobianPair* const rowMatrix = params->m_rowMatrix;
	dJacobianColum* const jacobianColElements = params->m_colMatrix;

	const dVector& bodyVeloc0 = m_state0->GetVelocity();
	const dVector& bodyOmega0 = m_state0->GetOmega();
	const dVector& bodyVeloc1 = m_state1->GetVelocity();
	const dVector& bodyOmega1 = m_state1->GetOmega();

	int count = params->m_rowsCount;

	dAssert (params->m_timeStep > dFloat (0.0f));
	for (int k = 0; k < count; k ++) {
		const dJacobianPair& Jt = rowMatrix[k];
		dJacobianColum& element = jacobianColElements[k];

		dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) + Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) + Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));

		dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
		dFloat aRel = element.m_deltaAccel;
		//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
		dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
		
		vRel *= restitution;
		vRel = dMin (dFloat (4.0f), vRel);
		element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
	}
}
void dComplentaritySolver::dBilateralJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
	dJacobianColum* const jacobianColElements = params->m_colMatrix;
	dJacobianPair* const jacobianRowElements = params->m_rowMatrix;

	const dVector& bodyVeloc0 = m_state0->m_veloc;
	const dVector& bodyOmega0 = m_state0->m_omega;
	const dVector& bodyVeloc1 = m_state1->m_veloc;
	const dVector& bodyOmega1 = m_state1->m_omega;

	dFloat timestep = params->m_timeStep;
	dFloat kd = COMPLEMENTARITY_VEL_DAMP * dFloat (4.0f);
	dFloat ks = COMPLEMENTARITY_POS_DAMP * dFloat (0.25f);
	for (int k = 0; k < params->m_rowsCount; k ++) {
		if (m_rowIsMotor[k]) {
			jacobianColElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianColElements[k].m_deltaAccel;
		} else {
			const dJacobianPair& Jt = jacobianRowElements[k];
			dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) +
				Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + 
				Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) +
				Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));

			dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			dFloat aRel = jacobianColElements[k].m_deltaAccel;
			dFloat ksd = timestep * ks;
			dFloat relPosit = 0.0f - vRel * timestep * params->m_firstPassCoefFlag;

			dFloat num = ks * relPosit - kd * vRel - ksd * vRel;
			dFloat den = dFloat (1.0f) + timestep * kd + timestep * ksd;
			dFloat aRelErr = num / den;
			jacobianColElements[k].m_coordenateAccel = aRelErr + aRel;
		}
	}
}
void dgContact::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
	dgJacobianMatrixElement* const rowMatrix = params->m_rowMatrix;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

	dgInt32 count = params->m_rowsCount;

	dgFloat32 timestep = dgFloat32 (1.0f);
	dgFloat32 invTimestep = dgFloat32 (1.0f);
	if (params->m_timeStep > dgFloat32 (0.0f)) {
		timestep = params->m_timeStep;
		invTimestep = params->m_invTimeStep;
	}

	for (dgInt32 k = 0; k < count; k ++) {
//		if (!rowMatrix[k].m_accelIsMotor) 
//		{
			dgJacobianMatrixElement* const row = &rowMatrix[k];

			dgVector relVeloc (row->m_Jt.m_jacobianM0.m_linear.CompProduct4(bodyVeloc0) + row->m_Jt.m_jacobianM0.m_angular.CompProduct4(bodyOmega0) + row->m_Jt.m_jacobianM1.m_linear.CompProduct4(bodyVeloc1) + row->m_Jt.m_jacobianM1.m_angular.CompProduct4(bodyOmega1));
			dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			dgFloat32 aRel = row->m_deltaAccel;

			if (row->m_normalForceIndex < 0) {
				dgFloat32 restitution = (vRel <= dgFloat32 (0.0f)) ? (dgFloat32 (1.0f) + row->m_restitution) : dgFloat32 (1.0f);

				dgFloat32 penetrationVeloc = dgFloat32 (0.0f);
				if (row->m_penetration > DG_RESTING_CONTACT_PENETRATION * dgFloat32 (0.125f)) {
					if (vRel > dgFloat32 (0.0f)) {
						dgFloat32 penetrationCorrection = vRel * timestep;
						dgAssert (penetrationCorrection >= dgFloat32 (0.0f));
						row->m_penetration = dgMax (dgFloat32 (0.0f), row->m_penetration - penetrationCorrection);
					} else {
						dgFloat32 penetrationCorrection = -vRel * timestep * row->m_restitution * dgFloat32 (8.0f);
						if (penetrationCorrection > row->m_penetration) {
							row->m_penetration = dgFloat32 (0.001f);
						}
					}
					penetrationVeloc = -(row->m_penetration * row->m_penetrationStiffness);
				}

				vRel *= restitution;
				vRel = dgMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
			}
			row->m_coordenateAccel = (aRel - vRel * invTimestep);
//		}
	}
}
void dgContact::JointAccelerations(const dgJointAccelerationDecriptor& params)
{
	const dgJacobianPair* const Jt = params.m_Jt;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

	for (dgInt32 k = 0; k < params.m_rowsCount; k ++) {
		if (!params.m_accelIsMotor[k]) {
			dgFloat32 vRel;
			dgFloat32 aRel;

			dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
			relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
			relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
			relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
			vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			aRel = params.m_externAccelaration[k];

			if (params.m_normalForceIndex[k] < 0) {
				dgFloat32 restitution;
				dgFloat32 penetrationVeloc;
				

				restitution = dgFloat32 (1.0f);
				if (vRel <= dgFloat32 (0.0f)) {
					restitution += params.m_restitution[k];
				}

				penetrationVeloc = 0.0f;
				if (params.m_penetration[k] > dgFloat32 (1.0e-2f)) {
					dgFloat32 penetrationCorrection;
					if (vRel > dgFloat32 (0.0f)) {
						penetrationCorrection = vRel * params.m_timeStep;
						_ASSERTE (penetrationCorrection >= dgFloat32 (0.0f));
						params.m_penetration[k] = GetMax (dgFloat32 (0.0f), params.m_penetration[k] - penetrationCorrection);
					}
					penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k]);
				}

				vRel *= restitution;
				vRel = GetMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
			}
			params.m_coordenateAccel[k] =  (aRel - vRel * params.m_invTimeStep);
		}
	}
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
	int constraintIndex;
	dFloat invTimestep;
	dMatrix bodyMatrix;  

	// get the simulation time
	invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyOmega;
	dVector bodyVelocity;
	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

	// all tire is on air check
	m_vehicleOnAir = 0;
	constraintIndex = 0;
	for ( int i = 0; i < m_tiresCount; i ++ ) {

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit); 
		tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
		tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );

		if (tire.m_posit < tire.m_suspensionLenght )  {
			dFloat distance;
			dFloat sideSlipCoef;
			dFloat slipRatioCoef;
			dFloat tireForceMag;
			dFloat tireTorqueMag;
			dFloat suspensionSpeed;
			dFloat axelLinealSpeed;
			dFloat tireRotationSpeed;
			dFloat frictionCircleMag;
			dFloat longitudinalForceMag;
			dFloat lateralFrictionForceMag;


			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 

			// calculate the relative velocity
			dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
			suspensionSpeed = - (relVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

			tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);

			// calculate relative velocity at the tire center
			dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
			dVector tireAngularVelocity (tire.m_lateralPin.Scale (tire.m_angularVelocity));
			dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
			dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);	

			longitudinalForceMag = 0.0f;
//			if (!tire.m_tireIsConstrained) {
				
				// calculate slip ratio and max longitudinal force
				tireRotationSpeed = tireRotationalVelocityAtContact % tire.m_longitudinalPin;
				slipRatioCoef = (dAbs (axelLinealSpeed) > 1.e-3f) ? ((-tireRotationSpeed - axelLinealSpeed) / dAbs (axelLinealSpeed)) : 0.0f;

				// calculate the formal longitudinal force the tire apply to the chassis
				longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
//			} 

		
			// now calculate relative velocity a velocity at contact point
			dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact); 

			// calculate the sideslip as the angle between the tire lateral speed and longitudila speed 
			sideSlipCoef = dAtan2 (dAbs (tireContactRelativeVelocity % tire.m_lateralPin), dAbs (axelLinealSpeed));
			lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;

			// Apply brake, need some little fix here.
			// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
			if ( tire.m_breakForce > 1.0e-3f ) {
				// row constrained force is save for later determine the dynamic state of this tire 
  				tire.m_isBrakingForceIndex = constraintIndex;
				constraintIndex ++;

				frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
				if (tire.m_breakForce > frictionCircleMag) {
					tire.m_breakForce = frictionCircleMag;
				}

				//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
				NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
				NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
				NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);

				// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
				tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
				if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
  					lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
				}
			} 


			//project the longitudinal and lateral forces over the circle of friction for this tire; 
			frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
			tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
			if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
				dFloat invMag2;
				invMag2 = frictionCircleMag / dSqrt (tireForceMag);
				longitudinalForceMag *= invMag2;
				lateralFrictionForceMag *= invMag2;
			}

			// submit this constraint for calculation of side slip forces
			lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
			tire.m_lateralForceIndex = constraintIndex;
			constraintIndex ++;
			NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_lateralPin[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);

			// accumulate the longitudinal force
			dVector tireForce (tire.m_longitudinalPin.Scale (longitudinalForceMag));
			tire.m_tireForceAcc += tireForce;

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector torque ((tire.m_tireAxelPosit - chassisMatrix.m_posit) * tire.m_tireForceAcc);
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );


			// calculate the net torque on the tire
			tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPin);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force can no be larger than the applied engine torque 
				// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
				tire.m_tireIsConstrained = 1;
				tireTorqueMag = tire.m_torque;
			}

			tire.m_torque -= tireTorqueMag;
		} 	
	}
}
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{

	// check if this is an impulsive time step
	
	if (timestep > 0.0f) {
		dMatrix matrix0;
		dVector v(0.0f);
		dVector w(0.0f);
		dVector cg(0.0f);

		dFloat invTimestep = 1.0f / timestep;

		// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
		NewtonBodyGetOmega (m_body0, &w[0]);
		NewtonBodyGetVelocity (m_body0, &v[0]);
		NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
		NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);

		dVector p0 (matrix0.TransformVector (m_localHandle));

		dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
		dVector relPosit (m_targetPosit - p0);
		dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
		dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); 
			
		// Restrict the movement on the pivot point along all tree orthonormal direction
		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		if (m_pickMode) {
			dQuaternion rotation;

			NewtonBodyGetRotation (m_body0, &rotation.m_q0);
			if (m_targetRot.DotProduct (rotation) < 0.0f) {
				rotation.m_q0 *= -1.0f; 
				rotation.m_q1 *= -1.0f; 
				rotation.m_q2 *= -1.0f; 
				rotation.m_q3 *= -1.0f; 
			}

			dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
			dFloat mag = relOmega % relOmega;
			if (mag > 1.0e-6f) {
				dVector pin (relOmega.Scale (1.0f / mag));
				dMatrix basis (dGrammSchmidt (pin)); 	
				dFloat relSpeed = dSqrt (relOmega % relOmega);
				dFloat relAlpha = relSpeed * invTimestep;

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

			} else {

				dVector relAlpha (w.Scale (-invTimestep));
				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
			}

		} else {
			// this is the single handle pick mode, add some angular friction

			dVector relAlpha = w.Scale (-invTimestep);
			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);
		}
	}
}
void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
	dgJacobianMatrixElement* const jacobianMatrixElements = params->m_rowMatrix;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

// remember the impulse branch 
//dgAssert (params->m_timeStep > dgFloat32 (0.0f));
	if (params->m_timeStep > dgFloat32 (0.0f)) {
		dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
		dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);
		dgFloat32 dt = params->m_timeStep;
		for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
			if (m_rowIsMotor[k]) {
				//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
   				jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
			} else {
				const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
				dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) + Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
								   Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) + Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));

				dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;

				//dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
				//dgFloat32 aRel = jacobianMatrixElements[k].m_coordenateAccel;
				dgFloat32 aRel = params->m_firstPassCoefFlag ? jacobianMatrixElements[k].m_deltaAccel : jacobianMatrixElements[k].m_coordenateAccel;

				//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
				//alphaError = num / den;

				//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
				//dgFloat32 dt = desc.m_timestep;
				//dgFloat32 ks = DG_POS_DAMP;
				//dgFloat32 kd = DG_VEL_DAMP;
				//dgFloat32 ksd = dt * ks;
				//dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
				//dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
				//accelError = num / den;

				dgFloat32 ksd = dt * ks;
				dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
				jacobianMatrixElements[k].m_penetration = relPosit;


				dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
				dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
				dgFloat32 aRelErr = num / den;

				//centripetal acceleration is stored in restitution member
				jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;
			}
		}
	} else {
		for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
			if (m_rowIsMotor[k]) {
				jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
			} else {
				const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
				dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) +
								   Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
								   Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) +
								   Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));

				dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
				jacobianMatrixElements[k].m_coordenateAccel = jacobianMatrixElements[k].m_deltaAccel - vRel;
			}
		}
	}
}
void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
	dgJacobianMatrixElement* const jacobianMatrixElements = params->m_rowMatrix;

//	const dgJacobianPair* const Jt = params.m_Jt;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

#if	1	
	dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
	dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);

	dgFloat32 dt = params->m_timeStep;
	for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
		if (m_rowIsMotor[k]) {
			//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
   			jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
		} else {
//			dgFloat32 num;
//			dgFloat32 den;
//			dgFloat32 ksd;
//			dgFloat32 vRel;
//			dgFloat32 aRel;
//			dgFloat32 aRelErr;
//			dgFloat32 relPosit;

			const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
			dgVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
			relVeloc += Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
			relVeloc += Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
			relVeloc += Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);

			dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
			//aRel = params.m_externAccelaration[k];
			dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
			//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
			//		alphaError = num / den;

			//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
			//		dgFloat32 dt = desc.m_timestep;
			//		dgFloat32 ks = DG_POS_DAMP;
			//		dgFloat32 kd = DG_VEL_DAMP;
			//		dgFloat32 ksd = dt * ks;
			//		dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
			//		dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
			//		accelError = num / den;

			dgFloat32 ksd = dt * ks;
			//dgFloat32 relPosit = params.m_penetration[k] - vRel * dt * params.m_firstPassCoefFlag;
			dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
			//params.m_penetration[k] = relPosit;
			jacobianMatrixElements[k].m_penetration = relPosit;


			dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
			dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
			dgFloat32 aRelErr = num / den;

			//centripetal acceleration is stored restitution member
			//params.m_coordenateAccel[k] = aRelErr + params.m_restitution[k] + aRel;
			jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;

#else
			dgFloat32 vRel;
			dgFloat32 aRel;
			dgFloat32 penetrationVeloc;
			dgFloat32 penetrationCorrection;

			dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
			relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
			relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
			relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);

			vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z ;		
			
			penetrationCorrection = vRel * params.m_timeStep * params.m_firstPassCoefFlag;
			params.m_penetration[k] = params.m_penetration[k] - penetrationCorrection;
			if (params.m_penetration[k] > dgFloat32 (1.0f) ) {
				params.m_penetration[k] = dgFloat32 (1.0f);
			} else if (params.m_penetration[k] < dgFloat32 (-1.0f) ) {
				params.m_penetration[k] = dgFloat32 (-1.0f);
			}
			penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k] * params.m_invTimeStep);
			
			vRel += penetrationVeloc;
			//centripetal acceleration is stored restitution member
			aRel = params.m_externAccelaration[k] + params.m_restitution[k];
			params.m_coordenateAccel[k] =  (aRel - vRel * params.m_invTimeStep);
#endif
		}
	}
}
void dgCollisionDeformableSolidMesh::ApplyExternalForces (dgFloat32 timestep)
{
	dgAssert (m_myBody);

	dgBody* const body = GetBody();


	dgAssert (body->GetMass().m_w > dgFloat32 (0.0f));
	dgAssert (body->GetMass().m_w < dgFloat32 (1.0e5f));
	const dgMatrix& matrix = body->GetCollision()->GetGlobalMatrix();

	dgFloat32 invMass = body->GetInvMass().m_w;
	dgVector velocyStep (body->GetForce().Scale4(invMass * timestep));
//velocyStep = dgVector(0.0f);

	dgVector* const veloc = m_particles.m_veloc;
	dgFloat32* const unitMass = m_particles.m_unitMass;

/*
invMass = 0;
dgVector w (0.0f, 0.0f, 1.0f, 0.0f);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
unitMass[i] = 1.0f;
veloc[i] = w * m_posit[i];
invMass += unitMass[i];
}
invMass = 1.0f / invMass;
*/

	dgVector com (dgFloat32 (0.0f));
	dgVector comVeloc (dgFloat32 (0.0f));
	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		dgVector mass (unitMass[i]);
        const dgVector& p = m_posit[i]; 
		veloc[i] += velocyStep;
		com += p.CompProduct4(mass);
		comVeloc += veloc[i].CompProduct4(mass);
	}
	com = com.Scale4(invMass);
	comVeloc = comVeloc.Scale4(invMass);


	const dgMatrix& indentity = dgGetIdentityMatrix();
	dgMatrix inertiaMatrix (dgGetZeroMatrix());
	inertiaMatrix.m_posit = dgVector::m_wOne;
	dgVector comAngularMomentum (dgFloat32 (0.0f));
	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		dgVector mass (unitMass[i]);
		dgVector r (m_posit[i] - com);
		dgVector mr (r.CompProduct4(mass));
		dgVector relVeloc (veloc[i] - comVeloc);
		comAngularMomentum += mr * relVeloc;

		dgMatrix inertia (mr, r);
		dgVector diagInertia (mr.DotProduct4(r));

		inertiaMatrix.m_front += (indentity.m_front.CompProduct4(diagInertia) - inertia.m_front); 
		inertiaMatrix.m_up += (indentity.m_up.CompProduct4(diagInertia) - inertia.m_up); 
		inertiaMatrix.m_right += (indentity.m_right.CompProduct4(diagInertia) - inertia.m_right);
		dgAssert (inertiaMatrix.TestSymetric3x3());
	}

dgVector damp (0.3f); 
	dgMatrix invInertia (inertiaMatrix.Symetric3by3Inverse());
	dgVector omega (invInertia.RotateVector(comAngularMomentum));
	for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
		dgVector r (m_posit[i] - com);
		dgVector deltaVeloc (comVeloc + omega * r - veloc[i]);
		veloc[i] += deltaVeloc.CompProduct4(damp);
	}

//Sleep (50);

	dgMatrix tmp;
	dgMatrix transform;
	dgVector scale;
	inertiaMatrix.PolarDecomposition (transform, scale, tmp, matrix);
	body->GetCollision()->SetGlobalMatrix(transform);
	body->SetMatrixOriginAndRotation(body->GetCollision()->GetLocalMatrix().Inverse() * transform);
    body->SetVelocity(comVeloc);
    body->SetOmega(omega);
}