void dgSolver::CalculateJointForces(const dgBodyCluster& cluster, dgBodyInfo* const bodyArray, dgJointInfo* const jointArray, dgFloat32 timestep)
{
DG_TRACKTIME();
m_cluster = &cluster;
m_bodyArray = bodyArray;
m_jointArray = jointArray;
m_timestep = timestep;
m_invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f);
m_invStepRK = dgFloat32 (0.25f);
m_timestepRK = m_timestep * m_invStepRK;
m_invTimestepRK = m_invTimestep * dgFloat32 (4.0f);
m_threadCounts = m_world->GetThreadCount();
m_solverPasses = m_world->GetSolverIterations();
dgInt32 mask = -dgInt32(DG_SOA_WORD_GROUP_SIZE - 1);
m_jointCount = ((m_cluster->m_jointCount + DG_SOA_WORD_GROUP_SIZE - 1) & mask) / DG_SOA_WORD_GROUP_SIZE;
m_bodyProxyArray = dgAlloca(dgBodyProxy, cluster.m_bodyCount);
m_soaRowStart = dgAlloca(dgInt32, cluster.m_jointCount / DG_SOA_WORD_GROUP_SIZE + 1);
InitWeights();
InitBodyArray();
InitJacobianMatrix();
CalculateForces();
}
void dgWorld::StepDynamics (dgFloat32 timestep)
{
//SerializeToFile ("xxx.bin");
dgAssert (m_inUpdate == 0);
dgAssert (GetThreadCount() >= 1);
m_inUpdate ++;
DG_TRACKTIME(__FUNCTION__);
UpdateSkeletons();
UpdateBroadphase(timestep);
UpdateDynamics (timestep);
if (m_listeners.GetCount()) {
for (dgListenerList::dgListNode* node = m_listeners.GetFirst(); node; node = node->GetNext()) {
dgListener& listener = node->GetInfo();
if (listener.m_onPostUpdate) {
listener.m_onPostUpdate(this, listener.m_userData, timestep);
}
}
}
m_inUpdate --;
}
void dgSolver::CalculateForces()
{
DG_TRACKTIME();
m_firstPassCoef = dgFloat32(0.0f);
const dgInt32 passes = m_solverPasses;
const dgInt32 threadCounts = m_world->GetThreadCount();
InitSkeletons();
for (dgInt32 step = 0; step < 4; step++) {
CalculateJointsAcceleration();
dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32(2.0f);
for (dgInt32 k = 0; (k < passes) && (accNorm > DG_SOLVER_MAX_ERROR); k++) {
CalculateJointsForce();
accNorm = dgFloat32(0.0f);
for (dgInt32 i = 0; i < threadCounts; i++) {
accNorm = dgMax(accNorm, m_accelNorm[i]);
}
}
UpdateSkeletons();
IntegrateBodiesVelocity();
}
UpdateForceFeedback();
dgInt32 hasJointFeeback = 0;
for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i++) {
hasJointFeeback |= m_hasJointFeeback[i];
}
CalculateBodiesAcceleration();
if (hasJointFeeback) {
UpdateKinematicFeedback();
}
}
void dgSolver::InitBodyArray()
{
DG_TRACKTIME();
for (dgInt32 i = 0; i < m_threadCounts; i++) {
m_world->QueueJob(InitBodyArrayKernel, this, NULL, "dgSolver::InitBodyArray");
}
m_world->SynchronizationBarrier();
m_bodyProxyArray->m_invWeight = dgFloat32 (1.0f);
}
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();
}
void dgWorld::RunStep ()
{
static int zzzz;
if (zzzz == 50) {
DG_START_RECORDING("../../../../sdk/dProfiler/xxxx.tt");
}
if (zzzz == 500) {
DG_STOP_RECORDING();
}
zzzz++;
DG_TRACKTIME(__FUNCTION__);
dgUnsigned64 timeAcc = dgGetTimeInMicrosenconds();
dgFloat32 step = m_savetimestep / m_numberOfSubsteps;
for (dgUnsigned32 i = 0; i < m_numberOfSubsteps; i ++) {
dgInterlockedExchange(&m_delayDelateLock, 1);
StepDynamics (step);
dgInterlockedExchange(&m_delayDelateLock, 0);
dgDeadBodies& bodyList = *this;
dgDeadJoints& jointList = *this;
jointList.DestroyJoints (*this);
bodyList.DestroyBodies (*this);
}
const dgBodyMasterList* const masterList = this;
dgBodyMasterList::dgListNode* node = masterList->GetFirst();
const dgInt32 threadsCount = GetThreadCount();
for (dgInt32 i = 0; i < threadsCount; i++) {
QueueJob(UpdateTransforms, this, node, "dgWorld::UpdateTransforms");
node = node ? node->GetNext() : NULL;
}
SynchronizationBarrier();
if (m_postUpdateCallback) {
m_postUpdateCallback (this, m_savetimestep);
}
m_lastExecutionTime = (dgGetTimeInMicrosenconds() - timeAcc) * dgFloat32 (1.0e-6f);
if (!m_concurrentUpdate) {
m_mutex.Release();
}
}
void dgSolver::InitWeights()
{
DG_TRACKTIME();
const dgJointInfo* const jointArray = m_jointArray;
const dgInt32 jointCount = m_cluster->m_jointCount;
dgBodyProxy* const weight = m_bodyProxyArray;
memset(m_bodyProxyArray, 0, m_cluster->m_bodyCount * sizeof(dgBodyProxy));
for (dgInt32 i = 0; i < jointCount; i++) {
const dgJointInfo* const jointInfo = &jointArray[i];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
weight[m0].m_weight += dgFloat32(1.0f);
weight[m1].m_weight += dgFloat32(1.0f);
}
m_bodyProxyArray[0].m_weight = dgFloat32(1.0f);
dgFloat32 extraPasses = dgFloat32(0.0f);
const dgInt32 bodyCount = m_cluster->m_bodyCount;
dgSkeletonList& skeletonList = *m_world;
const dgInt32 lru = skeletonList.m_lruMarker;
skeletonList.m_lruMarker += 1;
m_skeletonCount = 0;
for (dgInt32 i = 1; i < bodyCount; i++) {
extraPasses = dgMax(weight[i].m_weight, extraPasses);
dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body;
dgSkeletonContainer* const container = body->GetSkeleton();
if (container && (container->GetLru() != lru)) {
container->SetLru(lru);
m_skeletonArray[m_skeletonCount] = container;
m_skeletonCount++;
}
}
const dgInt32 conectivity = 7;
m_solverPasses += 2 * dgInt32(extraPasses) / conectivity + 1;
}
void dgWorld::UpdateSkeletons()
{
DG_TRACKTIME(__FUNCTION__);
dgSkeletonList& skelManager = *this;
if (skelManager.m_skelListIsDirty) {
skelManager.m_skelListIsDirty = false;
dgSkeletonList::Iterator iter(skelManager);
for (iter.Begin(); iter; iter++) {
dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo();
delete skeleton;
}
skelManager.RemoveAll();
m_dynamicsLru = m_dynamicsLru + 1;
dgUnsigned32 lru = m_dynamicsLru;
dgBodyMasterList& masterList = *this;
m_solverJacobiansMemory.ResizeIfNecessary((2 * (masterList.m_constraintCount + 1024)) * sizeof (dgBilateralConstraint*));
dgBilateralConstraint** const jointList = (dgBilateralConstraint**)&m_solverJacobiansMemory[0];
dgInt32 jointCount = 0;
for (dgBodyMasterList::dgListNode* node = masterList.GetFirst(); node; node = node->GetNext()) {
const dgBodyMasterListRow& graphNode = node->GetInfo();
dgBody* const srcBody = graphNode.GetBody();
for (dgBodyMasterListRow::dgListNode* jointNode = srcBody->m_masterNode->GetInfo().GetLast(); jointNode; jointNode = jointNode->GetPrev()) {
dgBodyMasterListCell* const cell = &jointNode->GetInfo();
dgConstraint* const constraint = cell->m_joint;
dgAssert(constraint);
dgAssert((constraint->m_body0 == srcBody) || (constraint->m_body1 == srcBody));
dgAssert((constraint->m_body0 == cell->m_bodyNode) || (constraint->m_body1 == cell->m_bodyNode));
if (constraint->IsBilateral() && (constraint->m_solverModel < 2) && (constraint->m_dynamicsLru != lru)) {
constraint->m_dynamicsLru = lru;
jointList[jointCount] = (dgBilateralConstraint*)constraint;
jointCount++;
}
}
}
dgSortIndirect(jointList, jointCount, CompareJointByInvMass);
const dgInt32 poolSize = 1024 * 4;
dgBilateralConstraint* loopJoints[64];
dgSkeletonContainer::dgNode* queuePool[poolSize];
m_dynamicsLru = m_dynamicsLru + 1;
lru = m_dynamicsLru;
for (dgInt32 i = 0; i < jointCount; i++) {
dgBilateralConstraint* const constraint = jointList[i];
if (constraint->m_dynamicsLru != lru) {
dgQueue<dgSkeletonContainer::dgNode*> queue(queuePool, poolSize);
dgInt32 loopCount = 0;
dgDynamicBody* const rootBody = (dgDynamicBody*)((constraint->GetBody0()->GetInvMass().m_w < constraint->GetBody1()->GetInvMass().m_w) ? constraint->GetBody0() : constraint->GetBody1());
dgSkeletonContainer* const skeleton = CreateNewtonSkeletonContainer(rootBody);
dgSkeletonContainer::dgNode* const rootNode = skeleton->GetRoot();
if (rootBody->GetInvMass().m_w == dgFloat32 (0.0f)) {
if (constraint->IsBilateral() && (constraint->m_dynamicsLru != lru)) {
constraint->m_dynamicsLru = lru;
dgDynamicBody* const childBody = (dgDynamicBody*)((constraint->GetBody0() == rootBody) ? constraint->GetBody1() : constraint->GetBody0());
if (!constraint->m_solverModel) {
if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) {
childBody->m_dynamicsLru = lru;
dgSkeletonContainer::dgNode* const node = skeleton->AddChild((dgBilateralConstraint*)constraint, rootNode);
queue.Insert(node);
}
}
}
} else {
queue.Insert(rootNode);
rootBody->m_dynamicsLru = lru;
}
while (!queue.IsEmpty()) {
dgInt32 count = queue.m_firstIndex - queue.m_lastIndex;
if (count < 0) {
count += queue.m_mod;
}
dgInt32 index = queue.m_lastIndex;
queue.Reset();
for (dgInt32 j = 0; j < count; j++) {
dgSkeletonContainer::dgNode* const parentNode = queue.m_pool[index];
dgDynamicBody* const parentBody = skeleton->GetBody(parentNode);
for (dgBodyMasterListRow::dgListNode* jointNode1 = parentBody->m_masterNode->GetInfo().GetFirst(); jointNode1; jointNode1 = jointNode1->GetNext()) {
dgBodyMasterListCell* const cell1 = &jointNode1->GetInfo();
dgConstraint* const constraint1 = cell1->m_joint;
if (constraint1->IsBilateral() && (constraint1->m_dynamicsLru != lru)) {
constraint1->m_dynamicsLru = lru;
dgDynamicBody* const childBody = (dgDynamicBody*)((constraint1->GetBody0() == parentBody) ? constraint1->GetBody1() : constraint1->GetBody0());
if (!constraint1->m_solverModel) {
if ((childBody->m_dynamicsLru != lru) && (childBody->GetInvMass().m_w != dgFloat32(0.0f))) {
childBody->m_dynamicsLru = lru;
dgSkeletonContainer::dgNode* const childNode = skeleton->AddChild((dgBilateralConstraint*)constraint1, parentNode);
queue.Insert(childNode);
} else if (loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) {
loopJoints[loopCount] = (dgBilateralConstraint*)constraint1;
loopCount++;
}
} else if ((constraint1->m_solverModel != 2) && loopCount < (sizeof (loopJoints) / sizeof(loopJoints[0]))) {
loopJoints[loopCount] = (dgBilateralConstraint*)constraint1;
loopCount++;
}
}
}
index++;
if (index >= queue.m_mod) {
index = 0;
}
}
}
skeleton->Finalize(loopCount, loopJoints);
}
}
}
dgSkeletonList::Iterator iter(skelManager);
for (iter.Begin(); iter; iter++) {
dgSkeletonContainer* const skeleton = iter.GetNode()->GetInfo();
skeleton->ClearSelfCollision();
}
}
