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, ¢erInChassis[0], ¢erInTire[0], &chassisPivotMatrix.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, ¢erInChassis[0], ¢erInTire[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, ¢erInChassis[0], ¢erInChassis[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, ¢erInChassis[0], ¢erInChassis[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;
}
