void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex)
{
NewtonBody* BodyAttach;
//NewtonBody* BodyFrame;
//
dVector tireOmega = dVector(0.0f, 0.0f, 0.0f);
//BodyFrame = GetBody0();
BodyAttach = GetBody1();
//
SteeringController(timestep);
//
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix);
//
NewtonBodyGetOmega(BodyAttach, &tireOmega[0]);
//
mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front));
//
TireCenterPin(timestep);
//
TireCenterBolt(timestep);
//
SuspenssionSpringLimits(timestep);
//
TireBreakAction(BodyAttach, timestep);
}
void 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);
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit);
dVector dir(p1 - p0);
dFloat mag2 = dir % dir;
dir = dir.Scale(1.0f / dSqrt(mag2));
dMatrix matrix(dGrammSchmidt(dir));
dFloat x = dSqrt(mag2) - m_distance;
dVector com0;
dVector com1;
dVector veloc0;
dVector veloc1;
dMatrix body0Matrix;
dMatrix body1Matrix;
NewtonBody* const body0 = GetBody0();
NewtonBody* const body1 = GetBody1();
NewtonBodyGetCentreOfMass(body0, &com0[0]);
NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]);
NewtonBodyGetCentreOfMass(body1, &com1[0]);
NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]);
dFloat v((veloc0 - veloc1) % dir);
dFloat a = (x - v * timestep) / (timestep * timestep);
dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
dFloat jacobian0[6];
dFloat jacobian1[6];
jacobian0[0] = matrix[0][0];
jacobian0[1] = matrix[0][1];
jacobian0[2] = matrix[0][2];
jacobian0[3] = r0[0];
jacobian0[4] = r0[1];
jacobian0[5] = r0[2];
jacobian1[0] = -matrix[0][0];
jacobian1[1] = -matrix[0][1];
jacobian1[2] = -matrix[0][2];
jacobian1[3] = -r1[0];
jacobian1[4] = -r1[1];
jacobian1[5] = -r1[2];
NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration(m_joint, a);
}
void dgConstraint::InitInfo (dgConstraintInfo* const info) const
{
info->m_attachBody_0 = GetBody0();
dgAssert (info->m_attachBody_0);
dgWorld* const world = info->m_attachBody_0->GetWorld();
if (info->m_attachBody_0 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_0 = NULL;
}
info->m_attachBody_1 = GetBody1();
if (info->m_attachBody_1 == (dgBody*)world->GetSentinelBody()) {
info->m_attachBody_1 = NULL;
}
info->m_attachMatrix_0 = dgGetIdentityMatrix();
info->m_attachMatrix_1 = dgGetIdentityMatrix();
info->m_discriptionType[0] = 0;
}
void dCustomTireSpringDG::TireMatrixProjection()
{
NewtonBody* tire;
NewtonBody* chassis;
//
dMatrix tireMatrix;
dMatrix chassisMatrix;
dMatrix tireMatrixInGlobalSpace;
dMatrix chassisMatrixInGlobalSpace;
dMatrix projectedChildMatrixInGlobalSpace;
//
dVector projectDist;
dVector projTireOmega;
dVector chassisOmega;
dVector tireOmega;
dVector tireOmegaCorrection;
//
// D.G: Thanks Julio for the helps in the past for this part of code.
// D.G: This code come from a pretty old vehicle implementation.
tire = GetBody1();
chassis = GetBody0();
//
NewtonBodyGetMatrix(tire, &tireMatrix[0][0]);
NewtonBodyGetMatrix(chassis, &chassisMatrix[0][0]);
//
// project the tire matrix to the right space.
tireMatrixInGlobalSpace = (m_localMatrix1 * tireMatrix);
chassisMatrixInGlobalSpace = (m_localMatrix0 * chassisMatrix);
//
projectDist = (tireMatrixInGlobalSpace.m_posit - chassisMatrixInGlobalSpace.m_posit).DotProduct3(chassisMatrixInGlobalSpace.m_up);
chassisMatrixInGlobalSpace.m_posit = chassisMatrixInGlobalSpace.m_posit + (chassisMatrixInGlobalSpace.m_up * projectDist);
//
chassisMatrixInGlobalSpace.m_up = tireMatrixInGlobalSpace.m_right.CrossProduct(chassisMatrixInGlobalSpace.m_front);
chassisMatrixInGlobalSpace.m_up = chassisMatrixInGlobalSpace.m_up * (1.0f / dSqrt(chassisMatrixInGlobalSpace.m_up.DotProduct3(chassisMatrixInGlobalSpace.m_up)));
chassisMatrixInGlobalSpace.m_right = chassisMatrixInGlobalSpace.m_front.CrossProduct(chassisMatrixInGlobalSpace.m_up);
chassisMatrixInGlobalSpace.m_up.m_w = 0.0;
chassisMatrixInGlobalSpace.m_right.m_w = 0.0;
//
projectedChildMatrixInGlobalSpace = (m_localMatrix1.Inverse() * chassisMatrixInGlobalSpace);
NewtonBodySetMatrix(tire, &projectedChildMatrixInGlobalSpace[0][0]);
//
NewtonBodyGetOmega(chassis, &chassisOmega[0]);
// D.G: Temporary disabled.
// D.G: The result is a lot better without this part.
// D.G: Causing problems, something is calculed wrong and the result give some bad bounce force on the wheels.
//
/*
NewtonBodyGetCentreOfMass(chassis, @chassisCom.V[0]);
NewtonBodyGetMatrix(chassis, @rlmat.V[0].V[0]);
chassisCom: = OXTransformVector(chassisCom, rlmat);
chassisVeloc: = VectorAdd(chassisVeloc, VectorCrossProduct(chassisOmega, VectorSubtract(chassisMatrixInGlobalSpace.m_posit, chassisCom)));
projTireVeloc: = VectorSubtract{ VectorAdd }(chassisVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(chassisVeloc, chassisMatrixInGlobalSpace.m_up)));
projTireVeloc: = VectorAdd{ VectorSubtract }(projTireVeloc, VectorScale(chassisMatrixInGlobalSpace.m_up, VectorDotProduct(tireVeloc, chassisMatrixInGlobalSpace.m_up)));
//NegateVector(projTireVeloc);
NewtonBodySetVelocity(tire, @projTireVeloc.V[0]);
*/
// project angular velocity
NewtonBodyGetOmega(tire, &tireOmega[0]);
tireOmegaCorrection = tireOmega;
//
if (GetVecLength(tireOmegaCorrection) > mFpsRequest) {
// I need to use this omega correction fix when NewtonBodySetLinearDamping,NewtonBodySetAngularDamping is set to zero.
// Because the tire rotation overpass the physics rotation limit in time, and it can give bad result without this fix.
// I prefered to have the body totally free rolling, because of this I need this fix.
// If you don't use this fix, Don't set the NewtonBodySetLinearDamping,NewtonBodySetAngularDamping to zero value, just don't call this both functions on the body at all.
//
//printf("Omega tire correction, Fix a Rotation limitation. \n");
tireOmegaCorrection = (tireOmegaCorrection * mTireOmegaCorrection);
NewtonBodySetOmega(tire, &tireOmegaCorrection[0]);
}
//
projTireOmega = chassisOmega - chassisMatrixInGlobalSpace.m_front * (chassisOmega.DotProduct3(chassisMatrixInGlobalSpace.m_front));
projTireOmega = projTireOmega + chassisMatrixInGlobalSpace.m_front * (tireOmegaCorrection.DotProduct3(chassisMatrixInGlobalSpace.m_front));
NewtonBodySetOmega(tire, &projTireOmega[0]);
}
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;
}
