void dComplentaritySolver::dFrictionLessContactJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
dJacobianPair* const rowMatrix = params->m_rowMatrix;
dJacobianColum* const jacobianColElements = params->m_colMatrix;
const dVector& bodyVeloc0 = m_state0->GetVelocity();
const dVector& bodyOmega0 = m_state0->GetOmega();
const dVector& bodyVeloc1 = m_state1->GetVelocity();
const dVector& bodyOmega1 = m_state1->GetOmega();
int count = params->m_rowsCount;
dAssert (params->m_timeStep > dFloat (0.0f));
for (int k = 0; k < count; k ++) {
const dJacobianPair& Jt = rowMatrix[k];
dJacobianColum& element = jacobianColElements[k];
dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) + Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) + Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) + Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));
dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dFloat aRel = element.m_deltaAccel;
//dFloat restitution = (vRel <= 0.0f) ? 1.05f : 1.0f;
dFloat restitution = (vRel <= 0.0f) ? (dFloat (1.0f) + m_restitution) : dFloat(1.0f);
vRel *= restitution;
vRel = dMin (dFloat (4.0f), vRel);
element.m_coordenateAccel = (aRel - vRel * params->m_invTimeStep);
}
}
void dComplentaritySolver::dBilateralJoint::JointAccelerations (dJointAccelerationDecriptor* const params)
{
dJacobianColum* const jacobianColElements = params->m_colMatrix;
dJacobianPair* const jacobianRowElements = params->m_rowMatrix;
const dVector& bodyVeloc0 = m_state0->m_veloc;
const dVector& bodyOmega0 = m_state0->m_omega;
const dVector& bodyVeloc1 = m_state1->m_veloc;
const dVector& bodyOmega1 = m_state1->m_omega;
dFloat timestep = params->m_timeStep;
dFloat kd = COMPLEMENTARITY_VEL_DAMP * dFloat (4.0f);
dFloat ks = COMPLEMENTARITY_POS_DAMP * dFloat (0.25f);
for (int k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
jacobianColElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianColElements[k].m_deltaAccel;
} else {
const dJacobianPair& Jt = jacobianRowElements[k];
dVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0) +
Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0) +
Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1) +
Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1));
dFloat vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dFloat aRel = jacobianColElements[k].m_deltaAccel;
dFloat ksd = timestep * ks;
dFloat relPosit = 0.0f - vRel * timestep * params->m_firstPassCoefFlag;
dFloat num = ks * relPosit - kd * vRel - ksd * vRel;
dFloat den = dFloat (1.0f) + timestep * kd + timestep * ksd;
dFloat aRelErr = num / den;
jacobianColElements[k].m_coordenateAccel = aRelErr + aRel;
}
}
}
void dgContact::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
dgJacobianMatrixElement* const rowMatrix = params->m_rowMatrix;
const dgVector& bodyVeloc0 = m_body0->m_veloc;
const dgVector& bodyOmega0 = m_body0->m_omega;
const dgVector& bodyVeloc1 = m_body1->m_veloc;
const dgVector& bodyOmega1 = m_body1->m_omega;
dgInt32 count = params->m_rowsCount;
dgFloat32 timestep = dgFloat32 (1.0f);
dgFloat32 invTimestep = dgFloat32 (1.0f);
if (params->m_timeStep > dgFloat32 (0.0f)) {
timestep = params->m_timeStep;
invTimestep = params->m_invTimeStep;
}
for (dgInt32 k = 0; k < count; k ++) {
// if (!rowMatrix[k].m_accelIsMotor)
// {
dgJacobianMatrixElement* const row = &rowMatrix[k];
dgVector relVeloc (row->m_Jt.m_jacobianM0.m_linear.CompProduct4(bodyVeloc0) + row->m_Jt.m_jacobianM0.m_angular.CompProduct4(bodyOmega0) + row->m_Jt.m_jacobianM1.m_linear.CompProduct4(bodyVeloc1) + row->m_Jt.m_jacobianM1.m_angular.CompProduct4(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
dgFloat32 aRel = row->m_deltaAccel;
if (row->m_normalForceIndex < 0) {
dgFloat32 restitution = (vRel <= dgFloat32 (0.0f)) ? (dgFloat32 (1.0f) + row->m_restitution) : dgFloat32 (1.0f);
dgFloat32 penetrationVeloc = dgFloat32 (0.0f);
if (row->m_penetration > DG_RESTING_CONTACT_PENETRATION * dgFloat32 (0.125f)) {
if (vRel > dgFloat32 (0.0f)) {
dgFloat32 penetrationCorrection = vRel * timestep;
dgAssert (penetrationCorrection >= dgFloat32 (0.0f));
row->m_penetration = dgMax (dgFloat32 (0.0f), row->m_penetration - penetrationCorrection);
} else {
dgFloat32 penetrationCorrection = -vRel * timestep * row->m_restitution * dgFloat32 (8.0f);
if (penetrationCorrection > row->m_penetration) {
row->m_penetration = dgFloat32 (0.001f);
}
}
penetrationVeloc = -(row->m_penetration * row->m_penetrationStiffness);
}
vRel *= restitution;
vRel = dgMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
}
row->m_coordenateAccel = (aRel - vRel * invTimestep);
// }
}
}
void dgContact::JointAccelerations(const dgJointAccelerationDecriptor& params)
{
const dgJacobianPair* const Jt = params.m_Jt;
const dgVector& bodyVeloc0 = m_body0->m_veloc;
const dgVector& bodyOmega0 = m_body0->m_omega;
const dgVector& bodyVeloc1 = m_body1->m_veloc;
const dgVector& bodyOmega1 = m_body1->m_omega;
for (dgInt32 k = 0; k < params.m_rowsCount; k ++) {
if (!params.m_accelIsMotor[k]) {
dgFloat32 vRel;
dgFloat32 aRel;
dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
aRel = params.m_externAccelaration[k];
if (params.m_normalForceIndex[k] < 0) {
dgFloat32 restitution;
dgFloat32 penetrationVeloc;
restitution = dgFloat32 (1.0f);
if (vRel <= dgFloat32 (0.0f)) {
restitution += params.m_restitution[k];
}
penetrationVeloc = 0.0f;
if (params.m_penetration[k] > dgFloat32 (1.0e-2f)) {
dgFloat32 penetrationCorrection;
if (vRel > dgFloat32 (0.0f)) {
penetrationCorrection = vRel * params.m_timeStep;
_ASSERTE (penetrationCorrection >= dgFloat32 (0.0f));
params.m_penetration[k] = GetMax (dgFloat32 (0.0f), params.m_penetration[k] - penetrationCorrection);
}
penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k]);
}
vRel *= restitution;
vRel = GetMin (dgFloat32 (4.0f), vRel + penetrationVeloc);
}
params.m_coordenateAccel[k] = (aRel - vRel * params.m_invTimeStep);
}
}
}
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 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 dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
dgJacobianMatrixElement* const jacobianMatrixElements = params->m_rowMatrix;
const dgVector& bodyVeloc0 = m_body0->m_veloc;
const dgVector& bodyOmega0 = m_body0->m_omega;
const dgVector& bodyVeloc1 = m_body1->m_veloc;
const dgVector& bodyOmega1 = m_body1->m_omega;
// remember the impulse branch
//dgAssert (params->m_timeStep > dgFloat32 (0.0f));
if (params->m_timeStep > dgFloat32 (0.0f)) {
dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);
dgFloat32 dt = params->m_timeStep;
for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
} else {
const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) + Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) + Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
//dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
//dgFloat32 aRel = jacobianMatrixElements[k].m_coordenateAccel;
dgFloat32 aRel = params->m_firstPassCoefFlag ? jacobianMatrixElements[k].m_deltaAccel : jacobianMatrixElements[k].m_coordenateAccel;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
//alphaError = num / den;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
//dgFloat32 dt = desc.m_timestep;
//dgFloat32 ks = DG_POS_DAMP;
//dgFloat32 kd = DG_VEL_DAMP;
//dgFloat32 ksd = dt * ks;
//dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
//dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
//accelError = num / den;
dgFloat32 ksd = dt * ks;
dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
jacobianMatrixElements[k].m_penetration = relPosit;
dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 aRelErr = num / den;
//centripetal acceleration is stored in restitution member
jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;
}
}
} else {
for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
} else {
const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) +
Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) +
Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
jacobianMatrixElements[k].m_coordenateAccel = jacobianMatrixElements[k].m_deltaAccel - vRel;
}
}
}
}
void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
dgJacobianMatrixElement* const jacobianMatrixElements = params->m_rowMatrix;
// const dgJacobianPair* const Jt = params.m_Jt;
const dgVector& bodyVeloc0 = m_body0->m_veloc;
const dgVector& bodyOmega0 = m_body0->m_omega;
const dgVector& bodyVeloc1 = m_body1->m_veloc;
const dgVector& bodyOmega1 = m_body1->m_omega;
#if 1
dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);
dgFloat32 dt = params->m_timeStep;
for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
if (m_rowIsMotor[k]) {
//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
} else {
// dgFloat32 num;
// dgFloat32 den;
// dgFloat32 ksd;
// dgFloat32 vRel;
// dgFloat32 aRel;
// dgFloat32 aRelErr;
// dgFloat32 relPosit;
const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
dgVector relVeloc (Jt.m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
relVeloc += Jt.m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
relVeloc += Jt.m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
relVeloc += Jt.m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
//aRel = params.m_externAccelaration[k];
dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
// alphaError = num / den;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
// dgFloat32 dt = desc.m_timestep;
// dgFloat32 ks = DG_POS_DAMP;
// dgFloat32 kd = DG_VEL_DAMP;
// dgFloat32 ksd = dt * ks;
// dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
// dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
// accelError = num / den;
dgFloat32 ksd = dt * ks;
//dgFloat32 relPosit = params.m_penetration[k] - vRel * dt * params.m_firstPassCoefFlag;
dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
//params.m_penetration[k] = relPosit;
jacobianMatrixElements[k].m_penetration = relPosit;
dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 aRelErr = num / den;
//centripetal acceleration is stored restitution member
//params.m_coordenateAccel[k] = aRelErr + params.m_restitution[k] + aRel;
jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;
#else
dgFloat32 vRel;
dgFloat32 aRel;
dgFloat32 penetrationVeloc;
dgFloat32 penetrationCorrection;
dgVector relVeloc (Jt[k].m_jacobian_IM0.m_linear.CompProduct(bodyVeloc0));
relVeloc += Jt[k].m_jacobian_IM0.m_angular.CompProduct(bodyOmega0);
relVeloc += Jt[k].m_jacobian_IM1.m_linear.CompProduct(bodyVeloc1);
relVeloc += Jt[k].m_jacobian_IM1.m_angular.CompProduct(bodyOmega1);
vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z ;
penetrationCorrection = vRel * params.m_timeStep * params.m_firstPassCoefFlag;
params.m_penetration[k] = params.m_penetration[k] - penetrationCorrection;
if (params.m_penetration[k] > dgFloat32 (1.0f) ) {
params.m_penetration[k] = dgFloat32 (1.0f);
} else if (params.m_penetration[k] < dgFloat32 (-1.0f) ) {
params.m_penetration[k] = dgFloat32 (-1.0f);
}
penetrationVeloc = -(params.m_penetration[k] * params.m_penetrationStiffness[k] * params.m_invTimeStep);
vRel += penetrationVeloc;
//centripetal acceleration is stored restitution member
aRel = params.m_externAccelaration[k] + params.m_restitution[k];
params.m_coordenateAccel[k] = (aRel - vRel * params.m_invTimeStep);
#endif
}
}
}
void dgCollisionDeformableSolidMesh::ApplyExternalForces (dgFloat32 timestep)
{
dgAssert (m_myBody);
dgBody* const body = GetBody();
dgAssert (body->GetMass().m_w > dgFloat32 (0.0f));
dgAssert (body->GetMass().m_w < dgFloat32 (1.0e5f));
const dgMatrix& matrix = body->GetCollision()->GetGlobalMatrix();
dgFloat32 invMass = body->GetInvMass().m_w;
dgVector velocyStep (body->GetForce().Scale4(invMass * timestep));
//velocyStep = dgVector(0.0f);
dgVector* const veloc = m_particles.m_veloc;
dgFloat32* const unitMass = m_particles.m_unitMass;
/*
invMass = 0;
dgVector w (0.0f, 0.0f, 1.0f, 0.0f);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
unitMass[i] = 1.0f;
veloc[i] = w * m_posit[i];
invMass += unitMass[i];
}
invMass = 1.0f / invMass;
*/
dgVector com (dgFloat32 (0.0f));
dgVector comVeloc (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
const dgVector& p = m_posit[i];
veloc[i] += velocyStep;
com += p.CompProduct4(mass);
comVeloc += veloc[i].CompProduct4(mass);
}
com = com.Scale4(invMass);
comVeloc = comVeloc.Scale4(invMass);
const dgMatrix& indentity = dgGetIdentityMatrix();
dgMatrix inertiaMatrix (dgGetZeroMatrix());
inertiaMatrix.m_posit = dgVector::m_wOne;
dgVector comAngularMomentum (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
dgVector r (m_posit[i] - com);
dgVector mr (r.CompProduct4(mass));
dgVector relVeloc (veloc[i] - comVeloc);
comAngularMomentum += mr * relVeloc;
dgMatrix inertia (mr, r);
dgVector diagInertia (mr.DotProduct4(r));
inertiaMatrix.m_front += (indentity.m_front.CompProduct4(diagInertia) - inertia.m_front);
inertiaMatrix.m_up += (indentity.m_up.CompProduct4(diagInertia) - inertia.m_up);
inertiaMatrix.m_right += (indentity.m_right.CompProduct4(diagInertia) - inertia.m_right);
dgAssert (inertiaMatrix.TestSymetric3x3());
}
dgVector damp (0.3f);
dgMatrix invInertia (inertiaMatrix.Symetric3by3Inverse());
dgVector omega (invInertia.RotateVector(comAngularMomentum));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector r (m_posit[i] - com);
dgVector deltaVeloc (comVeloc + omega * r - veloc[i]);
veloc[i] += deltaVeloc.CompProduct4(damp);
}
//Sleep (50);
dgMatrix tmp;
dgMatrix transform;
dgVector scale;
inertiaMatrix.PolarDecomposition (transform, scale, tmp, matrix);
body->GetCollision()->SetGlobalMatrix(transform);
body->SetMatrixOriginAndRotation(body->GetCollision()->GetLocalMatrix().Inverse() * transform);
body->SetVelocity(comVeloc);
body->SetOmega(omega);
}
