void CustomDGRayCastCar::ApplyChassisTorque (const dVector& vForce, const dVector& vPoint)
{
dVector Torque;
dMatrix M;
dVector com;
NewtonBodyGetCentreOfMass( m_body0, &com[0] );
NewtonBodyGetMatrix( m_body0, &M[0][0] );
Torque = ( vPoint - M.TransformVector( dVector( com.m_x, com.m_y, com.m_z, 1.0f ) ) ) * vForce;
NewtonBodyAddTorque( m_body0, &Torque[0] );
}
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 Roket_PhysicsBody::updateTorque()
{
// apply any torque we need to
for (int i = 0;i < torqueToApply.size(); i++)
{
float force[3];
force[0] = torqueToApply[i].X;
force[1] = torqueToApply[i].Y;
force[2] = torqueToApply[i].Z;
NewtonBodyAddTorque( body, &force[0] );
// outputs too much :P
//nonlua_debug("Applying torque " << i << ".",LVL_INFO);
}
torqueToApply.clear();
}
void cPhysicsBodyNewton::OnUpdateCallback(const NewtonBody *a_pBody, dFloat a_fTimeStep, int a_lThreadIndex)
{
float fMass;
float fX, fY, fZ;
cPhysicsBodyNewton *pRigidBody = static_cast<cPhysicsBodyNewton*>(NewtonBodyGetUserData(a_pBody));
if (pRigidBody->IsActive() == false) return;
cVector3f vGravity = pRigidBody->m_pWorld->GetGravity();
if (pRigidBody->m_bGravity)
{
NewtonBodyGetMassMatrix(a_pBody, &fMass, &fX, &fY, &fZ);
float fForce[3] = {fMass * vGravity.x, fMass * vGravity.y, fMass * vGravity.z};
NewtonBodyAddForce(a_pBody, &fForce[0]);
}
if (pRigidBody->m_Buoyancy.m_bActive)
{
g_SurfacePlane = pRigidBody->m_Buoyancy.m_Surface;
/*NewtonBodyAddBuoyancyForce(a_pBody,
pRigidBody->m_Buoyancy.m_fDensity,
pRigidBody->m_Buoyancy.m_fLinearViscosity,
pRigidBody->m_Buoyancy.m_fAngularViscosity,
vGravity.v, BuoyancyPlaneCallback,
pRigidBody);*/
}
NewtonBodyAddForce(a_pBody, pRigidBody->m_vTotalForce.v);
NewtonBodyAddTorque(a_pBody, pRigidBody->m_vTotalTorque.v);
if (pRigidBody->m_fMaxLinearSpeed > 0)
{
cVector3f vVel = pRigidBody->GetLinearVelocity();
float fSpeed = vVel.Length();
if (fSpeed > pRigidBody->m_fMaxAngularSpeed)
{
vVel = cMath::Vector3Normalize(vVel) * pRigidBody->m_fMaxAngularSpeed;
pRigidBody->SetAngularVelocity(vVel);
}
}
}
static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dVector com;
dVector veloc;
dVector omega;
dMatrix matrix;
// apply the thew body forces
if (chainForceCallback) {
chainForceCallback (body, timestep, threadIndex);
}
// add the mouse pick penalty force and torque
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetOmega(body, &omega[0]);
NewtonBodyGetVelocity(body, &veloc[0]);
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
force -= dampForce;
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetCentreOfMass (body, &com[0]);
// calculate local point relative to center of mass
dVector point (matrix.RotateVector (attachmentPoint - com));
dVector torque (point * force);
dVector torqueDamp (omega.Scale (mass * 0.1f));
NewtonBodyAddForce (body, &force.m_x);
NewtonBodyAddTorque (body, &torque.m_x);
// make sure the body is unfrozen, if it is picked
NewtonBodySetFreezeState (body, 0);
}
void OnInside(NewtonBody* const visitor)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz);
if (mass > 0.0f) {
dMatrix matrix;
dVector cog(0.0f);
dVector accelPerUnitMass(0.0f);
dVector torquePerUnitMass(0.0f);
const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f);
NewtonBodyGetMatrix (visitor, &matrix[0][0]);
NewtonBodyGetCentreOfMass(visitor, &cog[0]);
cog = matrix.TransformVector (cog);
NewtonCollision* const collision = NewtonBodyGetCollision(visitor);
dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume);
dFloat viscosity = 0.995f;
NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
dVector force (accelPerUnitMass.Scale (mass));
dVector torque (torquePerUnitMass.Scale (mass));
dVector omega(0.0f);
NewtonBodyGetOmega(visitor, &omega[0]);
omega = omega.Scale (viscosity);
NewtonBodySetOmega(visitor, &omega[0]);
NewtonBodyAddForce (visitor, &force[0]);
NewtonBodyAddTorque (visitor, &torque[0]);
}
}
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 CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dVector com;
dMatrix matrix;
dVector omega0;
dVector veloc0;
dVector omega1;
dVector veloc1;
dVector pointVeloc;
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
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 cPhysicsBodyNewton::OnUpdateCallback(const NewtonBody* apBody, dFloat timestep, int threadIndex)
{
float fMass;
float fX,fY,fZ;
cPhysicsBodyNewton* pRigidBody = (cPhysicsBodyNewton*) NewtonBodyGetUserData(apBody);
if(pRigidBody->IsActive()==false) return;
cVector3f vGravity = pRigidBody->mpWorld->GetGravity();
//Create some gravity
if (pRigidBody->mbGravity)
{
NewtonBodyGetMassMatrix(apBody, &fMass, &fX, &fY, &fZ);
float fForce[3] = { fMass * vGravity.x, fMass * vGravity.y, fMass * vGravity.z};
NewtonBodyAddForce(apBody, &fForce[0]);
}
// Create Buoyancy
if (pRigidBody->mBuoyancy.mbActive)
{
gSurfacePlane = pRigidBody->mBuoyancy.mSurface;
NewtonBodyAddBuoyancyForce( apBody,
pRigidBody->mBuoyancy.mfDensity,
pRigidBody->mBuoyancy.mfLinearViscosity,
pRigidBody->mBuoyancy.mfAngularViscosity,
vGravity.v, BuoyancyPlaneCallback,
pRigidBody);
}
// Add forces from calls to Addforce(..), etc
NewtonBodyAddForce(apBody, pRigidBody->mvTotalForce.v);
NewtonBodyAddTorque(apBody, pRigidBody->mvTotalTorque.v);
// Check so that all speeds are within thresholds
// Linear
if (pRigidBody->mfMaxLinearSpeed > 0)
{
cVector3f vVel = pRigidBody->GetLinearVelocity();
float fSpeed = vVel.Length();
if (fSpeed > pRigidBody->mfMaxLinearSpeed)
{
vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxLinearSpeed;
pRigidBody->SetLinearVelocity(vVel);
}
}
// Angular
if (pRigidBody->mfMaxAngularSpeed > 0)
{
cVector3f vVel = pRigidBody->GetAngularVelocity();
float fSpeed = vVel.Length();
if (fSpeed > pRigidBody->mfMaxAngularSpeed)
{
vVel = cMath::Vector3Normalize(vVel) * pRigidBody->mfMaxAngularSpeed;
pRigidBody->SetAngularVelocity(vVel);
}
}
//cVector3f vForce;
//NewtonBodyGetForce(apBody,vForce.v);
//Log("Engine force %s\n",pRigidBody->mvTotalForce.ToString().c_str());
//Log("Engine force %s, body force: %s \n",pRigidBody->mvTotalForce.ToString().c_str(),
// vForce.ToString().c_str());
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::applyTorque(const Math::Vector3& _torque)
{
NewtonBodyAddTorque(m_pActor, _torque.m_array);
}
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;
}
