void postSolveContacts(WorldContext* w, Contact** contacts, int numContacts, ContactConstraint* constraints) { for (int i = 0; i < numContacts; ++i) { Contact* c = contacts[i]; Body* a = c->colliderA->getBody(); Body* b = c->colliderB->getBody(); int idxA = a->getIndex(); int idxB = b->getIndex(); c->colliderA->callbackPostSolve(c); c->colliderB->callbackPostSolve(c); for (int j = 0; j < contacts[i]->manifold.numPoints; ++j) { vec3 impulse = c->accImpulseN[j] * c->manifold.normal + c->accImpulseT[j] * c->tangent + c->accImpulseBT[j] * c->biTangent; w->p[idxA].l -= impulse; w->p[idxB].l += impulse; w->p[idxA].a -= cross(c->rA[j], impulse); w->p[idxB].a += cross(c->rB[j], impulse); } w->v[idxA].v = w->m[idxA].invM * w->p[idxA].l; w->v[idxB].v = w->m[idxB].invM * w->p[idxB].l; w->v[idxA].w = w->m[idxA].invI*w->p[idxA].a; w->v[idxB].w = w->m[idxB].invI*w->p[idxB].a; } }
void solveContacts(WorldContext* w, Contact** contacts, int numContacts, ContactConstraint* constraints) { for (int j = 0; j < numContacts; ++j) { Contact* c = contacts[j]; Body* a = c->colliderA->getBody(); Body* b = c->colliderB->getBody(); int idxA = a->getIndex(); int idxB = b->getIndex(); ContactManifold* man = &c->manifold; for (int i = 0; i < man->numPoints; ++i) { vec3 vA = w->v[idxA].v + cross(w->v[idxA].w, c->rA[i]); vec3 vB = w->v[idxB].v + cross(w->v[idxB].w, c->rB[i]); vec3 dv = vB - vA; float vn = dot(dv, man->normal); float dImpulseN = (-vn + constraints[j].veloctiyBias[i]) * c->massN[i]; // clamp accumulated impulse float impulseN0 = c->accImpulseN[i]; c->accImpulseN[i] = ong_MAX(impulseN0 + dImpulseN, 0.0f); dImpulseN = c->accImpulseN[i] - impulseN0; vec3 impulse = dImpulseN * man->normal; w->v[idxA].v -= w->m[idxA].invM * (impulse); w->v[idxB].v += w->m[idxB].invM * (impulse); w->v[idxA].w -= w->m[idxA].invI * (cross(c->rA[i], impulse)); w->v[idxB].w += w->m[idxB].invI * (cross(c->rB[i], impulse)); // friction if (c->friction <= 0.0f) continue; vA = w->v[idxA].v + cross(w->v[idxA].w, c->rA[i]); vB = w->v[idxB].v + cross(w->v[idxB].w, c->rB[i]); dv = vB - vA; float vt = dot(dv, c->tangent); float vbt = dot(dv, c->biTangent); float dImpulseT = -vt * c->massT[i]; float dImpulseBT = -vbt * c->massBT[i]; float maxImpulse = c->friction * c->accImpulseN[i]; float impulseT0 = c->accImpulseT[i]; c->accImpulseT[i] = ong_clamp(impulseT0 + dImpulseT, -maxImpulse, maxImpulse); dImpulseT = c->accImpulseT[i] - impulseT0; float impulseBT0 = c->accImpulseBT[i]; c->accImpulseBT[i] = ong_clamp(impulseBT0 + dImpulseBT, -maxImpulse, maxImpulse); dImpulseBT = c->accImpulseBT[i] - impulseBT0; impulse = dImpulseT * c->tangent + dImpulseBT * c->biTangent; w->v[idxA].v -= w->m[idxA].invM * (impulse); w->v[idxB].v += w->m[idxB].invM * (impulse); w->v[idxA].w -= w->m[idxA].invI * (cross(c->rA[i], impulse)); w->v[idxB].w += w->m[idxB].invI * (cross(c->rB[i], impulse)); } } }
void preSolveContacts(WorldContext* w, Contact** contacts, int numContacts, float invDt, ContactConstraint* contactConstraints) { for (int i = 0; i < numContacts; ++i) { Contact* c = contacts[i]; Body* a = c->colliderA->getBody(); Body* b = c->colliderB->getBody(); int idxA = a->getIndex(); int idxB = b->getIndex(); c->friction = sqrt(c->colliderA->getMaterial()->friction * c->colliderB->getMaterial()->friction); c->e = ong_MAX(c->colliderA->getMaterial()->restitution, c->colliderB->getMaterial()->restitution); ContactManifold* man = &c->manifold; // calculate tangent space { vec3 v; if (abs(man->normal.x) <= abs(man->normal.y) && abs(man->normal.x) <= abs(man->normal.z)) v = vec3(man->normal.x > 0.0f ? 1.0f : -1.0f, 0.0f, 0.0f); else if (abs(man->normal.y) <= abs(man->normal.z)) v = vec3(0, man->normal.y > 0.0f ? 1.0f : -1.0f, 0.0f); else v = vec3(0, 0, man->normal.z > 0 ? 1.0f : -1.0f); c->biTangent = normalize(cross(v, man->normal)); c->tangent = normalize(cross(man->normal, c->biTangent)); } for (int j = 0; j < c->manifold.numPoints; ++j) { c->rA[j] = man->points[j].position - w->r[idxA].p; c->rB[j] = man->points[j].position - w->r[idxB].p; c->massN[j] = 1.0f / (w->m[idxA].invM + w->m[idxB].invM + dot(man->normal, cross(w->m[idxA].invI* cross(c->rA[j], man->normal), c->rA[j]) + cross(w->m[idxB].invI*cross(c->rB[j], man->normal), c->rB[j]))); c->massT[j] = 1.0f / (w->m[idxA].invM + w->m[idxB].invM + dot(c->tangent, cross(w->m[idxA].invI*cross(c->rA[j], c->tangent), c->rA[j]) + cross(w->m[idxB].invI*cross(c->rB[j], c->tangent), c->rB[j]))); c->massBT[j] = 1.0f / (w->m[idxA].invM + w->m[idxB].invM + dot(c->biTangent, cross(w->m[idxA].invI*cross(c->rA[j], c->biTangent), c->rA[j]) + cross(w->m[idxB].invI*cross(c->rB[j], c->biTangent), c->rB[j]))); vec3 vA = w->v[idxA].v + cross(w->v[idxA].w, c->rA[j]); vec3 vB = w->v[idxB].v + cross(w->v[idxB].w, c->rB[j]); vec3 dv = vB - vA; float vn = dot(dv, man->normal); float penetrationBias = -0.02f * invDt * ong_MIN(0.0f, (man->points[j].penetration + 0.02f)); penetrationBias = ong_MIN(0.3f, penetrationBias); float restititutionBias = 0.0f; // only apply restitution to velocities beyond a threshold if (abs(vn) > 1.0f) restititutionBias = -c->e * vn; contactConstraints[i].veloctiyBias[j] = penetrationBias + restititutionBias; //warmstarting if (c->accImpulseN[j] != 0.0f || c->accImpulseT[j] != 0.0f || c->accImpulseBT[j] != 0.0f) { vec3 impulse = c->accImpulseN[j] * c->manifold.normal + c->accImpulseT[j] * c->tangent + c->accImpulseBT[j] * c->biTangent; w->v[idxA].v -= w->m[idxA].invM * impulse; w->v[idxB].v += w->m[idxB].invM * impulse; w->v[idxA].w -= w->m[idxA].invI * cross(c->rA[j], impulse); w->v[idxB].w += w->m[idxB].invI * cross(c->rA[j], impulse); } } } }