void dgSolver::InitJacobianMatrix(dgInt32 threadID) { dgLeftHandSide* const leftHandSide = &m_world->GetSolverMemory().m_leftHandSizeBuffer[0]; dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0]; dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0]; dgContraintDescritor constraintParams; constraintParams.m_world = m_world; constraintParams.m_threadIndex = threadID; constraintParams.m_timestep = m_timestep; constraintParams.m_invTimestep = m_invTimestep; const dgInt32 step = m_threadCounts; const dgInt32 jointCount = m_cluster->m_jointCount; for (dgInt32 i = threadID; i < jointCount; i += step) { dgJointInfo* const jointInfo = &m_jointArray[i]; dgConstraint* const constraint = jointInfo->m_joint; dgAssert(jointInfo->m_m0 >= 0); dgAssert(jointInfo->m_m1 >= 0); dgAssert(jointInfo->m_m0 != jointInfo->m_m1); const dgInt32 rowBase = dgAtomicExchangeAndAdd(&m_jacobianMatrixRowAtomicIndex, jointInfo->m_pairCount); m_world->GetJacobianDerivatives(constraintParams, jointInfo, constraint, leftHandSide, rightHandSide, rowBase); BuildJacobianMatrix(jointInfo, leftHandSide, rightHandSide, internalForces); } }
void dgSolver::InitJacobianMatrix(dgInt32 threadID) { dgLeftHandSide* const leftHandSide = &m_world->GetSolverMemory().m_leftHandSizeBuffer[0]; dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0]; dgBodyJacobianPair* const bodyJacobiansPairs = m_bodyJacobiansPairs; dgContraintDescritor constraintParams; constraintParams.m_world = m_world; constraintParams.m_threadIndex = threadID; constraintParams.m_timestep = m_timestep; constraintParams.m_invTimestep = m_invTimestep; const dgInt32 step = m_threadCounts; const dgInt32 jointCount = m_cluster->m_jointCount; for (dgInt32 i = threadID; i < jointCount; i += step) { dgJointInfo* const jointInfo = &m_jointArray[i]; dgConstraint* const constraint = jointInfo->m_joint; dgAssert(jointInfo->m_m0 >= 0); dgAssert(jointInfo->m_m1 >= 0); dgAssert(jointInfo->m_m0 != jointInfo->m_m1); const dgInt32 rowBase = dgAtomicExchangeAndAdd(&m_jacobianMatrixRowAtomicIndex, jointInfo->m_pairCount); m_world->GetJacobianDerivatives(constraintParams, jointInfo, constraint, leftHandSide, rightHandSide, rowBase); BuildJacobianMatrix(jointInfo, leftHandSide, rightHandSide); bodyJacobiansPairs[i * 2 + 0].m_bodyIndex = jointInfo->m_m0; bodyJacobiansPairs[i * 2 + 0].m_rowCount = jointInfo->m_pairCount; bodyJacobiansPairs[i * 2 + 0].m_rowStart = jointInfo->m_pairStart * 4; bodyJacobiansPairs[i * 2 + 0].m_righHandStart = jointInfo->m_pairStart; bodyJacobiansPairs[i * 2 + 0].m_preconditioner = jointInfo->m_preconditioner0; bodyJacobiansPairs[i * 2 + 1].m_bodyIndex = jointInfo->m_m1; bodyJacobiansPairs[i * 2 + 1].m_rowCount = jointInfo->m_pairCount; bodyJacobiansPairs[i * 2 + 1].m_rowStart = jointInfo->m_pairStart * 4 + 1; bodyJacobiansPairs[i * 2 + 1].m_righHandStart = jointInfo->m_pairStart; bodyJacobiansPairs[i * 2 + 1].m_preconditioner = jointInfo->m_preconditioner1; } }
void dgWorldDynamicUpdate::BuildJacobianMatrix(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const { dTimeTrackerEvent(__FUNCTION__); dgAssert (cluster->m_bodyCount >= 2); dgWorld* const world = (dgWorld*) this; const dgInt32 bodyCount = cluster->m_bodyCount; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart]; dgAssert (((dgDynamicBody*) bodyArray[0].m_body)->IsRTTIType (dgBody::m_dynamicBodyRTTI)); dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_accel.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_accel)) == dgFloat32 (0.0f)); dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_alpha.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_alpha)) == dgFloat32 (0.0f)); dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_externalForce)) == dgFloat32 (0.0f)); dgAssert ((((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque.DotProduct3(((dgDynamicBody*)bodyArray[0].m_body)->m_externalTorque)) == dgFloat32 (0.0f)); internalForces[0].m_linear = dgVector::m_zero; internalForces[0].m_angular = dgVector::m_zero; if (timestep != dgFloat32 (0.0f)) { for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; //dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI)); dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); if (!body->m_equilibrium) { dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f)); body->AddDampingAcceleration(timestep); body->CalcInvInertiaMatrix (); // body->ApplyGyroTorque(); internalForces[i].m_linear = dgVector::m_zero; internalForces[i].m_angular = dgVector::m_zero; } else if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { internalForces[i].m_linear = body->m_externalForce * dgVector::m_negOne; internalForces[i].m_angular = body->m_externalTorque * dgVector::m_negOne; } else { internalForces[i].m_linear = dgVector::m_zero; internalForces[i].m_angular = dgVector::m_zero; } // re use these variables for temp storage body->m_accel = body->m_veloc; body->m_alpha = body->m_omega; } } else { for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert(body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || body->IsRTTIType(dgBody::m_kinematicBodyRTTI)); if (!body->m_equilibrium) { dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f)); body->CalcInvInertiaMatrix (); } // re use these variables for temp storage body->m_accel = body->m_veloc; body->m_alpha = body->m_omega; internalForces[i].m_linear = dgVector::m_zero; internalForces[i].m_angular = dgVector::m_zero; } } dgContraintDescritor constraintParams; constraintParams.m_world = world; constraintParams.m_threadIndex = threadID; constraintParams.m_timestep = timestep; constraintParams.m_invTimestep = (timestep > dgFloat32(1.0e-5f)) ? dgFloat32(1.0f / timestep) : dgFloat32(0.0f); const dgFloat32 forceOrImpulseScale = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) : dgFloat32(0.0f); dgJointInfo* const constraintArrayPtr = (dgJointInfo*)&world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_jacobianBuffer[cluster->m_rowsStart]; dgInt32 rowCount = 0; //const dgInt32 jointCount = island->m_jointCount; const dgInt32 jointCount = cluster->m_activeJointCount; for (dgInt32 i = 0; i < jointCount; i++) { dgJointInfo* const jointInfo = &constraintArray[i]; dgConstraint* const constraint = jointInfo->m_joint; dgAssert (dgInt32 (constraint->m_index) == i); dgAssert(jointInfo->m_m0 < cluster->m_bodyCount); dgAssert(jointInfo->m_m1 < cluster->m_bodyCount); //dgAssert (constraint->m_index == dgUnsigned32(j)); rowCount = GetJacobianDerivatives(constraintParams, jointInfo, constraint, matrixRow, rowCount); dgAssert(rowCount <= cluster->m_rowsCount); dgAssert(jointInfo->m_m0 >= 0); dgAssert(jointInfo->m_m0 < bodyCount); dgAssert(jointInfo->m_m1 >= 0); dgAssert(jointInfo->m_m1 < bodyCount); BuildJacobianMatrix(bodyArray, jointInfo, internalForces, matrixRow, forceOrImpulseScale); } }
void dgWorldDynamicUpdate::ResolveClusterForces(dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep) const { if (cluster->m_activeJointCount) { SortClusters(cluster, timestep, threadID); } if (!cluster->m_isContinueCollision) { if (cluster->m_activeJointCount) { BuildJacobianMatrix (cluster, threadID, timestep); CalculateClusterReactionForces(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); //CalculateClusterReactionForces_1(cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); } else { IntegrateExternalForce(cluster, timestep, threadID); } IntegrateVelocity (cluster, DG_SOLVER_MAX_ERROR, timestep, threadID); } else { // calculate reaction forces and new velocities BuildJacobianMatrix (cluster, threadID, timestep); IntegrateReactionsForces (cluster, threadID, timestep, DG_SOLVER_MAX_ERROR); // see if the island goes to sleep bool isAutoSleep = true; bool stackSleeping = true; dgInt32 sleepCounter = 10000; dgWorld* const world = (dgWorld*) this; const dgInt32 bodyCount = cluster->m_bodyCount; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; const dgFloat32 forceDamp = DG_FREEZZING_VELOCITY_DRAG; dgFloat32 maxAccel = dgFloat32 (0.0f); dgFloat32 maxAlpha = dgFloat32 (0.0f); dgFloat32 maxSpeed = dgFloat32 (0.0f); dgFloat32 maxOmega = dgFloat32 (0.0f); const dgFloat32 speedFreeze = world->m_freezeSpeed2; const dgFloat32 accelFreeze = world->m_freezeAccel2; const dgVector forceDampVect (forceDamp, forceDamp, forceDamp, dgFloat32 (0.0f)); for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { dgAssert (body->m_invMass.m_w); const dgFloat32 accel2 = body->m_accel.DotProduct3(body->m_accel); const dgFloat32 alpha2 = body->m_alpha.DotProduct3(body->m_alpha); const dgFloat32 speed2 = body->m_veloc.DotProduct3(body->m_veloc); const dgFloat32 omega2 = body->m_omega.DotProduct3(body->m_omega); maxAccel = dgMax (maxAccel, accel2); maxAlpha = dgMax (maxAlpha, alpha2); maxSpeed = dgMax (maxSpeed, speed2); maxOmega = dgMax (maxOmega, omega2); bool equilibrium = (accel2 < accelFreeze) && (alpha2 < accelFreeze) && (speed2 < speedFreeze) && (omega2 < speedFreeze); if (equilibrium) { dgVector veloc (body->m_veloc * forceDampVect); dgVector omega = body->m_omega * forceDampVect; body->m_veloc = (dgVector (veloc.DotProduct4(veloc)) > m_velocTol) & veloc; body->m_omega = (dgVector (omega.DotProduct4(omega)) > m_velocTol) & omega; } body->m_equilibrium = dgUnsigned32 (equilibrium); stackSleeping &= equilibrium; isAutoSleep &= body->m_autoSleep; sleepCounter = dgMin (sleepCounter, body->m_sleepingCounter); } // clear accel and angular acceleration body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; } if (isAutoSleep) { if (stackSleeping) { // the island went to sleep mode, for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; } } else { // island is not sleeping but may be resting with small residual velocity for a long time // see if we can force to go to sleep if ((maxAccel > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel) || (maxAlpha > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha) || (maxSpeed > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc) || (maxOmega > world->m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega)) { for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = 0; } } } else { dgInt32 index = 0; for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) { if ((maxAccel <= world->m_sleepTable[i].m_maxAccel) && (maxAlpha <= world->m_sleepTable[i].m_maxAlpha) && (maxSpeed <= world->m_sleepTable[i].m_maxVeloc) && (maxOmega <= world->m_sleepTable[i].m_maxOmega)) { index = i; break; } } dgInt32 timeScaleSleepCount = dgInt32 (dgFloat32 (60.0f) * sleepCounter * timestep); if (timeScaleSleepCount > world->m_sleepTable[index].m_steps) { // force island to sleep stackSleeping = true; for (dgInt32 i = 1; i < bodyCount; i ++) { dgBody* const body = bodyArray[i].m_body; dgAssert (body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || body->IsRTTIType (dgBody::m_kinematicBodyRTTI)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; body->m_veloc = dgVector::m_zero; body->m_omega = dgVector::m_zero; body->m_equilibrium = true; } } else { sleepCounter ++; for (dgInt32 i = 1; i < bodyCount; i ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->m_sleepingCounter = sleepCounter; } } } } } } if (!(isAutoSleep & stackSleeping)) { // island is not sleeping, need to integrate island velocity const dgUnsigned32 lru = world->GetBroadPhase()->m_lru; const dgInt32 jointCount = cluster->m_jointCount; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; dgFloat32 timeRemaining = timestep; const dgFloat32 timeTol = dgFloat32 (0.01f) * timestep; for (dgInt32 i = 0; (i < DG_MAX_CONTINUE_COLLISON_STEPS) && (timeRemaining > timeTol); i ++) { // calculate the closest time to impact dgFloat32 timeToImpact = timeRemaining; for (dgInt32 j = 0; (j < jointCount) && (timeToImpact > timeTol); j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { dgDynamicBody* const body0 = (dgDynamicBody*)contact->m_body0; dgDynamicBody* const body1 = (dgDynamicBody*)contact->m_body1; if (body0->m_continueCollisionMode | body1->m_continueCollisionMode) { dgVector p; dgVector q; dgVector normal; timeToImpact = dgMin (timeToImpact, world->CalculateTimeToImpact (contact, timeToImpact, threadID, p, q, normal, dgFloat32 (-1.0f / 256.0f))); } } } if (timeToImpact > timeTol) { timeRemaining -= timeToImpact; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } else { if (timeToImpact >= dgFloat32 (-1.0e-5f)) { for (dgInt32 j = 1; j < bodyCount; j++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[j].m_body; if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeToImpact); body->UpdateWorlCollisionMatrix(); } } } CalculateClusterContacts (cluster, timeRemaining, lru, threadID); BuildJacobianMatrix (cluster, threadID, 0.0f); IntegrateReactionsForces (cluster, threadID, 0.0f, DG_SOLVER_MAX_ERROR); bool clusterReceding = true; const dgFloat32 step = timestep * dgFloat32 (1.0f / DG_MAX_CONTINUE_COLLISON_STEPS); for (dgInt32 k = 0; (k < DG_MAX_CONTINUE_COLLISON_STEPS) && clusterReceding; k ++) { dgFloat32 smallTimeStep = dgMin (step, timeRemaining); timeRemaining -= smallTimeStep; for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity (smallTimeStep); body->UpdateWorlCollisionMatrix(); } } clusterReceding = false; if (timeRemaining > timeTol) { CalculateClusterContacts (cluster, timeRemaining, lru, threadID); bool isColliding = false; for (dgInt32 j = 0; (j < jointCount) && !isColliding; j ++) { dgContact* const contact = (dgContact*) constraintArray[j].m_joint; if (contact->GetId() == dgConstraint::m_contactConstraint) { const dgBody* const body0 = contact->m_body0; const dgBody* const body1 = contact->m_body1; const dgVector& veloc0 = body0->m_veloc; const dgVector& veloc1 = body1->m_veloc; const dgVector& omega0 = body0->m_omega; const dgVector& omega1 = body1->m_omega; const dgVector& com0 = body0->m_globalCentreOfMass; const dgVector& com1 = body1->m_globalCentreOfMass; for (dgList<dgContactMaterial>::dgListNode* node = contact->GetFirst(); node; node = node->GetNext()) { const dgContactMaterial* const contactMaterial = &node->GetInfo(); dgVector vel0 (veloc0 + omega0.CrossProduct3(contactMaterial->m_point - com0)); dgVector vel1 (veloc1 + omega1.CrossProduct3(contactMaterial->m_point - com1)); dgVector vRel (vel0 - vel1); dgAssert (contactMaterial->m_normal.m_w == dgFloat32 (0.0f)); dgFloat32 speed = vRel.DotProduct4(contactMaterial->m_normal).m_w; isColliding |= (speed < dgFloat32 (0.0f)); } } } clusterReceding = !isColliding; } } } } if (timeRemaining > dgFloat32 (0.0)) { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->IntegrateVelocity(timeRemaining); body->UpdateCollisionMatrix (timeRemaining, threadID); } } } else { for (dgInt32 j = 1; j < bodyCount; j ++) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[j].m_body; if (body->IsRTTIType (dgBody::m_dynamicBodyRTTI)) { body->UpdateCollisionMatrix (timestep, threadID); } } } } } }
dTireForceSolverSolver(CustomVehicleController* const controller, dFloat timestep, int threadId) :dComplemtaritySolver() ,m_controller(controller) { m_controller->m_externalContactStatesCount = 0; m_controller->m_freeContactList = m_controller->m_externalContactStatesPoll.GetFirst(); // apply all external forces and torques to chassis and all tire velocities dFloat timestepInv = 1.0f / timestep; m_controller->m_chassisState.UpdateDynamicInputs(); m_controller->m_sleepCounter --; bool isSleeping = m_controller->IsSleeping(); if (isSleeping) { if (m_controller->m_sleepCounter > 0) { isSleeping = false; } } else { m_controller->m_sleepCounter = VEHICLE_SLEEP_COUNTER; } if (isSleeping) { m_controller->m_chassisState.PutToSleep(); } else { NewtonBody* const body = controller->GetBody(); CustomControllerConvexCastPreFilter castFilter (body); for (dTireList::dListNode* node = m_controller->m_tireList.GetFirst(); node; node = node->GetNext()) { CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo(); tire->Collide(castFilter, timestepInv, threadId); tire->UpdateDynamicInputs(timestep); } // update all components if (m_controller->m_engine) { m_controller->m_engine->Update(timestep); } if (m_controller->m_steering) { m_controller->m_steering->Update(timestep); } if (m_controller->m_handBrakes) { m_controller->m_handBrakes->Update(timestep); } if (m_controller->m_brakes) { m_controller->m_brakes->Update(timestep); } // Get the number of active joints for this integration step int bodyCount = 0; for (dList<CustomVehicleControllerBodyState*>::dListNode* stateNode = m_controller->m_stateList.GetFirst(); stateNode; stateNode = stateNode->GetNext()) { m_bodyArray[bodyCount] = stateNode->GetInfo(); dAssert (bodyCount < int (sizeof (m_bodyArray) / sizeof(m_bodyArray[0]))); bodyCount ++; } for (int i = 0; i < m_controller->m_externalContactStatesCount; i ++) { m_bodyArray[bodyCount] = m_controller->m_externalContactStates[i]; dAssert (bodyCount < int (sizeof (m_bodyArray) / sizeof(m_bodyArray[0]))); bodyCount ++; } int jointCount = GetActiveJoints(); BuildJacobianMatrix (jointCount, m_jointArray, timestep, m_jacobianPairArray, m_jacobianColumn, sizeof (m_jacobianPairArray)/ sizeof (m_jacobianPairArray[0])); CalculateReactionsForces (bodyCount, m_bodyArray, jointCount, m_jointArray, timestep, m_jacobianPairArray, m_jacobianColumn); } }