void dgWorldDynamicUpdate::CalculateJointsForceParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0]; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; const dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0]; const int jointCount = syncData->m_bachIndex; dgParallelSolverSyncData::dgParallelJointMap* const jointInfoMap = syncData->m_jointConflicts; dgInt32* const atomicIndex = &syncData->m_atomicIndex; dgFloat32 accNorm = dgFloat32(0.0f); for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < jointCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgInt32 index = jointInfoMap[i].m_jointIndex; dgJointInfo* const jointInfo = &constraintArray[index]; dgFloat32 accel = world->CalculateJointForce(jointInfo, bodyArray, internalForces, matrixRow); accNorm = (accel > accNorm) ? accel : accNorm; } syncData->m_accelNorm[threadID] = accNorm; }
void dgWorldDynamicUpdate::InitializeBodyArrayParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; dgInt32* const atomicIndex = &syncData->m_atomicIndex; const dgIsland* const island = syncData->m_island; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0]; if (syncData->m_timestep != dgFloat32 (0.0f)) { for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgAssert (bodyArray[0].m_body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_accel % ((dgDynamicBody*)bodyArray[0].m_body)->m_accel) == dgFloat32 (0.0f)); dgAssert (bodyArray[0].m_body->IsRTTIType (dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_alpha % ((dgDynamicBody*)bodyArray[0].m_body)->m_alpha) == dgFloat32 (0.0f)); dgBody* const body = bodyArray[i].m_body; dgAssert (body->m_index == i); if (!body->m_equilibrium) { dgAssert (body->m_invMass.m_w > dgFloat32 (0.0f)); body->AddDampingAcceleration(syncData->m_timestep); body->CalcInvInertiaMatrix (); } if (body->m_active) { // re use these variables for temp storage body->m_netForce = body->m_veloc; body->m_netTorque = body->m_omega; internalForces[i].m_linear = dgVector::m_zero; internalForces[i].m_angular = dgVector::m_zero; } } } else { for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgAssert(bodyArray[0].m_body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_accel % ((dgDynamicBody*)bodyArray[0].m_body)->m_accel) == dgFloat32(0.0f)); dgAssert(bodyArray[0].m_body->IsRTTIType(dgBody::m_dynamicBodyRTTI) || (((dgDynamicBody*)bodyArray[0].m_body)->m_alpha % ((dgDynamicBody*)bodyArray[0].m_body)->m_alpha) == dgFloat32(0.0f)); dgBody* const body = bodyArray[i].m_body; if (!body->m_equilibrium) { dgAssert(body->m_invMass.m_w > dgFloat32(0.0f)); body->CalcInvInertiaMatrix(); } if (body->m_active) { //re use these variables for temp storage body->m_netForce = body->m_veloc; body->m_netTorque = body->m_omega; internalForces[i].m_linear = dgVector::m_zero; internalForces[i].m_angular = dgVector::m_zero; } } } }
void dgWorldDynamicUpdate::UpdateFeedbackForcesParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0]; dgInt32 hasJointFeeback = 0; dgInt32* const atomicIndex = &syncData->m_atomicIndex; for (dgInt32 curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1); curJoint < syncData->m_jointCount; curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgJointInfo* const jointInfo = &constraintArray[curJoint]; dgConstraint* const constraint = jointInfo->m_joint; if (constraint->m_solverActive) { const dgInt32 first = jointInfo->m_pairStart; const dgInt32 count = jointInfo->m_pairCount; for (dgInt32 j = 0; j < count; j++) { dgJacobianMatrixElement* const row = &matrixRow[j + first]; dgFloat32 val = row->m_force; dgAssert(dgCheckFloat(val)); row->m_jointFeebackForce[0].m_force = val; row->m_jointFeebackForce[0].m_impact = row->m_maxImpact * syncData->m_timestepRK; } hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0); } } syncData->m_hasJointFeeback[threadID] = hasJointFeeback; }
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 dgWorldDynamicUpdate::CalculateJointsAccelParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0]; dgJointAccelerationDecriptor joindDesc; joindDesc.m_timeStep = syncData->m_timestepRK; joindDesc.m_invTimeStep = syncData->m_invTimestepRK; joindDesc.m_firstPassCoefFlag = syncData->m_firstPassCoef; dgInt32* const atomicIndex = &syncData->m_atomicIndex; if (joindDesc.m_firstPassCoefFlag == dgFloat32 (0.0f)) { for (dgInt32 curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1); curJoint < syncData->m_jointCount; curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgJointInfo* const jointInfo = &constraintArray[curJoint]; dgConstraint* const constraint = jointInfo->m_joint; if (constraint->m_solverActive) { joindDesc.m_rowsCount = jointInfo->m_pairCount; joindDesc.m_rowMatrix = &matrixRow[jointInfo->m_pairStart]; constraint->JointAccelerations(&joindDesc); } } } else { const dgIsland* const island = syncData->m_island; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; const dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; for (dgInt32 curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1); curJoint < syncData->m_jointCount; curJoint = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgJointInfo* const jointInfo = &constraintArray[curJoint]; dgConstraint* const constraint = jointInfo->m_joint; if (constraint->m_solverActive) { const dgInt32 m0 = jointInfo->m_m0; const dgInt32 m1 = jointInfo->m_m1; const dgBody* const body0 = bodyArray[m0].m_body; const dgBody* const body1 = bodyArray[m1].m_body; if (!(body0->m_resting & body1->m_resting)) { joindDesc.m_rowsCount = jointInfo->m_pairCount; joindDesc.m_rowMatrix = &matrixRow[jointInfo->m_pairStart]; constraint->JointAccelerations(&joindDesc); } } } } }
void dgWorldDynamicUpdate::CalculateJointsVelocParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0]; dgVector speedFreeze2 (world->m_freezeSpeed2 * dgFloat32 (0.1f)); dgVector freezeOmega2 (world->m_freezeOmega2 * dgFloat32 (0.1f)); //dgVector forceActiveMask ((syncData->m_jointCount <= DG_SMALL_ISLAND_COUNT) ? dgFloat32 (-1.0f): dgFloat32 (0.0f)); dgVector forceActiveMask ((syncData->m_jointCount <= DG_SMALL_ISLAND_COUNT) ? dgVector (-1, -1, -1, -1) : dgFloat32 (0.0f)); dgInt32* const atomicIndex = &syncData->m_atomicIndex; if (syncData->m_timestepRK != dgFloat32 (0.0f)) { dgVector timestep4 (syncData->m_timestepRK); for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; dgAssert (body->m_index == i); world->ApplyNetVelcAndOmega (body, internalForces[i], timestep4, speedFreeze2, forceActiveMask); } } else { for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgBody* const body = bodyArray[i].m_body; if (body->m_active) { const dgVector& linearMomentum = internalForces[i].m_linear; const dgVector& angularMomentum = internalForces[i].m_angular; body->m_veloc += linearMomentum.Scale4(body->m_invMass.m_w); body->m_omega += body->m_invWorldInertiaMatrix.RotateVector(angularMomentum); } } } }
void dgWorldDynamicUpdate::BuildJacobianMatrixParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; dgInt32* const atomicIndex = &syncData->m_atomicIndex; const dgIsland* const island = syncData->m_island; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0]; dgAssert (syncData->m_jointCount); dgContraintDescritor constraintParams; constraintParams.m_world = world; constraintParams.m_threadIndex = threadID; constraintParams.m_timestep = syncData->m_timestep; constraintParams.m_invTimestep = syncData->m_invTimestep; dgFloat32 forceOrImpulseScale = (syncData->m_timestep > dgFloat32 (0.0f)) ? dgFloat32 (1.0f) : dgFloat32 (0.0f); for (dgInt32 jointIndex = dgAtomicExchangeAndAdd(atomicIndex, 1); jointIndex < syncData->m_jointCount; jointIndex = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgJointInfo* const jointInfo = &constraintArray[jointIndex]; dgConstraint* const constraint = jointInfo->m_joint; dgInt32 rowBase = dgAtomicExchangeAndAdd(&syncData->m_jacobianMatrixRowAtomicIndex, jointInfo->m_pairCount); world->GetJacobianDerivatives(constraintParams, jointInfo, constraint, matrixRow, rowBase); dgAssert (jointInfo->m_m0 >= 0); dgAssert (jointInfo->m_m1 >= 0); dgAssert (jointInfo->m_m0 != jointInfo->m_m1); dgAssert (jointInfo->m_m0 < island->m_bodyCount); dgAssert (jointInfo->m_m1 < island->m_bodyCount); world->BuildJacobianMatrix (bodyArray, jointInfo, matrixRow, forceOrImpulseScale); } }
void dgWorldDynamicUpdate::KinematicCallbackUpdateParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgInt32* const atomicIndex = &syncData->m_atomicIndex; for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgInt32 curJoint = i; if (constraintArray[curJoint].m_joint->m_updaFeedbackCallback) { constraintArray[curJoint].m_joint->m_updaFeedbackCallback (*constraintArray[curJoint].m_joint, syncData->m_timestep, threadID); } } }
void dgSolver::TransposeMassMatrix(dgInt32 threadID) { const dgJointInfo* const jointInfoArray = m_jointArray; dgSoaMatrixElement* const massMatrixArray = &m_massMatrix[0]; const dgInt32 step = m_threadCounts; const dgInt32 jointCount = m_jointCount; for (dgInt32 i = threadID; i < jointCount; i += step) { const dgInt32 index = i * DG_SOA_WORD_GROUP_SIZE; const dgInt32 rowCount = jointInfoArray[index].m_pairCount; const dgInt32 rowSoaStart = dgAtomicExchangeAndAdd(&m_soaRowsCount, rowCount); m_soaRowStart[i] = rowSoaStart; for (dgInt32 j = 0; j < rowCount; j ++) { dgSoaMatrixElement* const row = &massMatrixArray[rowSoaStart + j]; TransposeRow (row, &jointInfoArray[index], j); } } }
void dgWorldDynamicUpdate::UpdateBodyVelocityParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart]; dgFloat32 maxAccNorm2 = DG_SOLVER_MAX_ERROR * DG_SOLVER_MAX_ERROR; //dgFloat32 invTimestepSrc = dgFloat32 (1.0f) / syncData->m_timestep; dgFloat32 invTimestepSrc = syncData->m_invTimestep; dgVector invTime (invTimestepSrc); dgInt32* const atomicIndex = &syncData->m_atomicIndex; dgVector forceActiveMask ((syncData->m_jointCount <= DG_SMALL_ISLAND_COUNT) ? dgVector (-1, -1, -1, -1) : dgFloat32 (0.0f)); for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_bodyCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgDynamicBody* const body = (dgDynamicBody*) bodyArray[i].m_body; world->ApplyNetTorqueAndForce (body, invTime, maxAccNorm2, forceActiveMask); } }
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::SolverInitInternalForcesParallelKernel (void* const context, void* const worldContext, dgInt32 threadID) { dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context; dgWorld* const world = (dgWorld*) worldContext; const dgIsland* const island = syncData->m_island; dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0]; dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0]; dgInt32* const atomicIndex = &syncData->m_atomicIndex; dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0]; dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart]; dgInt32* const bodyLocks = syncData->m_bodyLocks; for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) { dgJointInfo* const jointInfo = &constraintArray[i]; if (jointInfo->m_joint->m_solverActive) { dgJacobian y0; dgJacobian y1; world->InitJointForce (jointInfo, matrixRow, y0, y1); const dgInt32 m0 = jointInfo->m_m0; const dgInt32 m1 = jointInfo->m_m1; dgAssert (m0 != m1); if (m0) { dgSpinLock(&bodyLocks[m0], false); internalForces[m0].m_linear += y0.m_linear; internalForces[m0].m_angular += y0.m_angular; dgSpinUnlock(&bodyLocks[m0]); } if (m1) { dgSpinLock(&bodyLocks[m1], false); internalForces[m1].m_linear += y1.m_linear; internalForces[m1].m_angular += y1.m_angular; dgSpinUnlock(&bodyLocks[m1]); } } } }
void dgWorldDynamicUpdate::CalculateClusterReactionForces(const dgBodyCluster* const cluster, dgInt32 threadID, dgFloat32 timestep, dgFloat32 maxAccNorm) const { dTimeTrackerEvent(__FUNCTION__); dgWorld* const world = (dgWorld*) this; const dgInt32 bodyCount = cluster->m_bodyCount; // const dgInt32 jointCount = island->m_jointCount; const dgInt32 jointCount = cluster->m_activeJointCount; dgJacobian* const internalForces = &m_solverMemory.m_internalForcesBuffer[cluster->m_bodyStart]; dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*)&world->m_bodiesMemory[0]; dgJointInfo* const constraintArrayPtr = (dgJointInfo*)&world->m_jointsMemory[0]; dgBodyInfo* const bodyArray = &bodyArrayPtr[cluster->m_bodyStart]; dgJointInfo* const constraintArray = &constraintArrayPtr[cluster->m_jointStart]; dgJacobianMatrixElement* const matrixRow = &m_solverMemory.m_jacobianBuffer[cluster->m_rowsStart]; const dgInt32 derivativesEvaluationsRK4 = 4; dgFloat32 invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f); dgFloat32 invStepRK = (dgFloat32(1.0f) / dgFloat32(derivativesEvaluationsRK4)); dgFloat32 timestepRK = timestep * invStepRK; dgFloat32 invTimestepRK = invTimestep * dgFloat32(derivativesEvaluationsRK4); dgAssert(bodyArray[0].m_body == world->m_sentinelBody); dgVector speedFreeze2(world->m_freezeSpeed2 * dgFloat32(0.1f)); dgVector freezeOmega2(world->m_freezeOmega2 * dgFloat32(0.1f)); dgJointAccelerationDecriptor joindDesc; joindDesc.m_timeStep = timestepRK; joindDesc.m_invTimeStep = invTimestepRK; joindDesc.m_firstPassCoefFlag = dgFloat32(0.0f); dgInt32 skeletonCount = 0; dgInt32 skeletonMemorySizeInBytes = 0; dgInt32 lru = dgAtomicExchangeAndAdd(&dgSkeletonContainer::m_lruMarker, 1); dgSkeletonContainer* skeletonArray[DG_MAX_SKELETON_JOINT_COUNT]; dgInt32 memorySizes[DG_MAX_SKELETON_JOINT_COUNT]; for (dgInt32 i = 1; i < bodyCount; i++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; dgSkeletonContainer* const container = body->GetSkeleton(); if (container && (container->m_lru != lru)) { container->m_lru = lru; memorySizes[skeletonCount] = container->GetMemoryBufferSizeInBytes(constraintArray, matrixRow); skeletonMemorySizeInBytes += memorySizes[skeletonCount]; skeletonArray[skeletonCount] = container; skeletonCount++; dgAssert(skeletonCount < dgInt32(sizeof(skeletonArray) / sizeof(skeletonArray[0]))); } } dgInt8* const skeletonMemory = (dgInt8*)dgAlloca(dgVector, skeletonMemorySizeInBytes / sizeof(dgVector)); dgAssert((dgInt64(skeletonMemory) & 0x0f) == 0); skeletonMemorySizeInBytes = 0; for (dgInt32 i = 0; i < skeletonCount; i++) { skeletonArray[i]->InitMassMatrix(constraintArray, matrixRow, &skeletonMemory[skeletonMemorySizeInBytes]); skeletonMemorySizeInBytes += memorySizes[i]; } const dgInt32 passes = world->m_solverMode; for (dgInt32 step = 0; step < derivativesEvaluationsRK4; step++) { for (dgInt32 i = 0; i < jointCount; i++) { dgJointInfo* const jointInfo = &constraintArray[i]; dgConstraint* const constraint = jointInfo->m_joint; joindDesc.m_rowsCount = jointInfo->m_pairCount; joindDesc.m_rowMatrix = &matrixRow[jointInfo->m_pairStart]; constraint->JointAccelerations(&joindDesc); } joindDesc.m_firstPassCoefFlag = dgFloat32(1.0f); dgFloat32 accNorm(maxAccNorm * dgFloat32(2.0f)); for (dgInt32 i = 0; (i < passes) && (accNorm > maxAccNorm); i++) { accNorm = dgFloat32(0.0f); for (dgInt32 j = 0; j < jointCount; j++) { dgJointInfo* const jointInfo = &constraintArray[j]; dgFloat32 accel = CalculateJointForceGaussSeidel(jointInfo, bodyArray, internalForces, matrixRow, maxAccNorm); accNorm = (accel > accNorm) ? accel : accNorm; } } for (dgInt32 j = 0; j < skeletonCount; j++) { skeletonArray[j]->CalculateJointForce(constraintArray, bodyArray, internalForces, matrixRow); } if (timestepRK != dgFloat32(0.0f)) { dgVector timestep4(timestepRK); for (dgInt32 i = 1; i < bodyCount; i++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; dgAssert(body->m_index == i); if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) { const dgJacobian& forceAndTorque = internalForces[i]; dgVector force(body->m_externalForce + forceAndTorque.m_linear); dgVector torque(body->m_externalTorque + forceAndTorque.m_angular); dgVector velocStep((force.Scale4(body->m_invMass.m_w)) * timestep4); dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4); body->m_veloc += velocStep; body->m_omega += omegaStep; dgAssert(body->m_veloc.m_w == dgFloat32(0.0f)); dgAssert(body->m_omega.m_w == dgFloat32(0.0f)); } } } else { for (dgInt32 i = 1; i < bodyCount; i++) { dgDynamicBody* const body = (dgDynamicBody*)bodyArray[i].m_body; const dgVector& linearMomentum = internalForces[i].m_linear; const dgVector& angularMomentum = internalForces[i].m_angular; body->m_veloc += linearMomentum.Scale4(body->m_invMass.m_w); body->m_omega += body->m_invWorldInertiaMatrix.RotateVector(angularMomentum); } } } dgInt32 hasJointFeeback = 0; if (timestepRK != dgFloat32(0.0f)) { for (dgInt32 i = 0; i < jointCount; i++) { dgJointInfo* const jointInfo = &constraintArray[i]; dgConstraint* const constraint = jointInfo->m_joint; const dgInt32 first = jointInfo->m_pairStart; const dgInt32 count = jointInfo->m_pairCount; for (dgInt32 j = 0; j < count; j++) { dgJacobianMatrixElement* const row = &matrixRow[j + first]; dgFloat32 val = row->m_force; dgAssert(dgCheckFloat(val)); row->m_jointFeebackForce->m_force = val; row->m_jointFeebackForce->m_impact = row->m_maxImpact * timestepRK; } hasJointFeeback |= (constraint->m_updaFeedbackCallback ? 1 : 0); } const dgVector invTime(invTimestep); const dgVector maxAccNorm2(maxAccNorm * maxAccNorm); for (dgInt32 i = 1; i < bodyCount; i++) { dgBody* const body = bodyArray[i].m_body; CalculateNetAcceleration(body, invTime, maxAccNorm2); } if (hasJointFeeback) { for (dgInt32 i = 0; i < jointCount; i++) { if (constraintArray[i].m_joint->m_updaFeedbackCallback) { constraintArray[i].m_joint->m_updaFeedbackCallback(*constraintArray[i].m_joint, timestep, threadID); } } } } 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)); body->m_accel = dgVector::m_zero; body->m_alpha = dgVector::m_zero; } } }