unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
h = a.inverse();
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = btScalar(1.0) / gamma;
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
// Update the matrices
u += delta;
h = u.inverse();
// Check for convergence
if (p1_norm(delta) <= m_tolerance * u_1)
{
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return i;
}
}
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return m_maxIterations;
}
void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
{
const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
if (multiBodyNumConstraints == 0)
return;
// 1. Compute b
{
BT_PROFILE("init b (rhs)");
m_multiBodyB.resize(multiBodyNumConstraints);
m_multiBodyB.setZero();
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
const btScalar jacDiag = constraint.m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
const btScalar rhs = constraint.m_rhs;
m_multiBodyB[i] = rhs / jacDiag;
}
}
}
// 2. Compute lo and hi
{
BT_PROFILE("init lo/ho");
m_multiBodyLo.resize(multiBodyNumConstraints);
m_multiBodyHi.resize(multiBodyNumConstraints);
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
m_multiBodyLo[i] = constraint.m_lowerLimit;
m_multiBodyHi[i] = constraint.m_upperLimit;
}
}
// 3. Construct A matrix by using the impulse testing
{
BT_PROFILE("Compute A");
{
BT_PROFILE("m_A.resize");
m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
}
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
// Compute the diagonal of A, which is A(i, i)
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
m_multiBodyA.setElem(i, i, diagA);
// Computes the off-diagonals of A:
// a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
// b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
for (int j = i + 1; j < multiBodyNumConstraints; ++j)
{
const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
// Set the off-diagonal values of A. Note that A is symmetric.
m_multiBodyA.setElem(i, j, offDiagA);
m_multiBodyA.setElem(j, i, offDiagA);
}
}
}
// Add CFM to the diagonal of m_A
for (int i = 0; i < m_multiBodyA.rows(); ++i)
{
m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
// 4. Initialize x
{
BT_PROFILE("resize/init x");
m_multiBodyX.resize(multiBodyNumConstraints);
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i = 0; i < multiBodyNumConstraints; ++i)
{
const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
m_multiBodyX[i] = constraint.m_appliedImpulse;
}
}
else
{
m_multiBodyX.setZero();
}
}
}
void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintPtrArray.size();
if (numConstraintRows == 0)
return;
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i = 0; i < numConstraintRows; i++)
{
btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
m_b[i] = rhs / jacDiag;
m_bSplit[i] = rhsPenetration / jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i = 0; i < numConstraintRows; i++)
{
if (0) //m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
}
else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
}
}
}
//
int m = m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies, -1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2 * m, 8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2 * m, 8);
JinvM3.setZero();
J3.setZero();
}
int cur = 0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c = 0;
int numRows = 0;
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
if (orgBodyA)
{
{
int slotA = -1;
//find free jointNode slot for sbA
slotA = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMass[r]);
JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB = -1;
//find free jointNode slot for sbA
slotB = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMassB[r]);
JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
rowOffset += numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n, n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c = 0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
// btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA >= 0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0 < c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r(JinvMrow,
Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB >= 0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1 < c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj = 0;
for (; row__ < numJointRows;)
{
//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
}
row__ += infom;
jj++;
}
}
}
if (1)
{
// add cfm to the diagonal of m_A
for (int i = 0; i < m_A.rows(); ++i)
{
m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i] = c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trA = transA * m_rbAFrame;
btTransform trB = transB * m_rbBFrame;
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
float swingAxisLen2 = swingAxis.length2();
if(btFuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if(!btFuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1.f;
if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
(swingLimit - swingLimit * m_limitSoftness);
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal(swingAxis);
// Calculate necessary axis & factors
m_swingAxis = quatRotate(qB, -swingAxis);
m_twistAxisA.setValue(0,0,0);
m_kSwing = btScalar(1.) /
(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross(ivA);
}
}
else
{
if(m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if(!(btFuzzyZero(y)))
{
m_solveSwingLimit = true;
if(m_swingSpan2 >= m_fixThresh)
{
y = btScalar(0.f);
btScalar span2 = btAtan2(z, x);
if(span2 > m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = btSin(m_swingSpan2);
}
else if(span2 < -m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = -btSin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if(!btFuzzyZero(z))
{
m_solveSwingLimit = true;
if(m_swingSpan1 >= m_fixThresh)
{
z = btScalar(0.f);
btScalar span1 = btAtan2(y, x);
if(span1 > m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = btSin(m_swingSpan1);
}
else if(span1 < -m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = -btSin(m_swingSpan1);
}
}
}
}
target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize();
m_swingAxis = -ivB.cross(target);
m_swingCorrection = m_swingAxis.length();
m_swingAxis.normalize();
}
}
if (m_twistSpan >= btScalar(0.f))
{
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
if (m_twistAngle > m_twistSpan*m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1.f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = quatRotate(qB, -twistAxis);
m_kTwist = btScalar(1.) /
(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
}
if (m_solveSwingLimit)
m_twistAxisA = quatRotate(qA, -twistAxis);
}
else
{
m_twistAngle = btScalar(0.f);
}
}
}
int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
{
startProfiling(timeStep);
BT_PROFILE("stepSimulation");
int numSimulationSubSteps = 0;
if (maxSubSteps)
{
//fixed timestep with interpolation
m_localTime += timeStep;
if (m_localTime >= fixedTimeStep)
{
numSimulationSubSteps = int( m_localTime / fixedTimeStep);
m_localTime -= numSimulationSubSteps * fixedTimeStep;
}
} else
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = timeStep;
if (btFuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
} else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if (getDebugDrawer())
{
btIDebugDraw* debugDrawer = getDebugDrawer ();
gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
}
if (numSimulationSubSteps)
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
saveKinematicState(fixedTimeStep*clampedSimulationSteps);
applyGravity();
for (int i=0;i<clampedSimulationSteps;i++)
{
internalSingleStepSimulation(fixedTimeStep);
synchronizeMotionStates();
}
} else
{
synchronizeMotionStates();
}
clearForces();
#ifndef BT_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
#endif //BT_NO_PROFILE
return numSimulationSubSteps;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
{
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
btTransform trA = transA*m_frameInA;
btVector3 twistAxis = trA.getBasis().getColumn(0);
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
int srow1 = srow + info->rowskip;
J1[srow+0] = p[0];
J1[srow+1] = p[1];
J1[srow+2] = p[2];
J1[srow1+0] = q[0];
J1[srow1+1] = q[1];
J1[srow1+2] = q[2];
J2[srow+0] = -p[0];
J2[srow+1] = -p[1];
J2[srow+2] = -p[2];
J2[srow1+0] = -q[0];
J2[srow1+1] = -q[1];
J2[srow1+2] = -q[2];
btScalar fact = info->fps;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
info->m_upperLimit[srow1] = SIMD_INFINITY;
srow = srow1 + info->rowskip;
{
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
}
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
void btRigidBody::applyDamping(btScalar timeStep)
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway)
// DrChat: bullet's old damping
/*
if (!btFuzzyZero(m_linearDamping) && !m_linearVelocity.fuzzyZero())
m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
if (!btFuzzyZero(m_angularDamping) && !m_angularVelocity.fuzzyZero())
m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
*/
if (!btFuzzyZero(m_linearDamping) && !m_linearVelocity.fuzzyZero()) {
m_linearVelocity *= btExp(-m_linearDamping * timeStep);
}
if (!btFuzzyZero(m_angularDamping) && !m_angularVelocity.fuzzyZero()) {
m_angularVelocity *= btExp(-m_angularDamping * timeStep);
}
if (m_additionalDamping)
{
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
{
m_angularVelocity *= m_additionalDampingFactor;
m_linearVelocity *= m_additionalDampingFactor;
}
btScalar speed = m_linearVelocity.length();
if (speed < m_linearDamping)
{
btScalar dampVel = btScalar(0.005);
if (speed > dampVel)
{
btVector3 dir = m_linearVelocity.normalized();
m_linearVelocity -= dir * dampVel;
} else
{
m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
}
}
btScalar angSpeed = m_angularVelocity.length();
if (angSpeed < m_angularDamping)
{
btScalar angDampVel = btScalar(0.005);
if (angSpeed > angDampVel)
{
btVector3 dir = m_angularVelocity.normalized();
m_angularVelocity -= dir * angDampVel;
} else
{
m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
}
}
}
}
