i am having trouble implementing 2d rigid body physics for box-vs-box or box vs plane using speculative contacts -> http://www.wildbunny.co.uk/blog/2011/03/25/speculative-contacts-an-continuous-collision-engine-approach-part-1/
There is always an impulse going on which results in my box falling on the plane instantly starting spinning and flying around (Will post video showing the behavior when i get home)
I assume that the contact points are not correct, because paul stated we should just use the closest points - but unfortunatly he never explained how to get more than one contact point :-(
So i implemented a hybrid approach, use a single closest point in case of a separation and use clipping to get the contact points for penetration.
What i dont know if this is the correct way to this. Do i need two contact points in a separation case as well???
If someone want to have a look how i implemented it, here it is:
Plane vs edge/box:
internal Face GetFace(U32 vertexCount, Vec2f *verts, Vec2f normal) {
S32 firstIndex = 0;
F32 firstDistance = Dot(verts[0], normal);
for (U32 vertexIndex = 1; vertexIndex < vertexCount; vertexIndex++) {
Vec2f v = verts[vertexIndex];
F32 p = Dot(v, normal);
if (p > firstDistance) {
firstDistance = p;
firstIndex = vertexIndex;
}
}
S32 secondIndex = -1;
F32 secondDistance = 0;
for (U32 vertexIndex = 0; vertexIndex < vertexCount; vertexIndex++) {
if (vertexIndex != firstIndex) {
Vec2f v = verts[vertexIndex];
F32 p = Dot(v, normal);
if (secondIndex == -1 || p > secondDistance) {
secondDistance = p;
secondIndex = vertexIndex;
}
}
}
Assert(secondIndex > -1);
Face result = {};
result.index = firstIndex;
result.points[0] = verts[firstIndex];
result.points[1] = verts[secondIndex];
return (result);
}
internal U32 PlaneEdgeContactGenerator(Physics *world, Transform transformA, Transform transformB, Shape *shapeA, Shape *shapeB, U32 offset, Contact *contacts) {
U32 result = 0;
Vec2f normal = transformA.rot.col1;
Vec2f posA = transformA.pos;
EdgeShape *edgeB = GetEdgeShape(shapeB);
Vec2f* localVertsB = edgeB->localVerts;
U32 numVerticesB = edgeB->numVertices;
Mat2f transposeB = Transpose2(transformB.rot);
Vec2f normalB = -(normal * transposeB);
Vec2f pointsOnA[2];
F32 distanceToA[2];
U32 clipTypeOnB[2];
U32 numPoints = 0;
Face faceB = GetFace(numVerticesB, localVertsB, normalB);
Vec2f supportPointB = faceB.points[0] * transformB;
Vec2f planePoint = posA;
Vec2f distanceToPlane = supportPointB - planePoint;
F32 d = Dot(distanceToPlane, normal);
if (d > 0) {
pointsOnA[0] = supportPointB + normal * -d;
distanceToA[0] = d;
numPoints = 1;
clipTypeOnB[0] = 0;
} else {
Vec2f p0 = faceB.points[0] * transformB;
Vec2f p1 = faceB.points[1] * transformB;
Vec2f distance0 = p0 - planePoint;
F32 d0 = Dot(distance0, normal);
if (d0 <= 0) {
U32 idx = numPoints++;
pointsOnA[idx] = p0 + normal * -d0;
distanceToA[idx] = d0;
clipTypeOnB[idx] = 1;
}
Vec2f distance1 = p1 - planePoint;
F32 d1 = Dot(distance1, normal);
if (d0 <= 0) {
U32 idx = numPoints++;
pointsOnA[idx] = p1 + normal * -d1;
distanceToA[idx] = d1;
clipTypeOnB[idx] = 2;
}
}
for (U32 pointIndex = 0; pointIndex < numPoints; pointIndex++) {
// NOTE(final): Plane has just one face - therefore only one feature
U32 feature = 0 * CONTACT_FEATURE_FACEA_PRIME + (faceB.index + 1) * CONTACT_FEATURE_FACEB_PRIME + clipTypeOnB[pointIndex] * CONTACT_FEATURE_CLIP_PRIME;
Contact *contact = contacts + (offset + result++);
SetContact(contact, distanceToA[pointIndex], normal, pointsOnA[pointIndex] - posA, feature);
}
return(result);
}
Initializing the contact with mass ratio and build contact arms:// Get center of masses
Vec2f comA = bodyA->position + bodyA->centroid;
Vec2f comB = bodyB->position + bodyB->centroid;
// Get contact points in world space (Point is already rotated, B is calculated on the fly)
Vec2f contactWorldA = bodyA->position + shapeA->localTransform.pos + contact->point;
Vec2f contactWorldB = contactWorldA + contact->normal * contact->distance;
// Get contact arms
contact->rA = contactWorldA - comA;
contact->rB = contactWorldB - comB;
// Get normal rotation on the z-plane
F32 rnA = Cross(contact->rA, contact->normal);
F32 rnB = Cross(contact->rB, contact->normal);
// Build normal matrix
F32 normalTensor = bodyA->invMass + bodyB->invMass + bodyA->invInertia * rnA * rnA + bodyB->invInertia * rnB * rnB;
contact->normalMass = normalTensor > 0 ? 1.0f / normalTensor : 0;
Solving the Impulse for each contact:// Relative velocity on contact = vB + cross(wB, rB) - vA - cross(wA, rA);
Vec2f vAB = *vB + Cross(bodyB->angularVelocity, contact->rB) - *vA - Cross(bodyA->angularVelocity, contact->rA);
// Project relative velocity on normal
F32 projNormalVel = Dot(vAB, normal);
// Normal impulse
F32 remove = projNormalVel + contact->distance / dt;
F32 normalImpulse = remove * contact->normalMass;
F32 newImpulse = Min(contact->normalImpulse + normalImpulse, 0.0f);
F32 impulseChange = newImpulse - contact->normalImpulse;
contact->normalImpulse = newImpulse;
// Apply Impulses
Vec2f impulseA = normal * impulseChange;
bodyA->velocity += impulseA * bodyA->invMass;
bodyA->angularVelocity += Cross(contact->rA, impulseA) * bodyA->invInertia;
Vec2f impulseB = -normal * impulseChange;
bodyB->velocity += impulseB * bodyB->invMass;
bodyB->angularVelocity += Cross(contact->rB, impulseB) * bodyB->invInertia;