void dVehicleChassis::CalculateSuspensionForces(dFloat timestep)
{
	const int maxSize = 64;
	dComplementaritySolver::dJacobianPair m_jt[maxSize];
	dComplementaritySolver::dJacobianPair m_jInvMass[maxSize];
	dVehicleVirtualTire* tires[maxSize];
	dFloat massMatrix[maxSize * maxSize];
	dFloat accel[maxSize];
	
	dComplementaritySolver::dBodyState* const chassisBody = m_vehicle->GetBody();

	const dMatrix& chassisMatrix = chassisBody->GetMatrix(); 
	const dMatrix& chassisInvInertia = chassisBody->GetInvInertia();
	dVector chassisOrigin (chassisMatrix.TransformVector (chassisBody->GetCOM()));
	dFloat chassisInvMass = chassisBody->GetInvMass();

	int tireCount = 0;
	const dList<dVehicleNode*>& children = m_vehicle->GetChildren();
	for (dList<dVehicleNode*>::dListNode* tireNode = children.GetFirst(); tireNode; tireNode = tireNode->GetNext()) {
		dVehicleVirtualTire* const tire = (dVehicleVirtualTire*) tireNode->GetInfo()->GetAsTire();
		if (tire) {
			const dVehicleVirtualTire::dTireInfo& info = tire->m_info;
			tires[tireCount] = tire;
			dFloat x = tire->m_position;
			dFloat v = tire->m_speed;
			dFloat weight = 1.0f;
/*
			switch (tire->m_suspentionType)
			{
				case m_offroad:
					weight = 0.9f;
					break;
				case m_confort:
					weight = 1.0f;
					break;
				case m_race:
					weight = 1.1f;
					break;
			}
*/
//x = 0.1f;
//v = 10.0f;
			dComplementaritySolver::dBodyState* const tireBody = tire->GetBody();

			const dFloat invMass = tireBody->GetInvMass();
			const dFloat kv = info.m_dampingRatio * invMass;
			const dFloat ks = info.m_springStiffness * invMass;
			accel[tireCount] = -NewtonCalculateSpringDamperAcceleration(timestep, ks * weight, x, kv, v);

			const dMatrix& tireMatrix = tireBody->GetMatrix(); 
			const dMatrix& tireInvInertia = tireBody->GetInvInertia();
			dFloat tireMass = tireBody->GetInvMass();

			m_jt[tireCount].m_jacobian_J01.m_linear = chassisMatrix.m_up.Scale(-1.0f);
			m_jt[tireCount].m_jacobian_J01.m_angular = dVector(0.0f);
			m_jt[tireCount].m_jacobian_J10.m_linear = chassisMatrix.m_up;
			m_jt[tireCount].m_jacobian_J10.m_angular = (tireMatrix.m_posit - chassisOrigin).CrossProduct(chassisMatrix.m_up);

			m_jInvMass[tireCount].m_jacobian_J01.m_linear = m_jt[tireCount].m_jacobian_J01.m_linear.Scale(tireMass);
			m_jInvMass[tireCount].m_jacobian_J01.m_angular = tireInvInertia.RotateVector(m_jt[tireCount].m_jacobian_J01.m_angular);
			m_jInvMass[tireCount].m_jacobian_J10.m_linear = m_jt[tireCount].m_jacobian_J10.m_linear.Scale(chassisInvMass);
			m_jInvMass[tireCount].m_jacobian_J10.m_angular = chassisInvInertia.RotateVector(m_jt[tireCount].m_jacobian_J10.m_angular);

			tireCount++;
		}
	}

	for (int i = 0; i < tireCount; i++) {
		dFloat* const row = &massMatrix[i * tireCount];

		dFloat aii = m_jInvMass[i].m_jacobian_J01.m_linear.DotProduct3(m_jt[i].m_jacobian_J01.m_linear) + m_jInvMass[i].m_jacobian_J01.m_angular.DotProduct3(m_jt[i].m_jacobian_J01.m_angular) +
					 m_jInvMass[i].m_jacobian_J10.m_linear.DotProduct3(m_jt[i].m_jacobian_J10.m_linear) + m_jInvMass[i].m_jacobian_J10.m_angular.DotProduct3(m_jt[i].m_jacobian_J10.m_angular);

		row[i] = aii * 1.0001f;
		for (int j = i + 1; j < tireCount; j++) {
			dFloat aij = m_jInvMass[i].m_jacobian_J10.m_linear.DotProduct3(m_jt[j].m_jacobian_J10.m_linear) + m_jInvMass[i].m_jacobian_J10.m_angular.DotProduct3(m_jt[j].m_jacobian_J10.m_angular);
			row[j] = aij;
			massMatrix[j * tireCount + i] = aij;
		}
	}

	dCholeskyFactorization(tireCount, massMatrix);
	dCholeskySolve(tireCount, tireCount, massMatrix, accel);

	dVector chassisForce(0.0f);
	dVector chassisTorque(0.0f);
	for (int i = 0; i < tireCount; i++) {
		dVehicleVirtualTire* const tire = tires[i];
		dComplementaritySolver::dBodyState* const tireBody = tire->GetBody();

		tires[i]->m_tireLoad = dMax(dFloat(1.0f), accel[i]);
		dVector tireForce(m_jt[i].m_jacobian_J01.m_linear.Scale(accel[i]));

		tireBody->SetForce(tireBody->GetForce() + tireForce);
		chassisForce += m_jt[i].m_jacobian_J10.m_linear.Scale(accel[i]);
		chassisTorque += m_jt[i].m_jacobian_J10.m_angular.Scale(accel[i]);
	}
	chassisBody->SetForce(chassisBody->GetForce() + chassisForce);
	chassisBody->SetTorque(chassisBody->GetTorque() + chassisTorque);
}
    void SubmitConstraints (dFloat timestep, int threadIndex)
    {
        dMatrix tirePivotMatrix;
        dMatrix chassisPivotMatrix;

        ProjectTireMatrix();

        // calculate the position of the pivot point and the Jacobian direction vectors, in global space.
        CalculateGlobalMatrix (m_chassisLocalMatrix, m_tireLocalMatrix, chassisPivotMatrix, tirePivotMatrix);

        // Restrict the movement on the pivot point along all two orthonormal direction
        dVector centerInTire (tirePivotMatrix.m_posit);
        dVector centerInChassis (chassisPivotMatrix.m_posit + chassisPivotMatrix.m_up.Scale ((centerInTire - chassisPivotMatrix.m_posit) % chassisPivotMatrix.m_up));
        NewtonUserJointAddLinearRow (m_joint, &centerInChassis[0], &centerInTire[0], &chassisPivotMatrix.m_front[0]);
        NewtonUserJointAddLinearRow (m_joint, &centerInChassis[0], &centerInTire[0], &chassisPivotMatrix.m_right[0]);

        // get a point along the pin axis at some reasonable large distance from the pivot
        dVector pointInPinInTire (centerInChassis + chassisPivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH));
        dVector pointInPinInChassis (centerInTire + tirePivotMatrix.m_front.Scale(MIN_JOINT_PIN_LENGTH));
        NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_right[0]);
        NewtonUserJointAddLinearRow (m_joint, &pointInPinInTire[0], &pointInPinInChassis[0], &chassisPivotMatrix.m_up[0]);

        //calculate the suspension spring and damper force
        dFloat dist;
        dFloat speed;
        dFloat force;

        dVector tireVeloc;
        dVector chassisVeloc;
        dVector chassisOmega;
        dVector chassisCom;
        dMatrix chassisMatrix;

        const NewtonBody* tire;
        const NewtonBody* chassis;

        // calculate the velocity of tire attachments point on the car chassis

        tire = GetBody1();
        chassis = GetBody0();

        NewtonBodyGetVelocity (tire, &tireVeloc[0]);
        NewtonBodyGetVelocity (chassis, &chassisVeloc[0]);
        NewtonBodyGetOmega (chassis, &chassisOmega[0]);
        NewtonBodyGetMatrix (chassis, &chassisMatrix[0][0]);
        NewtonBodyGetCentreOfMass (chassis, &chassisCom[0]);
        chassisCom = chassisMatrix.TransformVector(chassisCom);
        chassisVeloc += chassisOmega * (centerInChassis - chassisCom);


        // get the spring damper parameters
        speed = (chassisVeloc - tireVeloc) % chassisPivotMatrix.m_up;
        dist = (chassisPivotMatrix.m_posit - tirePivotMatrix.m_posit) % chassisPivotMatrix.m_up;
        // check if the suspension pass the bumpers limits
        if (-dist > m_suspenstionSpan* 0.5f) {
            // if it hit the bumpers then speed is zero
            speed = 0;
            NewtonUserJointAddLinearRow (m_joint, &centerInChassis[0], &centerInChassis[0], &chassisPivotMatrix.m_up[0]);
            NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
        } else if (dist > 0.0f) {
            // if it hit the bumpers then speed is zero
            speed = 0;
            NewtonUserJointAddLinearRow (m_joint, &centerInChassis[0], &centerInChassis[0], &chassisPivotMatrix.m_up[0]);
            NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
        }

        // calculate magnitude of suspension force
        force = NewtonCalculateSpringDamperAcceleration (timestep, m_spring, dist, m_damper, speed) * m_effectiveSpringMass;

        dVector chassisForce (chassisMatrix.m_up.Scale (force));
        dVector chassisTorque ((centerInChassis - chassisCom) * chassisForce);
        NewtonBodyAddForce (chassis, &chassisForce[0]);
        NewtonBodyAddTorque (chassis, &chassisTorque[0]);

        dVector tireForce (chassisForce.Scale (-1.0f));
        NewtonBodyAddForce(tire, &tireForce[0]);

        // apply the engine torque to tire torque
        dFloat relOmega;
        dMatrix tireMatrix;
        dVector tireOmega;

        NewtonBodyGetOmega(tire, &tireOmega[0]);
        NewtonBodyGetMatrix (tire, &tireMatrix[0][0]);

        relOmega = ((tireOmega - chassisOmega) % tireMatrix.m_front);

        // apply engine torque plus some tire angular drag
        dVector tireTorque (tireMatrix.m_front.Scale (m_enginetorque - relOmega * m_Ixx * m_angularDragCoef));
        NewtonBodyAddTorque (tire, &tireTorque[0]);

        dVector chassisReationTorque (chassisMatrix.m_right.Scale (- m_enginetorque));
        NewtonBodyAddTorque(chassis, &chassisTorque[0]);

        m_enginetorque = 0.0f;

        // add the brake torque row
        if (dAbs(m_brakeToque) > 1.0e-3f) {

            relOmega /= timestep;
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &tireMatrix.m_front[0]);
            NewtonUserJointSetRowAcceleration(m_joint, relOmega);
            NewtonUserJointSetRowMaximumFriction(m_joint, m_brakeToque);
            NewtonUserJointSetRowMinimumFriction(m_joint, -m_brakeToque);
        }
        m_brakeToque = 0.0f;

    }