void PhysicsWorld::solvePositions(Contact & contact)
{
if (!contact.intersect())
{
return;
}
contact.update();
auto & bodyA = contact.bodyA();
auto & bodyB = contact.bodyB();
auto & transformA = bodyA.transform();
auto & transformB = bodyB.transform();
auto mA = bodyA.inverseMass();
auto mB = bodyB.inverseMass();
auto & iA = bodyA.worldInverseInertia();
auto & iB = bodyB.worldInverseInertia();
auto & cA = transformA.position();
auto & cB = transformB.position();
for (uint p = 0; p < contact.numPoints(); p++)
{
auto & point = contact.point(p);
auto & n = point.normal;
auto position = point.position;
auto separation = -point.depth;
auto rA = position - cA;
auto rB = position - cB;
// Track max constraint error.
// auto minSeparation = std::min(minSeparation, separation);
// Prevent large corrections and allow slop.
auto C = std::min(std::max(0.5f * (separation + LINEAR_SLOP), -MAX_LINEAR_CORRECTION), 0.0f);
// Compute the effective mass.
auto normalMass = point.normalMass;
// Compute normal impulse
auto impulse = normalMass > 0.0f ? -C / normalMass : 0.0f;
auto P = impulse * n;
std::cout << P << " " << separation << " " << (-mA * P) << " " << (mB * P) << std::endl;
transformA.setPosition(transformA.position() - mA * P);
transformA.setOrientation(QuaternionAxisRotation(transformA.orientation(), iA * glm::cross(rA, -P)));
transformB.setPosition(transformB.position() + mB * P);
transformB.setOrientation(QuaternionAxisRotation(transformB.orientation(), iB * glm::cross(rB, P)));
}
}
void PhysicsWorld::warmStart(Contact & contact) const
{
if (!contact.intersect())
{
return;
}
auto & bodyA = contact.bodyA();
auto & bodyB = contact.bodyB();
auto mA = bodyA.inverseMass();
auto & iA = bodyA.worldInverseInertia();
auto & vA = bodyA.linearVelocity();
auto & wA = bodyA.angularVelocity();
auto & cA = bodyA.transform().position();
auto mB = bodyB.inverseMass();
auto & iB = bodyB.worldInverseInertia();
auto & vB = bodyB.linearVelocity();
auto & wB = bodyB.angularVelocity();
auto & cB = bodyB.transform().position();
for (uint p = 0; p < contact.numPoints(); p++)
{
auto & point = contact.point(p);
auto & n = point.normal;
auto rA = point.position - cA;
auto rB = point.position - cB;
auto lambda = point.normalImpulseAccumulator;
auto J = lambda * n;
bodyA.setLinearVelocity(vA - mA * J);
bodyA.setAngularVelocity(wA - iA * glm::cross(rA, J));
bodyB.setLinearVelocity(vB + mB * J);
bodyB.setAngularVelocity(wB + iB * glm::cross(rB, J));
}
}
void PhysicsWorld::solveContactVelocities(Contact & contact)
{
if (!contact.intersect())
{
return;
}
auto & bodyA = contact.bodyA();
auto & bodyB = contact.bodyB();
auto mA = bodyA.inverseMass();
auto & iA = bodyA.worldInverseInertia();
auto & vA = bodyA.linearVelocity();
auto & wA = bodyA.angularVelocity();
auto & cA = bodyA.transform().position();
auto mB = bodyB.inverseMass();
auto & iB = bodyB.worldInverseInertia();
auto & vB = bodyB.linearVelocity();
auto & wB = bodyB.angularVelocity();
auto & cB = bodyB.transform().position();
// Coefficient of restitution
auto e = contact.restitution();
/**
* Solve tangent velocities
*/
for (uint p = 0; p < contact.numPoints(); p++)
{
auto & point = contact.point(p);
auto rA = point.position - cA;
auto rB = point.position - cB;
// Relative velocity along normal
auto vra = bodyA.localVelocity(rA);
auto vrb = bodyB.localVelocity(rB);
auto vRel = vrb - vra;
auto vRelNormal = glm::dot(point.normal, vRel);
// Relative velocity along tangent
auto vRelTangent = vRel - (vRelNormal * point.normal);
if (vRelTangent == glm::vec3(0.0f)) {
continue;
}
auto tangent = glm::normalize(vRelTangent);
auto tangentMass = glm::dot(glm::cross(iA * glm::cross(rA, tangent), rA) +
glm::cross(iB * glm::cross(rB, tangent), rB), tangent) + mA + mB;
if (tangentMass == 0.0f) {
continue;
}
auto lambda = -glm::length(vRelTangent) / tangentMass;
auto maxFriction = contact.friction() * point.normalImpulseAccumulator;
auto newImpulse = std::max(-maxFriction, std::min<float>(maxFriction, point.tangentImpulseAccumulator + lambda));
lambda = newImpulse - point.tangentImpulseAccumulator;
auto P = lambda * tangent;
bodyA.setLinearVelocity(vA - mA * P);
bodyA.setAngularVelocity(wA - iA * glm::cross(rA, P));
bodyB.setLinearVelocity(vB + mB * P);
bodyB.setAngularVelocity(wB + iB * glm::cross(rB, P));
}
/**
* Solve normal velocities
*/
for (uint p = 0; p < contact.numPoints(); p++)
{
auto & point = contact.point(p);
auto rA = point.position - cA;
auto rB = point.position - cB;
auto & n = point.normal;
auto normalMass = point.normalMass;
auto velocityBias = point.velocityBias;
// Relative velocity along normal
auto vra = bodyA.localVelocity(rA);
auto vrb = bodyB.localVelocity(rB);
auto vRel = glm::dot(n, vrb - vra);
//
auto vDelta = velocityBias - vRel;
auto lambda = vDelta / normalMass;
auto newNormalImpulseAccumulator = std::max(lambda + point.normalImpulseAccumulator, 0.0f);
lambda = newNormalImpulseAccumulator - point.normalImpulseAccumulator;
point.normalImpulseAccumulator += lambda;
// Bias lambda
auto bias = (BAUMGARTE * std::max(point.depth - LINEAR_SLOP, 0.0f)) / m_timestep;
// J - impulse magnitude
auto J = (lambda + bias) * n;
bodyA.setLinearVelocity(vA - mA * J);
bodyA.setAngularVelocity(wA - iA * glm::cross(rA, J));
bodyB.setLinearVelocity(vB + mB * J);
bodyB.setAngularVelocity(wB + iB * glm::cross(rB, J));
}
}
