void dCustomBallAndSocket::SubmitConstraintTwistLimits(const dMatrix& matrix0, const dMatrix& matrix1, const dVector& relOmega, dFloat timestep)
{
	dFloat jointOmega = relOmega.DotProduct3(matrix0.m_front);
	dFloat twistAngle = m_twistAngle.GetAngle() + jointOmega * timestep;
	if (twistAngle < m_minTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_minTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
	} else if (twistAngle > m_maxTwistAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_maxTwistAngle - m_twistAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

	} else if (m_twistFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

		dFloat accel = NewtonUserJointCalculateRowZeroAccelaration(m_joint);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
	}
}
void dCustomHinge::SubmitConstraintsFrictionAndLimit(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dFloat angle = GetPitch();
	if (angle < m_minAngle) {
		dFloat relAngle = m_minAngle - angle;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
//		m_lastRowWasUsed = true;
	} else if (angle > m_maxAngle) {
		dFloat relAngle = m_maxAngle - angle;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
//		m_lastRowWasUsed = true;
	} else {
		// friction but not limits
		dFloat alpha = m_jointOmega / timestep;
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowAcceleration(m_joint, -alpha);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
//		m_lastRowWasUsed = true;
	}
}
void dCustomCorkScrew::SubmitConstraintLimits(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dFloat angle = m_curJointAngle.GetAngle() + m_angularOmega * timestep;
	if (angle < m_minAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_minAngle - m_curJointAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
	} else if (angle > m_maxAngle) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, 1.0f);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);

		const dFloat invtimestep = 1.0f / timestep;
		const dFloat speed = 0.5f * (m_maxAngle - m_curJointAngle.GetAngle()) * invtimestep;
		const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
		NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

	} else if (m_angularFriction != 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
	}
}
// rolling friction works as follow: the idealization of the contact of a spherical object 
// with a another surface is a point that pass by the center of the sphere.
// in most cases this is enough to model the collision but in insufficient for modeling 
// the rolling friction. In reality contact with the sphere with the other surface is not 
// a point but a contact patch. A contact patch has the property the it generates a fix 
// constant rolling torque that opposes the movement of the sphere.
// we can model this torque by adding a clamped torque aligned to the instantaneously axis 
// of rotation of the ball. and with a magnitude of the stopping angular acceleration.
void CustomDryRollingFriction::SubmitConstrainst (dFloat timestep, int threadIndex)
{
	dVector omega;
	dFloat omegaMag;
	dFloat torqueFriction;

	// get the omega vector
	NewtonBodyGetOmega(m_body0, &omega[0]);

	omegaMag = dSqrt (omega % omega);
	if (omegaMag > 0.1f) {
		// tell newton to used this the friction of the omega vector to apply the rolling friction
		dVector pin (omega.Scale (1.0f / omegaMag));
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &pin[0]);

		// calculate the acceleration to stop the ball in one time step
		NewtonUserJointSetRowAcceleration (m_joint, -omegaMag / timestep);

		// set the friction limit proportional the sphere Inertia
		torqueFriction = m_frictionTorque * m_frictionCoef;
		NewtonUserJointSetRowMinimumFriction (m_joint, -torqueFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint, torqueFriction);

	} else {
		// when omega is too low sheath a little bit and damp the omega directly
		omega = omega.Scale (0.2f);
		NewtonBodySetOmega(m_body0, &omega[0]);
	}
}
	void SubmitConstraints(dFloat timestep, int threadIndex)
	{
		CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
		float invTimestep = 1.0f / timestep;

		dMatrix matrix0;
		dMatrix matrix1;

		CalculateGlobalMatrix(matrix0, matrix1);

		if (m_anim_speed != 0.0f) // some animation to illustrate purpose
		{
			m_anim_time += timestep * m_anim_speed;
			float a0 = sin(m_anim_time);
			float a1 = m_anim_offset * 3.14f;
			dVector axis(sin(a1), 0.0f, cos(a1));
			//dVector axis (1,0,0);
			m_target = dQuaternion(axis, a0 * 0.5f);
		}

		// measure error
		dQuaternion q0(matrix0);
		dQuaternion q1(matrix1);
		dQuaternion qt0 = m_target * q1;
		dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f)	? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;

		float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
		dVector errorAngVel(0, 0, 0);

		dMatrix basis;
		if (errorAngle > 1.0e-10f) {
			dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
			errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
			errorAngVel = errorAxis.Scale(errorAngle * invTimestep);

			basis = dGrammSchmidt(errorAxis);
		} else {
			basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
		}

		dVector angVel0, angVel1;
		NewtonBodyGetOmega(m_body0, (float*)&angVel0);
		NewtonBodyGetOmega(m_body1, (float*)&angVel1);

		dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);

		// motor
		for (int n = 0; n < 3; n++) {
			// calculate the desired acceleration
			dVector &axis = basis[n];
			float relAccel = angAcc % axis;

			NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
			NewtonUserJointSetRowAcceleration(m_joint, relAccel);
			NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		}
	}
void dCustomHinge::SubmitConstraintsFrictionOnly(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dFloat alpha = m_jointOmega / timestep;
	NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
	NewtonUserJointSetRowAcceleration(m_joint, -alpha);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
	NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
}
void CustomUniversalActuator::SubmitConstraints (dFloat timestep, int threadIndex)
{
	CustomUniversal::SubmitConstraints (timestep, threadIndex);

	if (m_flag0 | m_flag1){
		dMatrix matrix0;
		dMatrix matrix1;

		CalculateGlobalMatrix (matrix0, matrix1);
		if (m_flag0) {
			dFloat jointAngle = GetJointAngle_0();
			dFloat relAngle = jointAngle - m_angle0;
			NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix0.m_front[0]);
			dFloat step = m_angularRate0 * timestep;
			if (dAbs (relAngle) > 2.0f * dAbs (step)) {
				dFloat desiredSpeed = dSign(relAngle) * m_angularRate0;
				dFloat currentSpeed = GetJointOmega_0 ();
				dFloat accel = (desiredSpeed - currentSpeed) / timestep;
				NewtonUserJointSetRowAcceleration (m_joint, accel);
			}
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce0);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce0);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		}

		if (m_flag1) {
			dFloat jointAngle = GetJointAngle_1();
			dFloat relAngle = jointAngle - m_angle1;
			NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix1.m_up[0]);
			dFloat step = m_angularRate1 * timestep;
			if (dAbs (relAngle) > 2.0f * dAbs (step)) {
				dFloat desiredSpeed = dSign(relAngle) * m_angularRate1;
				dFloat currentSpeed = GetJointOmega_1 ();
				dFloat accel = (desiredSpeed - currentSpeed) / timestep;
				NewtonUserJointSetRowAcceleration (m_joint, accel);
			}
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce1);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce1);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		}
	}
}
void CustomBallAndSocketWithFriction::SubmitConstraints(dFloat timestep, int threadIndex)
{
	CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
	dVector omega0(0.0f, 0.0f, 0.0f, 0.0f);
	dVector omega1(0.0f, 0.0f, 0.0f, 0.0f);

	// get the omega vector
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}

	dVector relOmega(omega0 - omega1);
	dFloat omegaMag = dSqrt(relOmega % relOmega);
	if (omegaMag > 0.1f) {
		// tell newton to used this the friction of the omega vector to apply the rolling friction
		dMatrix basis(dGrammSchmidt(relOmega));

		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);

		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);

		// calculate the acceleration to stop the ball in one time step
		dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[0][0]);
		NewtonUserJointSetRowAcceleration(m_joint, -omegaMag * invTimestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
	} else {
		// when omega is too low this is correct but the small angle approximation theorem.
		dMatrix basis(dGetIdentityMatrix());
		for (int i = 0; i < 3; i++) {
			NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[i][0]);
			NewtonUserJointSetRowMinimumFriction(m_joint, -m_dryFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, m_dryFriction);
		}
	}
}
void CustomSlider::SubmitConstraintsFreeDof(dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
	// if limit are enable ...
	if (m_limitsOn) {
		if (m_posit < m_minDist) {
			// get a point along the up vector and set a constraint  
			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			m_lastRowWasUsed = true;
		} else if (m_posit > m_maxDist) {
			// get a point along the up vector and set a constraint  

			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
			m_lastRowWasUsed = true;
		} else {

/*
			// uncomment this for a slider with friction

			// take any point on body0 (origin)
			const dVector& p0 = matrix0.m_posit;

			dVector veloc0(0.0f); 
			dVector veloc1(0.0f); 
			dVector omega1(0.0f); 

			NewtonBodyGetVelocity(m_body0, &veloc0[0]);
			NewtonBodyGetVelocity(m_body1, &veloc1[0]);
			NewtonBodyGetOmega(m_body1, &omega1[0]);

			// this assumes the origin of the bodies the matrix pivot are the same
			veloc1 += omega1 * (matrix1.m_posit - p0);

			dFloat relAccel; 
			relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;

			#define MaxFriction 10.0f
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAccel);
			NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
			m_lastRowWasUsed = false;
*/
		}
	} 
}
void dCustomTireSpringDG::SuspenssionSpringLimits(dFloat steptime)
{
	// Suspenssion limitas
	if (mDistance < mMinSuspenssion) {
		NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInChassis[0], &mChassisPivotMatrix.m_up[0]);
		NewtonUserJointSetRowMinimumFriction(m_joint, -0.0);
	}
	else
	if (mDistance > mMaxSuspenssion) {
		NewtonUserJointAddLinearRow(m_joint, &mCenterInChassis[0], &mCenterInChassis[0], &mChassisPivotMatrix.m_up[0]);
		NewtonUserJointSetRowMaximumFriction(m_joint, 0.0);
	}
}
void CustomDGRayCastCar::ApplyTireFrictionVelocitySiding (Tire& tire, const dMatrix& chassisMatrix, const dVector& tireAxelVeloc, const dVector& tireAxelPosit, dFloat timestep, dFloat invTimestep)
{
	dFloat invMag2;
	dFloat frictionCircleMag;
	dFloat lateralFrictionForceMag;
	dFloat longitudinalFrictionForceMag;
//	dFloat tireContactSpeed;
//	dFloat tireRelativeSpeed;
//	dFloat lateralForceMagnitud;

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

	// now calculate relative velocity a velocity at contact point
	dVector tireAngularVelocity ( tire.m_lateralPin.Scale (tire.m_angularVelocity));
	dVector tireRadius (tire.m_contactPoint - tireAxelPosit);
	dVector tireContactVelocity ( tireAngularVelocity * tireRadius);	
	dVector tireContactRelativeVelocity ( tireAxelRelativeVelocity + tireContactVelocity ); 


	// 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 ( dAbs( tire.m_breakForce ) > 1.0e-3f ) {
		_ASSERTE (0);
//		tire.m_isBraking = 1;
//		tire.m_torque = 0.0f;
//		tire.m_turnforce = tire.m_turnforce * 0.5f;
//		tire.m_breakForce /= timestep;
//		NewtonUserJointAddLinearRow ( m_joint, &tireAxelPosit[0], &tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
//		NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce );
//		NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce );
	} 

	tire.m_breakForce = 0.0f;  

	//submit constrained for applying side forces.
	frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
	lateralFrictionForceMag = frictionCircleMag;
	longitudinalFrictionForceMag = tire.m_tireLoad;
	invMag2 = frictionCircleMag / dSqrt ( lateralFrictionForceMag * lateralFrictionForceMag + longitudinalFrictionForceMag * longitudinalFrictionForceMag );

	lateralFrictionForceMag *= invMag2;
	longitudinalFrictionForceMag = invMag2;
	NewtonUserJointAddLinearRow (m_joint, &tireAxelPosit[0], &tireAxelPosit[0], &tire.m_lateralPin[0]);
	NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
	NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);

	// save the tire contact longitudinal velocity for integration after the solver
//	tire.m_currentSlipVeloc = tireAxelRelativeVelocity % tire.m_longitudinalPin;
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	const dFloat angleError = GetMaxAngleError();
	dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
	NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle0) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
	NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle1) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));

	// save the current joint Omega
	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);

	if (m_options.m_option2) {
		if (m_options.m_option3) {
			dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
		} else {
			dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
		}
	} else if (m_options.m_option3) {
		dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
	} else if (m_angularFriction != 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
	}
}
void dCustomHinge::SubmitConstraintsLimitsOnly(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dFloat angle = GetPitch();
	if (angle < m_minAngle) {
		dFloat relAngle = m_minAngle - angle;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
		//		m_lastRowWasUsed = true;
	} else if (angle > m_maxAngle) {
		dFloat relAngle = m_maxAngle - angle;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
		//		m_lastRowWasUsed = true;
	}
}
void dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
	if (m_actuatorFlag) {
		dFloat jointangle = GetActuatorAngle();
		dFloat relAngle = jointangle - m_angle;
		dFloat currentSpeed = GetJointOmega();
		dFloat step = dFloat(2.0f) * m_angularRate * timestep;
		dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
		dFloat accel = (desiredSpeed - currentSpeed) / timestep;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
        NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
        NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce);
		NewtonUserJointSetRowStiffness (m_joint, 1.0f);
	} else {
		dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
	}
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dMatrix localMatrix(matrix0 * matrix1.Inverse());
	dVector euler0;
	dVector euler1;
	localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);

	dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
	rollPin = matrix1.RotateVector(rollPin);

	NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	m_curJointAngle.Update(euler0.m_x);

	// save the current joint Omega
	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);

	if (m_options.m_option2) {
		if (m_options.m_option3) {
			dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
		} else {
			dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
		}
	} else if (m_options.m_option3) {
		dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
	} else if (m_angularFriction != 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
	}
}
		void ApplySuspesionForce (
			dFloat timestep,
			const NewtonBody* thread, const dVector& threadPointLocal, const dMatrix& threadMatrix, const dVector& threadCOM, const dVector& threadVeloc, const dVector& threadOmega,
			const NewtonBody* parent, const dVector& parentPointLocal, const dMatrix& parentMatrix, const dVector& parentCOM, const dVector& parentVeloc, const dVector& parentOmega)
		{
			dFloat dist;
			dFloat speed;
			dFloat forceMag;

			// calculate separation and speed of hard points
			dVector threadPoint (threadMatrix.TransformVector(threadPointLocal));
			dVector parentPoint (parentMatrix.TransformVector(parentPointLocal));
			dist = (parentPoint - threadPoint) % parentMatrix.m_up;
			speed = ((parentVeloc + parentOmega * (parentPoint - parentCOM) -
					  threadVeloc - threadOmega * (threadPoint - threadCOM)) % parentMatrix.m_up);

			if (dist > MAX_COMPRESION_DIST) {
				NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
				NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
			} else if (dist < MIN_EXPANSION_DIST) {
				// submit a contact constraint to prevent the body 
				NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
				NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
			} 

			// apply the spring force
			forceMag = NewtonCalculateSpringDamperAcceleration (timestep, SPRING_CONST, dist, DAMPER_CONST, speed) * m_massScale;


			dVector forceParent (parentMatrix.m_up.Scale (forceMag));
			dVector torqueParent ((parentPoint - parentCOM) * forceParent);
			NewtonBodyAddForce(m_body1, &forceParent[0]);
			NewtonBodyAddTorque(m_body1, &torqueParent[0]);
			
		
			dVector forceThread (forceParent.Scale (-1.0f));
			dVector torqueThread ((threadPoint - threadCOM) * forceThread);
			NewtonBodyAddForce(m_body0, &forceThread[0]);
			NewtonBodyAddTorque(m_body0, &torqueThread[0]);
		}
void dCustomTireSpringDG::TireBreakAction(NewtonBody* const attBody, dFloat steptime)
{
	dMatrix tireMatrix;
	//
	NewtonBodyGetMatrix(attBody, &tireMatrix[0][0]);
	//
	if ((mUseBreak) || (mUseHardBreak)) {
		if (dAbs(mBrakeTorque) > VEHICLE_BREAK_CHECK) {
			NewtonUserJointAddAngularRow(m_joint, 0.0, &tireMatrix.m_front[0]);
			//dFloat relOmega = mRealOmega / steptime;
			// D.G: I get a error when I enable this newton function.
			// D.G: Maybe my mRealOmega calcul is wrong.
			// D.G: The error happen at the begin if you break or randomly, most of time the vehicle is in air when it happen.
			// D.G: I'm not sure if the function is really needed here.
			//NewtonUserJointSetRowAcceleration(m_joint, relOmega);
			//
			NewtonUserJointSetRowMinimumFriction(m_joint, -mBrakeTorque);
			NewtonUserJointSetRowMaximumFriction(m_joint, mBrakeTorque);
		}
		mBrakeTorque = 0.0;
	}
}
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// add the linear limits
	const dVector& p0 = matrix0.m_posit;
	const dVector& p1 = matrix1.m_posit;
	dVector dp (p0 - p1);

	for (int i = 0; i < 3; i ++) {
		if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		} else {
			// it is a limited linear dof, check if it pass the limits
			dFloat dist = dp.DotProduct3(matrix1[i]);
			if (dist > m_maxLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
				if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

			} else if (dist < m_minLinearLimits[i]) {
				dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));

				// clamp the error, so the not too much energy is added when constraint violation occurs
				dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
				if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
					q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
				}

				NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
				NewtonUserJointSetRowStiffness (m_joint, 1.0f);
				// allow the object to return but not to kick going forward
				NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			}
		}
	}

	dVector euler0(0.0f);
	dVector euler1(0.0f);
	dMatrix localMatrix (matrix0 * matrix1.Inverse());
	localMatrix.GetEulerAngles(euler0, euler1);

	AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
	AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
	if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
		euler0 = euler1;
	}

	dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);

//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f,  euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));

	bool limitViolation = false;
	for (int i = 0; i < 3; i ++) {
		if (euler[i] < m_minAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_minAngularLimits[i];
		} else if (euler[i] > m_maxAngularLimits[i]) {
			limitViolation = true;
			euler[i] = m_maxAngularLimits[i];
		}
	}

	if (limitViolation) {
		//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
		dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
		dMatrix baseMatrix (p0y0r0 * matrix1);
        dMatrix rotation (matrix0.Inverse() * baseMatrix);

        dQuaternion quat (rotation);
        if (quat.m_q0 > dFloat (0.99995f)) {
			//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
			//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

			//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);

        } else {
            dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));

			dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
			dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
			NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
			NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

			//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
			//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
        }
	}
}
void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
	
 	// three rows to restrict rotation around around the parent coordinate system
	dFloat sinAngle;
	dFloat cosAngle;
	CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);

	CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);

	CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);


	// calculate position and speed	
	dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f); 
	dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);  
	if (m_body0) {
		NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	}
	if (m_body1) {
		NewtonBodyGetVelocity(m_body1, &veloc1[0]);
	}
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
	m_speed = (veloc0 - veloc1) % matrix1.m_front;

	// if limit are enable ...
	m_hitLimitOnLastUpdate = false;
	if (m_limitsOn) {
		if (m_posit < m_minDist) {
			// indicate that this row hit a limit
			m_hitLimitOnLastUpdate = true;

			// get a point along the up vector and set a constraint  
			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			
		} else if (m_posit > m_maxDist) {
			// indicate that this row hit a limit
			m_hitLimitOnLastUpdate = true;

			// get a point along the up vector and set a constraint  

			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else {

/*
			// uncomment this for a slider with friction

			// take any point on body0 (origin)
			const dVector& p0 = matrix0.m_posit;

			dVector veloc0; 
			dVector veloc1; 
			dVector omega1; 

			NewtonBodyGetVelocity(m_body0, &veloc0[0]);
			NewtonBodyGetVelocity(m_body1, &veloc1[0]);
			NewtonBodyGetOmega(m_body1, &omega1[0]);

			// this assumes the origin of the bodies the matrix pivot are the same
			veloc1 += omega1 * (matrix1.m_posit - p0);

			dFloat relAccel; 
			relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;

			#define MaxFriction 10.0f
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAccel);
			NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
		}
	} 
 }
void CustomCorkScrew::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
	
	// two rows to restrict rotation around around the parent coordinate system
	dFloat sinAngle;
	dFloat cosAngle;
	CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);

	CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);

	// if limit are enable ...
	if (m_limitsLinearOn) {
		dFloat dist = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front;
		if (dist < m_minLinearDist) {
			// get a point along the up vector and set a constraint  
			NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			
			
		} else if (dist > m_maxLinearDist) {
			// get a point along the up vector and set a constraint  
			NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
		}
	}

	CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
	dFloat angle = -m_curJointAngle.Update (cosAngle, sinAngle);

	if (m_limitsAngularOn) {
		// the joint angle can be determine by getting the angle between any two non parallel vectors
		if (angle < m_minAngularDist) {
			dFloat relAngle = angle - m_minAngularDist;
			// the angle was clipped save the new clip limit
			//m_curJointAngle.m_angle = m_minAngularDist;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);


		} else if (angle  > m_maxAngularDist) {
			dFloat relAngle = angle - m_maxAngularDist;

			// the angle was clipped save the new clip limit
			//m_curJointAngle.m_angle = m_maxAngularDist;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
		}
	}

	if (m_angularmotorOn) {
		dVector omega0 (0.0f, 0.0f, 0.0f);
		dVector omega1 (0.0f, 0.0f, 0.0f);

		// get relative angular velocity
		NewtonBodyGetOmega(m_body0, &omega0[0]);
		if (m_body1) {
			NewtonBodyGetOmega(m_body1, &omega1[0]);
		}

		// calculate the desired acceleration
		dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
		dFloat relAccel = m_angularAccel - m_angularDamp * relOmega;

		// if the motor capability is on, then set angular acceleration with zero angular correction 
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
		
		// override the angular acceleration for this Jacobian to the desired acceleration
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}
 }
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 MSP::BallAndSocket::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) {
    MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint));
    BallAndSocketData* cj_data = reinterpret_cast<BallAndSocketData*>(joint_data->m_cj_data);

    dFloat inv_timestep = 1.0f / timestep;

    // Calculate the position of the pivot point and the Jacobian direction vectors, in global space.
    dMatrix matrix0, matrix1;
    MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1);

    dFloat last_cone_angle = cj_data->m_cur_cone_angle;

    // Calculate current cone angle
    dFloat cur_cone_angle_cos = matrix0.m_right.DotProduct3(matrix1.m_right);
    cj_data->m_cur_cone_angle = dAcos(Util::clamp_float(cur_cone_angle_cos, -1.0f, 1.0f));

    // Calculate current twist angle, omega, and acceleration.
    if (cur_cone_angle_cos < -0.999999f) {
        cj_data->m_cur_twist_omega = 0.0f;
        cj_data->m_cur_twist_alpha = 0.0f;
    }
    else {
        dFloat last_twist_angle = cj_data->m_twist_ai->get_angle();
        dFloat last_twist_omega = cj_data->m_cur_twist_omega;
        dMatrix rot_matrix0;
        Util::rotate_matrix_to_dir(matrix0, matrix1.m_right, rot_matrix0);
        dFloat sin_angle;
        dFloat cos_angle;
        MSP::Joint::c_calculate_angle(matrix1.m_front, rot_matrix0.m_front, matrix1.m_right, sin_angle, cos_angle);
        cj_data->m_twist_ai->update(cos_angle, sin_angle);
        cj_data->m_cur_twist_omega = (cj_data->m_twist_ai->get_angle() - last_twist_angle) * inv_timestep;
        cj_data->m_cur_twist_alpha = (cj_data->m_cur_twist_omega - last_twist_omega) * inv_timestep;
    }

    // Get the current lateral and tangent dir
    dVector lateral_dir;
    dVector front_dir;
    if (dAbs(cur_cone_angle_cos) > 0.999999f) {
        lateral_dir = matrix1.m_front;
        front_dir = matrix1.m_up;
    }
    else {
        lateral_dir = matrix1.m_right.CrossProduct(matrix0.m_right);
        front_dir = matrix1.m_right.CrossProduct(lateral_dir);
    }

    // Restrict the movement on the pivot point along all tree orthonormal directions.
    NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
    NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

    NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
    NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

    NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
    NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

    // Calculate friction
    dFloat power = cj_data->m_friction * dAbs(cj_data->m_controller);

    // Handle cone angle
    if (cj_data->m_cone_limits_enabled && cj_data->m_max_cone_angle < Joint::ANGULAR_LIMIT_EPSILON2) {
        // Handle in case joint being a hinge; max cone angle is near zero.
        NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_front), &matrix1.m_front[0]);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

        NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_up), &matrix1.m_up[0]);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
    }
    else if (cj_data->m_cone_limits_enabled && cj_data->m_cur_cone_angle > cj_data->m_max_cone_angle) {
        // Handle in case current cone angle is greater than max cone angle
        dFloat dangle = cj_data->m_cur_cone_angle - cj_data->m_max_cone_angle;
        NewtonUserJointAddAngularRow(joint, dangle, &lateral_dir[0]);
        NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

        NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]);
        NewtonUserJointSetRowMinimumFriction(joint, -power);
        NewtonUserJointSetRowMaximumFriction(joint, power);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
    }
    else {
        // Handle in case limits are not necessary
        dFloat cur_cone_omega = (cj_data->m_cur_cone_angle - last_cone_angle) * inv_timestep;
        dFloat des_cone_accel = -cur_cone_omega * inv_timestep;

        NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]);
        NewtonUserJointSetRowAcceleration(joint, des_cone_accel);
        NewtonUserJointSetRowMinimumFriction(joint, -power);
        NewtonUserJointSetRowMaximumFriction(joint, power);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);

        NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]);
        NewtonUserJointSetRowMinimumFriction(joint, -power);
        NewtonUserJointSetRowMaximumFriction(joint, power);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
    }

    // Handle twist angle
    bool bcontinue = false;
    if (cj_data->m_twist_limits_enabled) {
        if (cj_data->m_min_twist_angle > cj_data->m_max_twist_angle) {
            // Handle in case min angle is greater than max
            NewtonUserJointAddAngularRow(joint, (cj_data->m_min_twist_angle + cj_data->m_max_twist_angle) * 0.5f - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]);
            NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
        }
        else if (cj_data->m_max_twist_angle - cj_data->m_min_twist_angle < Joint::ANGULAR_LIMIT_EPSILON2) {
            // Handle in case min angle is almost equal to max
            NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]);
            NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
        }
        else if (cj_data->m_twist_ai->get_angle() < cj_data->m_min_twist_angle) {
            // Handle in case current twist angle is less than min
            NewtonUserJointAddAngularRow(joint, cj_data->m_min_twist_angle - cj_data->m_twist_ai->get_angle() + Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]);
            NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
            NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
        }
        else if (cj_data->m_twist_ai->get_angle() > cj_data->m_max_twist_angle) {
            // Handle in case current twist angle is greater than max
            NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle() - Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]);
            NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
            NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
        }
        else
            bcontinue = true;
    }
    else
        bcontinue = true;
    if (bcontinue) {
        // Handle in case limits are not necessary
        NewtonUserJointAddAngularRow(joint, 0.0f, &matrix0.m_right[0]);
        NewtonUserJointSetRowAcceleration(joint, -cj_data->m_cur_twist_omega * inv_timestep);
        NewtonUserJointSetRowMinimumFriction(joint, -power);
        NewtonUserJointSetRowMaximumFriction(joint, power);
        NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness);
    }
}
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dFloat angle;
	dFloat sinAngle;
	dFloat cosAngle;
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

	// get the pin fixed to the first body
	const dVector& dir0 = matrix0.m_front;
	// get the pin fixed to the second body
	const dVector& dir1 = matrix1.m_up;

	// construct an orthogonal coordinate system with these two vectors
	dVector dir2 (dir0 * dir1);
	dir2 = dir2.Scale (1.0f / dSqrt (dir2 % dir2));

	const dVector& p0 = matrix0.m_posit;
	const dVector& p1 = matrix1.m_posit;
	NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir0[0]);
	NewtonUserJointSetRowStiffness (m_joint, 1.0f);
	NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir1[0]);
	NewtonUserJointSetRowStiffness (m_joint, 1.0f);
	NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &dir2[0]);
	NewtonUserJointSetRowStiffness (m_joint, 1.0f);


	dVector dir3 (dir2 * dir0);
	dVector q0 (p0 + dir3.Scale(MIN_JOINT_PIN_LENGTH));
	dVector q1 (p1 + dir1.Scale(MIN_JOINT_PIN_LENGTH));
	NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &dir0[0]);
	NewtonUserJointSetRowStiffness (m_joint, 1.0f);


	// check is the joint limit are enable
	if (m_limit_0_On) {
		sinAngle = (matrix0.m_up * matrix1.m_up) % matrix0.m_front;
		cosAngle = matrix0.m_up % matrix1.m_up;
		angle = dAtan2 (sinAngle, cosAngle);

		if (angle < m_minAngle_0) {
			dFloat relAngle;
			relAngle = angle - m_minAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle > m_maxAngle_0) {
			dFloat relAngle;
			relAngle = angle - m_maxAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
		}

		// check is the joint limit motor is enable
	} else if (m_angularMotor_0_On) {

		dFloat relOmega;
		dFloat relAccel;
		dVector omega0 (0.0f, 0.0f, 0.0f);
		dVector omega1 (0.0f, 0.0f, 0.0f);

		// get relative angular velocity
		NewtonBodyGetOmega(m_body0, &omega0[0]);
		if (m_body1) {
			NewtonBodyGetOmega(m_body1, &omega1[0]);
		}

		// calculate the desired acceleration
		relOmega = (omega0 - omega1) % matrix0.m_front;
		relAccel = m_angularAccel_0 - m_angularDamp_0 * relOmega;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);

		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}


	// if limit are enable ...
	if (m_limit_1_On) {
		sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up;
		cosAngle = matrix0.m_front % matrix1.m_front;
		angle = dAtan2 (sinAngle, cosAngle);
 
		if (angle < m_minAngle_1) {
			dFloat relAngle;
			relAngle = angle - m_minAngle_1;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle > m_maxAngle_1) {
			dFloat relAngle;
			relAngle = angle - m_maxAngle_1;
			
			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
  		}
	} else if (m_angularMotor_1_On) {
		dFloat relOmega;
		dFloat relAccel;
		dVector omega0 (0.0f, 0.0f, 0.0f);
		dVector omega1 (0.0f, 0.0f, 0.0f);

		// get relative angular velocity
		NewtonBodyGetOmega(m_body0, &omega0[0]);
		if (m_body1) {
			NewtonBodyGetOmega(m_body1, &omega1[0]);
		}

		// calculate the desired acceleration
		relOmega = (omega0 - omega1) % matrix1.m_up;
		relAccel = m_angularAccel_1 - m_angularDamp_1 * relOmega;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
		
		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}
 }
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{

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

	// get the vehicle global matrix, and use it in several calculations
	dMatrix bodyMatrix;  
	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 bodyForce;
	dVector bodyOmega;
	dVector bodyVelocity;


	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

//static int xxx;
//dTrace (("frame %d veloc(%f %f %f)\n", xxx, bodyVelocity[0], bodyVelocity[1], bodyVelocity[2]));
//xxx ++;
//if (xxx >= 210) {
//xxx *=1;
//bodyVelocity.m_x = 0;
//bodyVelocity.m_z = 10;
//NewtonBodySetVelocity (m_body0, &bodyVelocity[0]);
//}

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

//		dTrace (("tire: %d ", 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_tireAxelPositGlobal = chassisMatrix.TransformVector (tire.m_harpointInJointSpace - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVelocGlobal = bodyVelocity + bodyOmega * (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit); 
		tire.m_lateralPinGlobal = chassisMatrix.RotateVector (tire.m_localAxisInJointSpace);
		tire.m_longitudinalPinGlobal = chassisMatrix.m_up * tire.m_lateralPinGlobal;

		if (tire.m_posit < tire.m_suspensionLenght )  {

			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 tireHubVeloc (tire.m_tireAxelVelocGlobal - tire.m_hitBodyPointVelocity);
			dFloat suspensionSpeed = - (tireHubVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			dFloat 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);

//			dTrace (("(load = %f) ", tire.m_tireLoad));


			//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;
			dFloat axelLinearSpeed = tireHubVeloc % chassisMatrix.m_front;

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


			// calculate slip ratio and max longitudinal force
			//dFloat tireRotationSpeed = -(tireRotationalVelocityAtContact % tire.m_longitudinalPinGlobal);
			//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;

			//dTrace (("(slipRatio = %f) ", slipRatioCoef));

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

			dFloat longitudinalForceMag = CalculateLongitudinalForce (i, axelLinearSpeed, tire.m_tireLoad * tire.m_groundFriction);

//			dTrace (("(longForce = %f) ", longitudinalForceMag));

#if 0

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

			// calculate the side slip as the angle between the tire lateral speed and longitudinal speed 
			//dFloat lateralSpeed = tireContactRelativeVelocity % tire.m_lateralPin;
			dFloat lateralSpeed = tireHubVeloc % tire.m_lateralPinGlobal;

			dFloat sideSlipCoef = dAtan2 (dAbs (lateralSpeed), dAbs (axelLinearSpeed));
			dFloat 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 ) {
				_ASSERTE (0);
/*
				// 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; 
			dFloat frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;

			dFloat 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_tireAxelPositGlobal[0], &tire.m_tireAxelPositGlobal[0], &tire.m_lateralPinGlobal[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
#endif

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

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector r (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);

			// add the toque the tire asserts on the car body (principle of action reaction)
			dVector torque (r * tire.m_tireForceAcc - tire.m_lateralPinGlobal.Scale (tire.m_torque));
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );
/*
			// calculate the net torque on the tire
			dFloat tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPinGlobal);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force cannot 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;
*/
//			normalForces += tire.m_tireForceAcc;

		} else {

			// there is a next torque on the tire
			tire.m_torque -= tire.m_angularVelocity * tire.m_Ixx * DG_TIRE_VISCUOS_DAMP;
			tire.m_angularVelocity += tire.m_torque * tire.m_IxxInv * timestep;
			if (m_tires[i].m_breakForce > dFloat (0.1f)) {
				tire.m_angularVelocity = 0.0f;
			}
		}

//		dTrace (("(tireTorque = %f) ", tire.m_torque));

		// spin the tire by the angular velocity
		tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);

		// reset the tire torque
		tire.m_torque = 0.0f;
		tire.m_breakForce = 0.0f;  

//		dTrace (("\n"));

	}


	// add a row to simulate the engine rolling resistance
//	float bodyWeight = dAbs (normalForces % chassisMatrix.m_up) * m_rollingResistance;
//	if (bodyWeight > (1.0e-3f) * m_mass) {
//		NewtonUserJointAddLinearRow (m_joint, &chassisMatrix.m_posit[0], &chassisMatrix.m_posit[0], &chassisMatrix.m_front[0]);
//		NewtonUserJointSetRowMaximumFriction( m_joint,  bodyWeight);
//		NewtonUserJointSetRowMinimumFriction( m_joint, -bodyWeight);
//	}
}
void MSNewton::Slider::submit_constraints(const NewtonJoint* joint, dgFloat32 timestep, int thread_index) {
	JointData* joint_data = (JointData*)NewtonJointGetUserData(joint);
	SliderData* cj_data = (SliderData*)joint_data->cj_data;

	// Calculate position of pivot points and Jacobian direction vectors in global space.
	dMatrix matrix0, matrix1, matrix2;
	MSNewton::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1, matrix2);

	const dVector& pos0 = matrix0.m_posit;
	dVector pos1(matrix1.m_posit + matrix1.m_right.Scale((pos0 - matrix1.m_posit) % matrix1.m_right));

	// Calculate position, velocity, and acceleration
	dFloat last_pos = cj_data->cur_pos;
	dFloat last_vel = cj_data->cur_vel;
	cj_data->cur_pos = matrix1.UntransformVector(matrix0.m_posit).m_z;
	cj_data->cur_vel = (cj_data->cur_pos - last_pos) / timestep;
	cj_data->cur_accel = (cj_data->cur_vel - last_vel) / timestep;

	// Restrict movement on axis perpendicular to the pin direction.
	NewtonUserJointAddLinearRow(joint, &pos0[0], &pos1[0], &matrix0.m_front[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	NewtonUserJointAddLinearRow(joint, &pos0[0], &pos1[0], &matrix0.m_up[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	// Add three angular rows to restrict rotation around all axis.
	/*NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_front), &matrix0.m_front[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_right, matrix1.m_right, matrix0.m_up), &matrix0.m_up[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	NewtonUserJointAddAngularRow(joint, Joint::c_calculate_angle(matrix0.m_front, matrix1.m_front, matrix0.m_right), &matrix0.m_right[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);*/

	// Get a point along the ping axis at some reasonable large distance from the pivot
	dVector q0(pos0 + matrix0.m_right.Scale(MIN_JOINT_PIN_LENGTH));
	dVector q1(pos1 + matrix1.m_right.Scale(MIN_JOINT_PIN_LENGTH));

	// Add two constraints row perpendicular to the pin
	NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_front[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	NewtonUserJointAddLinearRow(joint, &q0[0], &q1[0], &matrix0.m_up[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	// Get a point along the ping axis at some reasonable large distance from the pivot
	dVector r0(pos0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
	dVector r1(pos1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));

	// Add one constraint row perpendicular to the pin
	NewtonUserJointAddLinearRow(joint, &r0[0], &r1[0], &matrix0.m_up[0]);
	if (joint_data->ctype == CT_FLEXIBLE)
		NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::ANGULAR_STIFF, Joint::ANGULAR_DAMP);
	else if (joint_data->ctype == CT_ROBUST)
		NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
	NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);

	// Add limits and friction
	if (cj_data->limits_enabled == true && cj_data->cur_pos < cj_data->min - Joint::LINEAR_LIMIT_EPSILON) {
		const dVector& s0 = matrix0.m_posit;
		dVector s1(s0 + matrix1.m_right.Scale(cj_data->min - cj_data->cur_pos));
		NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]);
		NewtonUserJointSetRowMinimumFriction(joint, 0.0f);
		if (joint_data->ctype == CT_FLEXIBLE)
			NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
		else if (joint_data->ctype == CT_ROBUST)
			NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
		NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
	}
	else if (cj_data->limits_enabled == true && cj_data->cur_pos > cj_data->max + Joint::LINEAR_LIMIT_EPSILON) {
		const dVector& s0 = matrix0.m_posit;
		dVector s1(s0 + matrix1.m_right.Scale(cj_data->max - cj_data->cur_pos));
		NewtonUserJointAddLinearRow(joint, &s0[0], &s1[0], &matrix1.m_right[0]);
		NewtonUserJointSetRowMaximumFriction(joint, 0.0f);
		if (joint_data->ctype == CT_FLEXIBLE)
			NewtonUserJointSetRowSpringDamperAcceleration(joint, Joint::LINEAR_STIFF, Joint::LINEAR_DAMP);
		else if (joint_data->ctype == CT_ROBUST)
			NewtonUserJointSetRowAcceleration(joint, NewtonUserCalculateRowZeroAccelaration(joint));
		NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
	}
	else {
		dVector point(matrix1.UntransformVector(matrix0.m_posit));
		point.m_z = 0.0f;
		point = matrix1.TransformVector(point);
		NewtonUserJointAddLinearRow(joint, &point[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
		dFloat power = cj_data->friction * cj_data->controller;
		/*BodyData* cbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child);
		if (cbody_data->bstatic == false && cbody_data->mass >= MIN_MASS)
			power *= cbody_data->mass;
		else {
			BodyData* pbody_data = (BodyData*)NewtonBodyGetUserData(joint_data->child);
			if (pbody_data->bstatic == false && pbody_data->mass >= MIN_MASS) power *= pbody_data->mass;
		}*/
		NewtonUserJointSetRowMinimumFriction(joint, -power);
		NewtonUserJointSetRowMaximumFriction(joint, power);
		NewtonUserJointSetRowStiffness(joint, joint_data->stiffness);
	}
}
void CustomHinge::SubmitConstraintsFreeDof(dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
	// four possibilities
	dFloat angle = m_curJointAngle.GetAngle();
	if (m_friction != 0.0f) {
		if (m_limitsOn) {
			// friction and limits at the same time
			if (angle < m_minAngle) {
				dFloat relAngle = angle - m_minAngle;

				// tell joint error will minimize the exceeded angle error
				NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);

				// need high stiffness here
				NewtonUserJointSetRowStiffness(m_joint, 1.0f);

				// allow the joint to move back freely 
				NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);

				m_lastRowWasUsed = true;
			} else if (angle > m_maxAngle) {
				dFloat relAngle = angle - m_maxAngle;

				// tell joint error will minimize the exceeded angle error
				NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);

				// need high stiffness here
				NewtonUserJointSetRowStiffness(m_joint, 1.0f);

				// allow the joint to move back freely
				NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);

				m_lastRowWasUsed = true;
			} else {
				// friction but not limits
				dFloat alpha = m_jointOmega / timestep;
				NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
				NewtonUserJointSetRowAcceleration(m_joint, -alpha);
				NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
				NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
				NewtonUserJointSetRowStiffness(m_joint, 1.0f);
				m_lastRowWasUsed = true;
			}
		} else {
			// friction but not limits
			dFloat alpha = m_jointOmega / timestep;
			NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
			NewtonUserJointSetRowAcceleration(m_joint, -alpha);
			NewtonUserJointSetRowMinimumFriction(m_joint, -m_friction);
			NewtonUserJointSetRowMaximumFriction(m_joint, m_friction);
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);

			m_lastRowWasUsed = true;
		}
	} else if (m_limitsOn) {
		// only limit are on 
		// the joint angle can be determine by getting the angle between any two non parallel vectors
		if ((m_minAngle > -1.e-4f) && (m_maxAngle < 1.e-4f)) {
			NewtonUserJointAddAngularRow(m_joint, -angle, &matrix1.m_front[0]);
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);
			m_lastRowWasUsed = true;

		} else if (angle < m_minAngle) {
			dFloat relAngle = angle - m_minAngle;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
			m_lastRowWasUsed = true;
		} else if (angle > m_maxAngle) {
			dFloat relAngle = angle - m_maxAngle;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow(m_joint, -relAngle, &matrix1.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
			m_lastRowWasUsed = true;
		}
	}
}
void CustomSlidingContact::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dFloat sinAngle;
	dFloat cosAngle;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
	dVector p0(matrix0.m_posit);
	dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
	NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);

	// construct an orthogonal coordinate system with these two vectors
	dMatrix matrix1_1;
	matrix1_1.m_up = matrix1.m_up;
	matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
	matrix1_1.m_right = matrix1_1.m_right.Scale(1.0f / dSqrt(matrix1_1.m_right % matrix1_1.m_right));
	matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
	NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_front), &matrix1_1.m_front[0]);
	NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_right), &matrix1_1.m_right[0]);

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	CalculateAngle(matrix1_1.m_front, matrix1.m_front, matrix1.m_up, sinAngle, cosAngle);
	m_curJointAngle.Update(cosAngle, sinAngle);

	dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
	dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
	dAssert(m_body0);
	NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
	if (m_body1) {
		NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
	}
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
	m_speed = (veloc0 - veloc1) % matrix1.m_front;
	
	// if limit are enable ...
	if (m_limitsLinearOn) {
		if (m_posit < m_minLinearDist) {
			// get a point along the up vector and set a constraint  
			dVector p (matrix1.m_posit + matrix1.m_front.Scale(m_minLinearDist));
			NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
		} else if (m_posit > m_maxLinearDist) {
			dVector p(matrix1.m_posit + matrix1.m_front.Scale(m_maxLinearDist));
			NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
		}
	}

	if (m_limitsAngularOn) {
		dFloat angle1 = m_curJointAngle.GetAngle();
		if (angle1 < m_minAngularDist) {
			dFloat relAngle = angle1 - m_minAngularDist;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);

		}
		else if (angle1 > m_maxAngularDist) {
			dFloat relAngle = angle1 - m_maxAngularDist;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness(m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
		}
	}
}
void CustomUniversal::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// Restrict the movement on the pivot point along all tree orthonormal direction
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);


	// construct an orthogonal coordinate system with these two vectors
	dMatrix matrix1_1;
	matrix1_1.m_up = matrix1.m_up;
	matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
	matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
	matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
	NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);

	dFloat sinAngle_0;
	dFloat cosAngle_0;
	CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
	dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);

	dFloat sinAngle_1;
	dFloat cosAngle_1;
	CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
	dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);

	dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
	dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}

	// calculate the desired acceleration
	dVector relOmega (omega0 - omega1);
	m_jointOmega_0 = relOmega % matrix0.m_front;
	m_jointOmega_1 = relOmega % matrix1.m_up;
	
	// check is the joint limit are enable
	if (m_limit_0_On) {
		if (angle0 < m_minAngle_0) {
			dFloat relAngle = angle0 - m_minAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle0 > m_maxAngle_0) {
			dFloat relAngle = angle0 - m_maxAngle_0;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
		}

		// check is the joint limit motor is enable
	} else if (m_angularMotor_0_On) {
		// calculate the desired acceleration
//		dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
		dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);

		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}

	// if limit are enable ...
	if (m_limit_1_On) {
		if (angle1 < m_minAngle_1) {
			dFloat relAngle = angle1 - m_minAngle_1;

			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffeners here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely 
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else if (angle1 > m_maxAngle_1) {
			dFloat relAngle = angle1 - m_maxAngle_1;
			
			// tell joint error will minimize the exceeded angle error
			NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);

			// need high stiffness here
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);

			// allow the joint to move back freely
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
  		}
	} else if (m_angularMotor_1_On) {
		// calculate the desired acceleration
		dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;

		// add and angular constraint row to that will set the relative acceleration to zero
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
		
		// override the joint acceleration.
		NewtonUserJointSetRowAcceleration (m_joint, relAccel);
	}
}
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 CustomLimitBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix(matrix0, matrix1);

	const dVector& p0 = matrix0.m_posit;
	const dVector& p1 = matrix1.m_posit;

	// Restrict the movement on the pivot point along all tree orthonormal direction
	NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
	NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);

	matrix1 = m_rotationOffset * matrix1;

	// handle special case of the joint being a hinge
	if (m_coneAngleCos > 0.9999f) {
		NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
		NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);

		// the joint angle can be determined by getting the angle between any two non parallel vectors
		dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
		if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
			NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
		} else {
			if (pitchAngle > m_maxTwistAngle) {
				pitchAngle -= m_maxTwistAngle;
				NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
				NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
			} else if (pitchAngle < m_minTwistAngle) {
				pitchAngle -= m_minTwistAngle;
				NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
				NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
			}
		}

	} else {

		const dVector& coneDir0 = matrix0.m_front;
		const dVector& coneDir1 = matrix1.m_front;
		dFloat cosAngle = coneDir0 % coneDir1;
		if (cosAngle <= m_coneAngleCos) {
			dVector lateralDir(coneDir0 * coneDir1);
			dFloat mag2 = lateralDir % lateralDir;
			dAssert(mag2 > 1.0e-4f);
			lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));

			dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
			dVector frontDir(rot.UnrotateVector(coneDir1));
			dVector upDir(lateralDir * frontDir);
			NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
			NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
			NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
		}

		//handle twist angle
		dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
		if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
			NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
		} else {
			if (pitchAngle > m_maxTwistAngle) {
				pitchAngle -= m_maxTwistAngle;
				NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
				NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
			} else if (pitchAngle < m_minTwistAngle) {
				pitchAngle -= m_minTwistAngle;
				NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
				NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
			}
		}
	}
}