// Sequential position solver for position constraints. bool ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) { float32 minSeparation = 0.0f; for (int32 i = 0; i < m_count; ++i) { ContactPositionConstraint* pc = m_positionConstraints + i; int32 indexA = pc->indexA; int32 indexB = pc->indexB; Vec2 localCenterA = pc->localCenterA; Vec2 localCenterB = pc->localCenterB; int32 pointCount = pc->pointCount; float32 mA = 0.0f; float32 iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc->invMassA; iA = pc->invIA; } float32 mB = 0.0f; float32 iB = 0.; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc->invMassB; iB = pc->invIB; } Vec2 cA = m_positions[indexA].c; float32 aA = m_positions[indexA].a; Vec2 cB = m_positions[indexB].c; float32 aB = m_positions[indexB].a; // Solve normal constraints for (int32 j = 0; j < pointCount; ++j) { Transform xfA, xfB; xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - Mul(xfA.q, localCenterA); xfB.p = cB - Mul(xfB.q, localCenterB); PositionSolverManifold psm; psm.Initialize(pc, xfA, xfB, j); Vec2 normal = psm.normal; Vec2 point = psm.point; float32 separation = psm.separation; Vec2 rA = point - cA; Vec2 rB = point - cB; // Track max constraint error. minSeparation = Min(minSeparation, separation); // Prevent large corrections and allow slop. float32 C = Clamp(toiBaugarte * (separation + linearSlop), -maxLinearCorrection, 0.0f); // Compute the effective mass. float32 rnA = Cross(rA, normal); float32 rnB = Cross(rB, normal); float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float32 impulse = K > 0.0f ? - C / K : 0.0f; Vec2 P = impulse * normal; cA -= mA * P; aA -= iA * Cross(rA, P); cB += mB * P; aB += iB * Cross(rB, P); } m_positions[indexA].c = cA; m_positions[indexA].a = aA; m_positions[indexB].c = cB; m_positions[indexB].a = aB; } // We can't expect minSpeparation >= -linearSlop because we don't // push the separation above -linearSlop. return minSeparation >= -1.5f * linearSlop; }
// Sequential solver. bool ContactSolver::SolvePositionConstraints() { real32 minSeparation = 0.0f; for (s32 i = 0; i < m_count; ++i) { ContactPositionConstraint* pc = m_positionConstraints + i; s32 indexA = pc->indexA; s32 indexB = pc->indexB; glm::vec2 localCenterA = pc->localCenterA; real32 mA = pc->invMassA; real32 iA = pc->invIA; glm::vec2 localCenterB = pc->localCenterB; real32 mB = pc->invMassB; real32 iB = pc->invIB; s32 pointCount = pc->pointCount; glm::vec2 cA = m_positions[indexA].c; real32 aA = m_positions[indexA].a; glm::vec2 cB = m_positions[indexB].c; real32 aB = m_positions[indexB].a; // Solve normal constraints for (s32 j = 0; j < pointCount; ++j) { Transform2D xfA, xfB; xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - Rotation2D::Mul(xfA.q, localCenterA); xfB.p = cB - Rotation2D::Mul(xfB.q, localCenterB); PositionSolverManifold psm; psm.Initialize(pc, xfA, xfB, j); glm::vec2 normal = psm.normal; glm::vec2 point = psm.point; real32 separation = psm.separation; glm::vec2 rA = point - cA; glm::vec2 rB = point - cB; // Track max constraint error. minSeparation = glm::min(minSeparation, separation); // Prevent large corrections and allow slop. real32 C = glm::clamp(baumgarte * (separation + linearSlop), - maxLinearCorrection, 0.0f); // Compute the effective mass. real32 rnA = MathUtils::Cross2(rA, normal); real32 rnB = MathUtils::Cross2(rB, normal); real32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse real32 impulse = K > 0.0f ? - C / K : 0.0f; glm::vec2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils::Cross2(rA, P); cB += mB * P; aB += iB * MathUtils::Cross2(rB, P); } m_positions[indexA].c = cA; m_positions[indexA].a = aA; m_positions[indexB].c = cB; m_positions[indexB].a = aB; } // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above -_linearSlop. return minSeparation >= -3.0f * linearSlop; }