DG_INLINE dgFloat32 dgSolver::CalculateJointForce(const dgJointInfo* const jointInfo, dgSoaMatrixElement* const massMatrix, const dgSoaFloat* const internalForces) const
{
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
dgSoaFloat preconditioner0;
dgSoaFloat preconditioner1;
dgSoaFloat accNorm(m_soaZero);
dgSoaFloat normalForce[DG_CONSTRAINT_MAX_ROWS + 1];
const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
for (dgInt32 i = 0; i < DG_SOA_WORD_GROUP_SIZE; i++) {
const dgInt32 m0 = jointInfo[i].m_m0;
const dgInt32 m1 = jointInfo[i].m_m1;
forceM0.m_linear.m_x[i] = internalForces[m0][0];
forceM0.m_linear.m_y[i] = internalForces[m0][1];
forceM0.m_linear.m_z[i] = internalForces[m0][2];
forceM0.m_angular.m_x[i] = internalForces[m0][4];
forceM0.m_angular.m_y[i] = internalForces[m0][5];
forceM0.m_angular.m_z[i] = internalForces[m0][6];
forceM1.m_linear.m_x[i] = internalForces[m1][0];
forceM1.m_linear.m_y[i] = internalForces[m1][1];
forceM1.m_linear.m_z[i] = internalForces[m1][2];
forceM1.m_angular.m_x[i] = internalForces[m1][4];
forceM1.m_angular.m_y[i] = internalForces[m1][5];
forceM1.m_angular.m_z[i] = internalForces[m1][6];
preconditioner0[i] = jointInfo[i].m_preconditioner0 * bodyProxyArray[m0].m_weight;
preconditioner1[i] = jointInfo[i].m_preconditioner1 * bodyProxyArray[m1].m_weight;
}
forceM0.m_linear.m_x = forceM0.m_linear.m_x * preconditioner0;
forceM0.m_linear.m_y = forceM0.m_linear.m_y * preconditioner0;
forceM0.m_linear.m_z = forceM0.m_linear.m_z * preconditioner0;
forceM0.m_angular.m_x = forceM0.m_angular.m_x * preconditioner0;
forceM0.m_angular.m_y = forceM0.m_angular.m_y * preconditioner0;
forceM0.m_angular.m_z = forceM0.m_angular.m_z * preconditioner0;
forceM1.m_linear.m_x = forceM1.m_linear.m_x * preconditioner1;
forceM1.m_linear.m_y = forceM1.m_linear.m_y * preconditioner1;
forceM1.m_linear.m_z = forceM1.m_linear.m_z * preconditioner1;
forceM1.m_angular.m_x = forceM1.m_angular.m_x * preconditioner1;
forceM1.m_angular.m_y = forceM1.m_angular.m_y * preconditioner1;
forceM1.m_angular.m_z = forceM1.m_angular.m_z * preconditioner1;
const dgInt32 rowsCount = jointInfo->m_pairCount;
normalForce[0] = m_soaOne;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[j];
dgSoaFloat a;
a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z);
a = a.MulSub(row->m_force, row->m_diagDamp);
dgSoaFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a));
dgSoaFloat frictionNormal;
for (dgInt32 k = 0; k < DG_SOA_WORD_GROUP_SIZE; k++) {
dgAssert(row->m_normalForceIndex.m_i[k] >= -1);
dgAssert(row->m_normalForceIndex.m_i[k] <= rowsCount);
const dgInt32 frictionIndex = dgInt32(row->m_normalForceIndex.m_i[k] + 1);
frictionNormal[k] = normalForce[frictionIndex][k];
}
dgSoaFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgSoaFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
a = a.AndNot((f > upperFrictionForce) | (f < lowerFrictionForce));
f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce);
accNorm = accNorm + a * a;
dgSoaFloat deltaForce(f - row->m_force);
row->m_force = f;
normalForce[j + 1] = f;
dgSoaFloat deltaForce0(deltaForce * preconditioner0);
dgSoaFloat deltaForce1(deltaForce * preconditioner1);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1);
}
const dgFloat32 tol = dgFloat32(0.5f);
const dgFloat32 tol2 = tol * tol;
dgSoaFloat maxAccel(accNorm);
for (dgInt32 i = 0; (i < 4) && (maxAccel.AddHorizontal() > tol2); i++) {
maxAccel = m_soaZero;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[j];
dgSoaFloat a;
a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z);
a = a.MulSub(row->m_force, row->m_diagDamp);
dgSoaFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a));
dgSoaFloat frictionNormal;
for (dgInt32 k = 0; k < DG_SOA_WORD_GROUP_SIZE; k++) {
dgAssert(row->m_normalForceIndex.m_i[k] >= -1);
dgAssert(row->m_normalForceIndex.m_i[k] <= rowsCount);
const dgInt32 frictionIndex = dgInt32 (row->m_normalForceIndex.m_i[k] + 1);
frictionNormal[k] = normalForce[frictionIndex][k];
}
dgSoaFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgSoaFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
a = a.AndNot((f > upperFrictionForce) | (f < lowerFrictionForce));
f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce);
maxAccel = maxAccel + a * a;
dgSoaFloat deltaForce(f - row->m_force);
row->m_force = f;
normalForce[j + 1] = f;
dgSoaFloat deltaForce0(deltaForce * preconditioner0);
dgSoaFloat deltaForce1(deltaForce * preconditioner1);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1);
}
}
return accNorm.AddHorizontal();
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceDanzig(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgFloat32 massMatrix[DG_CONSTRAINT_MAX_ROWS * DG_CONSTRAINT_MAX_ROWS];
dgFloat32 f[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 b[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 low[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 high[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
cacheForce[0] = dgFloat32(1.0f);
cacheForce[1] = dgFloat32(1.0f);
cacheForce[2] = dgFloat32(1.0f);
cacheForce[3] = dgFloat32(1.0f);
dgFloat32* const normalForce = &cacheForce[4];
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgAssert (rowsCount <= DG_CONSTRAINT_MAX_ROWS);
dgVector accNorm(dgVector::m_zero);
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row_i = &matrixRow[index + i];
dgFloat32* const massMatrixRow = &massMatrix[rowsCount * i];
dgJacobian JMinvM0(row_i->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row_i->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * row_i->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_i->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_i->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_i->m_Jt.m_jacobianM1.m_angular);
diag = diag.AddHorizontal();
dgFloat32 val = diag.GetScalar() + row_i->m_diagDamp;
dgAssert(val > dgFloat32(0.0f));
massMatrixRow[i] = val;
for (dgInt32 j = i + 1; j < rowsCount; j++) {
const dgJacobianMatrixElement* const row_j = &matrixRow[index + j];
dgVector offDiag(JMinvM0.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM1.m_angular);
offDiag = offDiag.AddHorizontal();
dgFloat32 val1 = offDiag.GetScalar();
massMatrixRow[j] = val1;
massMatrix[j * rowsCount + i] = val1;
}
}
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgJacobian JMinvM0(row->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * y0.m_linear + JMinvM0.m_angular * y0.m_angular +
JMinvM1.m_linear * y1.m_linear + JMinvM1.m_angular * y1.m_angular);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
normalForce[i] = row->m_force;
dgFloat32 frictionNormal = normalForce[frictionIndex];
dgFloat32 lowerFrictionForce = frictionNormal * row->m_lowerBoundFrictionCoefficent;
dgFloat32 upperFrictionForce = frictionNormal * row->m_upperBoundFrictionCoefficent;
//f[i] = row->m_force;
f[i] = dgFloat32 (0.0f);
b[i] = accel.GetScalar();
low[i] = lowerFrictionForce - row->m_force;
high[i] = upperFrictionForce - row->m_force;
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
accNorm = accNorm.GetMax(accel.Abs());
}
dgSolveDantzigLCP(rowsCount, massMatrix, f, b, low, high);
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgVector jointForce (f[i]);
row->m_force = f[i] + row->m_force;
linearM0 += row->m_Jt.m_jacobianM0.m_linear * jointForce;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * jointForce;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * jointForce;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * jointForce;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
return accNorm.GetScalar();
}
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;
}
}
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceJacobi1(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgVector accNorm(dgVector::m_zero);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
cacheForce[0] = dgFloat32(1.0f);
cacheForce[1] = dgFloat32(1.0f);
cacheForce[2] = dgFloat32(1.0f);
cacheForce[3] = dgFloat32(1.0f);
dgFloat32* const normalForce = &cacheForce[4];
dgVector maxAccel(1.0e10f);
dgFloat32 prevError = dgFloat32(1.0e20f);
dgVector firstPass(dgVector::m_one);
for (dgInt32 j = 0; (j < 5) && (maxAccel.GetScalar() > restAcceleration) && (prevError - maxAccel.GetScalar()) > dgFloat32(1.0e-2f); j++) {
prevError = maxAccel.GetScalar();
maxAccel = dgVector::m_zero;
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
dgVector diag(row->m_JMinv.m_jacobianM0.m_linear * linearM0 + row->m_JMinv.m_jacobianM0.m_angular * angularM0 +
row->m_JMinv.m_jacobianM1.m_linear * linearM1 + row->m_JMinv.m_jacobianM1.m_angular * angularM1);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
const dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
const dgFloat32 frictionNormal = normalForce[frictionIndex];
dgVector lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgVector upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
dgVector accelAbs(accel.Abs());
maxAccel = maxAccel.GetMax(accelAbs);
accNorm = accNorm.GetMax(accelAbs * firstPass);
dgAssert(maxAccel.m_x >= dgAbsf(accel.m_x));
dgFloat32 f = (force.GetMax(lowerFrictionForce).GetMin(upperFrictionForce)).GetScalar();
dgVector deltaForce(f - row->m_force);
row->m_force = f;
normalForce[i] = f;
dgVector deltaforce0(scale0 * deltaForce);
dgVector deltaforce1(scale1 * deltaForce);
linearM0 += row->m_Jt.m_jacobianM0.m_linear * deltaforce0;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * deltaforce0;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * deltaforce1;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * deltaforce1;
}
firstPass = dgVector::m_zero;
}
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
row->m_maxImpact = dgMax(dgAbsf(row->m_force), row->m_maxImpact);
}
return accNorm.GetScalar();
}