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.)); } } } }