void dgSolver::InitJacobianMatrix()
{
m_jacobianMatrixRowAtomicIndex = 0;
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(InitJacobianMatrixKernel, this, NULL, "dgSolver::InitJacobianMatrix");
}
m_world->SynchronizationBarrier();
dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgBodyJacobianPair* const bodyJacobiansPairs = m_bodyJacobiansPairs;
const dgInt32 entryCount = m_cluster->m_jointCount * 2;
dgSort(bodyJacobiansPairs, entryCount, CompareBodyJointsPairs);
for (dgInt32 i = entryCount - 1; i >= 0; i--) {
dgInt32 index = bodyJacobiansPairs[i].m_bodyIndex;
bodyProxyArray[index].m_jointStart = i;
}
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(InitInternalForcesKernel, this, NULL, "dgSolver::InitInternalForces");
}
m_world->SynchronizationBarrier();
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
internalForces[0].m_linear = m_zero;
internalForces[0].m_angular = m_zero;
dgJointInfo* const jointArray = m_jointArray;
dgSort(jointArray, m_cluster->m_jointCount, CompareJointInfos);
const dgInt32 jointCount = m_jointCount * DG_SOA_WORD_GROUP_SIZE;
for (dgInt32 i = m_cluster->m_jointCount; i < jointCount; i++) {
memset(&jointArray[i], 0, sizeof(dgJointInfo));
}
dgInt32 size = 0;
for (dgInt32 i = 0; i < jointCount; i += DG_WORK_GROUP_SIZE) {
const dgConstraint* const joint1 = jointArray[i + DG_WORK_GROUP_SIZE - 1].m_joint;
if (joint1) {
if (!(joint1->GetBody0()->m_resting & joint1->GetBody1()->m_resting)) {
const dgConstraint* const joint0 = jointArray[i].m_joint;
if (joint0->GetBody0()->m_resting & joint0->GetBody1()->m_resting) {
SortWorkGroup(i);
}
}
} else {
SortWorkGroup(i);
}
size += jointArray[i].m_pairCount;
}
m_massMatrix.ResizeIfNecessary(size);
m_soaRowsCount = 0;
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(TransposeMassMatrixKernel, this, NULL, "dgSolver::TransposeMassMatrix");
}
m_world->SynchronizationBarrier();
}
void dgSolver::InitJacobianMatrix()
{
DG_TRACKTIME();
m_jacobianMatrixRowAtomicIndex = 0;
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
memset(internalForces, 0, m_cluster->m_bodyCount * sizeof (dgJacobian));
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(InitJacobianMatrixKernel, this, NULL, "dgSolver::InitJacobianMatrix");
}
m_world->SynchronizationBarrier();
dgJointInfo* const jointArray = m_jointArray;
// dgSort(jointArray, m_cluster->m_jointCount, CompareJointInfos);
dgParallelSort(*m_world, jointArray, m_cluster->m_jointCount, CompareJointInfos);
const dgInt32 jointCount = m_jointCount * DG_SOA_WORD_GROUP_SIZE;
for (dgInt32 i = m_cluster->m_jointCount; i < jointCount; i++) {
memset(&jointArray[i], 0, sizeof(dgJointInfo));
}
dgInt32 size = 0;
for (dgInt32 i = 0; i < jointCount; i += DG_SOA_WORD_GROUP_SIZE) {
const dgConstraint* const joint1 = jointArray[i + DG_SOA_WORD_GROUP_SIZE - 1].m_joint;
if (joint1) {
if (!(joint1->GetBody0()->m_resting & joint1->GetBody1()->m_resting)) {
const dgConstraint* const joint0 = jointArray[i].m_joint;
if (joint0->GetBody0()->m_resting & joint0->GetBody1()->m_resting) {
SortWorkGroup(i);
}
}
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
dgConstraint* const joint = jointArray[i + j].m_joint;
joint->SetIndex (i + j);
}
} else {
SortWorkGroup(i);
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
dgConstraint* const joint = jointArray[i + j].m_joint;
if (joint) {
joint->SetIndex (i + j);
}
}
}
size += jointArray[i].m_pairCount;
}
m_massMatrix.ResizeIfNecessary(size);
m_soaRowsCount = 0;
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(TransposeMassMatrixKernel, this, NULL, "dgSolver::TransposeMassMatrix");
}
m_world->SynchronizationBarrier();
}
