void PhysicsSimulation::resolveCollisions() {
PerformanceTimer perfTimer("resolve");
// walk all collisions, accumulate movement on shapes, and build a list of affected shapes
QSet<Shape*> shapes;
int numCollisions = _collisions.size();
for (int i = 0; i < numCollisions; ++i) {
CollisionInfo* collision = _collisions.getCollision(i);
collision->apply();
// there is always a shapeA
shapes.insert(collision->getShapeA());
// but need to check for valid shapeB
if (collision->_shapeB) {
shapes.insert(collision->getShapeB());
}
}
// walk all affected shapes and apply accumulated movement
QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
while (shapeItr != shapes.constEnd()) {
(*shapeItr)->applyAccumulatedDelta();
++shapeItr;
}
}
ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) :
_lastFrame(frame), _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()),
_offsetA(0.0f), _offsetB(0.0f),
_relativeMassA(0.5f), _relativeMassB(0.5f),
_numPointsA(0), _numPoints(0), _normal(0.0f) {
glm::vec3 pointA = collision._contactPoint;
glm::vec3 pointB = collision._contactPoint - collision._penetration;
float pLength = glm::length(collision._penetration);
if (pLength > EPSILON) {
_normal = collision._penetration / pLength;
}
if (_shapeA->getID() > _shapeB->getID()) {
// swap so that _shapeA always has lower ID
_shapeA = collision.getShapeB();
_shapeB = collision.getShapeA();
_normal = - _normal;
pointA = pointB;
pointB = collision._contactPoint;
}
// bring the contact points inside the shapes to help maintain collision updates
pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
_offsetA = pointA - _shapeA->getTranslation();
_offsetB = pointB - _shapeB->getTranslation();
_shapeA->getVerletPoints(_points);
_numPointsA = _points.size();
_shapeB->getVerletPoints(_points);
_numPoints = _points.size();
// compute and cache relative masses
float massA = EPSILON;
for (int i = 0; i < _numPointsA; ++i) {
massA += _points[i]->getMass();
}
float massB = EPSILON;
for (int i = _numPointsA; i < _numPoints; ++i) {
massB += _points[i]->getMass();
}
float invTotalMass = 1.0f / (massA + massB);
_relativeMassA = massA * invTotalMass;
_relativeMassB = massB * invTotalMass;
// _contactPoint will be the weighted average of the two
_contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
// compute offsets for shapeA
for (int i = 0; i < _numPointsA; ++i) {
glm::vec3 offset = _points[i]->_position - pointA;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
// compute offsets for shapeB
for (int i = _numPointsA; i < _numPoints; ++i) {
glm::vec3 offset = _points[i]->_position - pointB;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
}