void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
{
//btPersistentManifold* manifold = 0;
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* manifold= manifoldPtr[i];
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
if (!fcA && !fcB)
{
//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
convertContact(manifold,infoGlobal);
} else
{
convertMultiBodyContact(manifold,infoGlobal);
}
}
//also convert the multibody constraints, if any
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
{
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
m_data.m_fixedBodyId = m_fixedBodyId;
c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
}
}
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
{
B3_PROFILE("GPU solveGroupCacheFriendlySetup");
batchConstraints.resize(numConstraints);
m_gpuData->m_gpuBatchConstraints->resize(numConstraints);
m_staticIdx = -1;
m_maxOverrideNumSolverIterations = 0;
/* m_gpuData->m_gpuBodies->resize(numBodies);
m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies);
b3OpenCLArray<b3InertiaData> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue);
gpuInertias.resize(numBodies);
gpuInertias.copyFromHostPointer(inertias,numBodies);
*/
m_gpuData->m_gpuSolverBodies->resize(numBodies);
m_tmpSolverBodyPool.resize(numBodies);
{
if (useGpuInitSolverBodies)
{
B3_PROFILE("m_initSolverBodiesKernel");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initSolverBodiesKernel, "m_initSolverBodiesKernel");
launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setConst(numBodies);
launcher.launch1D(numBodies);
clFinish(m_gpuData->m_queue);
// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
}
else
{
gpuBodies->copyToHost(m_gpuData->m_cpuBodies);
for (int i = 0; i < numBodies; i++)
{
b3RigidBodyData& body = m_gpuData->m_cpuBodies[i];
b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i];
initSolverBody(i, &solverBody, &body);
solverBody.m_originalBodyIndex = i;
}
m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool);
}
}
// int totalBodies = 0;
int totalNumRows = 0;
//b3RigidBody* rb0=0,*rb1=0;
//if (1)
{
{
// int i;
m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
// b3OpenCLArray<b3GpuGenericConstraint> gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue);
if (useGpuInfo1)
{
B3_PROFILE("info1 and init batchConstraint");
m_gpuData->m_gpuConstraintInfo1->resize(numConstraints);
if (1)
{
B3_PROFILE("getInfo1Kernel");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo1Kernel, "m_getInfo1Kernel");
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setConst(numConstraints);
launcher.launch1D(numConstraints);
clFinish(m_gpuData->m_queue);
}
if (m_gpuData->m_batchSizes.size() == 0)
{
B3_PROFILE("initBatchConstraintsKernel");
m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints);
unsigned int total = 0;
m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1, *m_gpuData->m_gpuConstraintRowOffsets, numConstraints, &total);
unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints - 1);
totalNumRows = total + lastElem;
{
B3_PROFILE("init batch constraints");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initBatchConstraintsKernel, "m_initBatchConstraintsKernel");
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setConst(numConstraints);
launcher.launch1D(numConstraints);
clFinish(m_gpuData->m_queue);
}
//assume the batching happens on CPU, so copy the data
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
}
}
else
{
totalNumRows = 0;
gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
//calculate the total number of contraint rows
for (int i = 0; i < numConstraints; i++)
{
unsigned int& info1 = m_tmpConstraintSizesPool[i];
// unsigned int info1;
if (m_gpuData->m_cpuConstraints[i].isEnabled())
{
m_gpuData->m_cpuConstraints[i].getInfo1(&info1, &m_gpuData->m_cpuBodies[0]);
}
else
{
info1 = 0;
}
totalNumRows += info1;
}
m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool);
}
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
m_gpuData->m_gpuConstraintRows->resize(totalNumRows);
// b3GpuConstraintArray verify;
if (useGpuInfo2)
{
{
B3_PROFILE("getInfo2Kernel");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo2Kernel, "m_getInfo2Kernel");
launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setBuffer(gpuInertias->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
launcher.setConst(infoGlobal.m_timeStep);
launcher.setConst(infoGlobal.m_erp);
launcher.setConst(infoGlobal.m_globalCfm);
launcher.setConst(infoGlobal.m_damping);
launcher.setConst(infoGlobal.m_numIterations);
launcher.setConst(numConstraints);
launcher.launch1D(numConstraints);
clFinish(m_gpuData->m_queue);
if (m_gpuData->m_batchSizes.size() == 0)
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
//m_gpuData->m_gpuConstraintRows->copyToHost(verify);
//m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool);
}
}
else
{
gpuInertias->copyToHost(m_gpuData->m_cpuInertias);
///setup the b3SolverConstraints
for (int i = 0; i < numConstraints; i++)
{
const int& info1 = m_tmpConstraintSizesPool[i];
if (info1)
{
int constraintIndex = batchConstraints[i].m_originalConstraintIndex;
int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[constraintIndex];
b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset];
b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i];
b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[constraint.getRigidBodyA()];
//b3RigidBody& rbA = constraint.getRigidBodyA();
// b3RigidBody& rbB = constraint.getRigidBodyB();
b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[constraint.getRigidBodyB()];
int solverBodyIdA = constraint.getRigidBodyA(); //getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias);
int solverBodyIdB = constraint.getRigidBodyB(); //getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias);
b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
if (rbA.m_invMass)
{
batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;
}
else
{
if (!solverBodyIdA)
m_staticIdx = 0;
batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;
}
if (rbB.m_invMass)
{
batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;
}
else
{
if (!solverBodyIdB)
m_staticIdx = 0;
batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;
}
int overrideNumSolverIterations = 0; //constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
int j;
for (j = 0; j < info1; j++)
{
memset(¤tConstraintRow[j], 0, sizeof(b3GpuSolverConstraint));
currentConstraintRow[j].m_angularComponentA.setValue(0, 0, 0);
currentConstraintRow[j].m_angularComponentB.setValue(0, 0, 0);
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_cfm = 0.f;
currentConstraintRow[j].m_contactNormal.setValue(0, 0, 0);
currentConstraintRow[j].m_friction = 0.f;
currentConstraintRow[j].m_frictionIndex = 0;
currentConstraintRow[j].m_jacDiagABInv = 0.f;
currentConstraintRow[j].m_lowerLimit = 0.f;
currentConstraintRow[j].m_upperLimit = 0.f;
currentConstraintRow[j].m_originalContactPoint = 0;
currentConstraintRow[j].m_overrideNumSolverIterations = 0;
currentConstraintRow[j].m_relpos1CrossNormal.setValue(0, 0, 0);
currentConstraintRow[j].m_relpos2CrossNormal.setValue(0, 0, 0);
currentConstraintRow[j].m_rhs = 0.f;
currentConstraintRow[j].m_rhsPenetration = 0.f;
currentConstraintRow[j].m_solverBodyIdA = 0;
currentConstraintRow[j].m_solverBodyIdB = 0;
currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
currentConstraintRow[j].m_upperLimit = B3_INFINITY;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
}
bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
b3GpuConstraintInfo2 info2;
info2.fps = 1.f / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxis = 0;
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
info2.rowskip = sizeof(b3GpuSolverConstraint) / sizeof(b3Scalar); //check this
///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar
b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3GpuSolverConstraint));
info2.m_constraintError = ¤tConstraintRow->m_rhs;
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
info2.cfm = ¤tConstraintRow->m_cfm;
info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
info2.m_numIterations = infoGlobal.m_numIterations;
m_gpuData->m_cpuConstraints[i].getInfo2(&info2, &m_gpuData->m_cpuBodies[0]);
///finalize the constraint setup
for (j = 0; j < info1; j++)
{
b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j];
if (solverConstraint.m_upperLimit >= m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold())
{
solverConstraint.m_upperLimit = m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold();
}
if (solverConstraint.m_lowerLimit <= -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold())
{
solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold();
}
// solverConstraint.m_originalContactPoint = constraint;
b3Matrix3x3& invInertiaWorldA = m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld;
{
//b3Vector3 angularFactorA(1,1,1);
const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA;
}
b3Matrix3x3& invInertiaWorldB = m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld;
{
const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint.getRigidBodyB().getAngularFactor();
}
{
//it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
//because it gets multiplied iMJlB
b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass;
b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal;
b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal?
b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal;
b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
b3Scalar fsum = b3Fabs(sum);
b3Assert(fsum > B3_EPSILON);
solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f;
}
///fix rhs
///todo: add force/torque accelerators
{
b3Scalar rel_vel;
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel);
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel);
rel_vel = vel1Dotn + vel2Dotn;
b3Scalar restitution = 0.f;
b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2
b3Scalar velocityError = restitution - rel_vel * info2.m_damping;
b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
}
m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool);
m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool);
if (m_gpuData->m_batchSizes.size() == 0)
m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
else
m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool);
} //end useGpuInfo2
}
#ifdef B3_SUPPORT_CONTACT_CONSTRAINTS
{
int i;
for (i = 0; i < numManifolds; i++)
{
b3Contact4& manifold = manifoldPtr[i];
convertContact(bodies, inertias, &manifold, infoGlobal);
}
}
#endif //B3_SUPPORT_CONTACT_CONSTRAINTS
}
// b3ContactSolverInfo info = infoGlobal;
// int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
// int numConstraintPool = m_tmpSolverContactConstraintPool.size();
// int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
return 0.f;
}
static void ImportHistory(MCONTACT hContact, PROTOACCOUNT **protocol, int protoCount)
{
// Is it contats history import?
MCONTACT hDst = (protoCount == 0) ? convertContact(hContact) : NULL; //system history import
// OK to import this chain?
if (hDst == INVALID_CONTACT_ID) {
nSkippedContacts++;
return;
}
int i = 0, skipAll = 0;
DWORD cbAlloc = 4096;
BYTE* eventBuf = (PBYTE)mir_alloc(cbAlloc);
// Get the start of the event chain
HANDLE hEvent = srcDb->FindFirstEvent(hContact);
while (hEvent) {
int skip = 0;
// Copy the event and import it
DBEVENTINFO dbei = { sizeof(DBEVENTINFO) };
dbei.cbBlob = srcDb->GetBlobSize(hEvent);
if (dbei.cbBlob > cbAlloc) {
cbAlloc = dbei.cbBlob + 4096 - dbei.cbBlob % 4096;
eventBuf = (PBYTE)mir_realloc(eventBuf, cbAlloc);
}
dbei.pBlob = eventBuf;
if (!srcDb->GetEvent(hEvent, &dbei)) {
// check protocols during system history import
if (hDst == NULL) {
skipAll = 1;
for (int i = 0; i < protoCount; i++)
if (!strcmp(dbei.szModule, protocol[i]->szModuleName)) {
skipAll = 0;
break;
}
skip = skipAll;
}
// custom filtering
if (!skip && nImportOption == IMPORT_CUSTOM) {
BOOL sent = (dbei.flags & DBEF_SENT);
if (dbei.timestamp < (DWORD)dwSinceDate)
skip = 1;
if (!skip) {
if (hDst) {
skip = 1;
switch (dbei.eventType) {
case EVENTTYPE_MESSAGE:
if ((sent ? IOPT_MSGSENT : IOPT_MSGRECV) & nCustomOptions)
skip = 0;
break;
case EVENTTYPE_FILE:
if ((sent ? IOPT_FILESENT : IOPT_FILERECV) & nCustomOptions)
skip = 0;
break;
case EVENTTYPE_URL:
if ((sent ? IOPT_URLSENT : IOPT_URLRECV) & nCustomOptions)
skip = 0;
break;
default:
if ((sent ? IOPT_OTHERSENT : IOPT_OTHERRECV) & nCustomOptions)
skip = 0;
break;
}
}
else if (!(nCustomOptions & IOPT_SYSTEM))
skip = 1;
}
if (skip)
nSkippedEvents++;
}
if (!skip) {
// Check for duplicate entries
if (!IsDuplicateEvent((MCONTACT)hDst, dbei)) {
// Add dbevent
if (dstDb->AddEvent(hDst, &dbei) != NULL)
nMessagesCount++;
else
AddMessage(LPGENT("Failed to add message"));
}
else
nDupes++;
}
}
if (!(i % 10)) {
MSG msg;
if (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE)) {
TranslateMessage(&msg);
DispatchMessage(&msg);
}
}
// skip this chain if needed
if (skipAll)
break;
// Get next event
hEvent = srcDb->FindNextEvent(hContact, hEvent);
i++;
}
mir_free(eventBuf);
}
