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;
}
}
}
