void wheelCallback( float deltax, float deltay)
{
if (!m_mouseButton)
{
if (b3Fabs(deltax)>b3Fabs(deltay))
{
m_azi -= deltax*0.1;
} else
{
//m_cameraDistance -= deltay*0.1;
b3Vector3 fwd = m_cameraTargetPosition-m_cameraPosition;
fwd.normalize();
m_cameraTargetPosition += fwd*deltay*0.1;
}
} else
{
if (b3Fabs(deltax)>b3Fabs(deltay))
{
b3Vector3 fwd = m_cameraTargetPosition-m_cameraPosition;
b3Vector3 side = m_cameraUp.cross(fwd);
side.normalize();
m_cameraTargetPosition += side * deltax*0.1;
} else
{
m_cameraTargetPosition -= m_cameraUp * deltay*0.1;
}
}
}
void wheelCallback( float deltax, float deltay)
{
if (!m_leftMouseButton)
{
if (deltay<0 || m_cameraDistance>1)
{
m_cameraDistance -= deltay*0.1;
if (m_cameraDistance<1)
m_cameraDistance=1;
} else
{
b3Vector3 fwd = m_cameraTargetPosition-m_cameraPosition;
fwd.normalize();
m_cameraTargetPosition += fwd*deltay*WHEEL_MULTIPLIER;
}
} else
{
if (b3Fabs(deltax)>b3Fabs(deltay))
{
b3Vector3 fwd = m_cameraTargetPosition-m_cameraPosition;
b3Vector3 side = m_cameraUp.cross(fwd);
side.normalize();
m_cameraTargetPosition += side * deltax*WHEEL_MULTIPLIER;
} else
{
m_cameraTargetPosition -= m_cameraUp * deltay*WHEEL_MULTIPLIER;
}
}
}
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
{
const b3Vector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
{
const b3Vector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const b3Vector3& N3 = planeEquations[k];
b3Vector3 n2n3; n2n3 = N2.cross(N3);
b3Vector3 n3n1; n3n1 = N3.cross(N1);
b3Vector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > b3Scalar(0.0001) ) &&
( n3n1.length2() > b3Scalar(0.0001) ) &&
( n1n2.length2() > b3Scalar(0.0001) ) )
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
b3Scalar quotient = (N1.dot(n2n3));
if (b3Fabs(quotient) > b3Scalar(0.000001))
{
quotient = b3Scalar(-1.) / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
b3Vector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
}
}
}
}
}
}
b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
{
B3_PROFILE("solveGroupCacheFriendlyFinish");
// int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
// int i,j;
{
if (gpuBreakConstraints)
{
B3_PROFILE("breakViolatedConstraintsKernel");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_breakViolatedConstraintsKernel, "m_breakViolatedConstraintsKernel");
launcher.setBuffer(gpuConstraints->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
launcher.setConst(numConstraints);
launcher.launch1D(numConstraints);
}
else
{
gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints);
m_gpuData->m_gpuConstraintRows->copyToHost(m_gpuData->m_cpuConstraintRows);
gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1);
m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets);
for (int cid = 0; cid < numConstraints; cid++)
{
int originalConstraintIndex = batchConstraints[cid].m_originalConstraintIndex;
int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[originalConstraintIndex];
int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex];
if (numRows)
{
// printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold);
for (int i = 0; i < numRows; i++)
{
int rowIndex = constraintRowOffset + i;
int orgConstraintIndex = m_gpuData->m_cpuConstraintRows[rowIndex].m_originalConstraintIndex;
float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold;
// printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold)
{
m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags = 0; //&= ~B3_CONSTRAINT_FLAG_ENABLED;
}
}
}
}
gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints);
}
}
{
if (useGpuWriteBackVelocities)
{
B3_PROFILE("GPU write back velocities and transforms");
b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_writeBackVelocitiesKernel, "m_writeBackVelocitiesKernel");
launcher.setBuffer(gpuBodies->getBufferCL());
launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
launcher.setConst(numBodies);
launcher.launch1D(numBodies);
clFinish(m_gpuData->m_queue);
// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
// m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies);
//m_gpuData->m_gpuBodies->copyToHost(testBodies);
}
else
{
B3_PROFILE("CPU write back velocities and transforms");
m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
gpuBodies->copyToHost(m_gpuData->m_cpuBodies);
for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
{
int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
//printf("bodyIndex=%d\n",bodyIndex);
b3Assert(i == bodyIndex);
b3RigidBodyData* body = &m_gpuData->m_cpuBodies[bodyIndex];
if (body->m_invMass)
{
if (infoGlobal.m_splitImpulse)
m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
else
m_tmpSolverBodyPool[i].writebackVelocity();
if (m_usePgs)
{
body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
}
else
{
b3Assert(0);
}
/*
if (infoGlobal.m_splitImpulse)
{
body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin();
b3Quaternion orn;
orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation();
body->m_quat = orn;
}
*/
}
} //for
gpuBodies->copyFromHost(m_gpuData->m_cpuBodies);
}
}
clFinish(m_gpuData->m_queue);
m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
m_tmpSolverBodyPool.resizeNoInitialize(0);
return 0.f;
}
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;
}
