base
This commit is contained in:
+316
-101
@@ -64,8 +64,10 @@ void Physics::Step(float dt)
|
||||
|
||||
// Integrate forces ? velocities
|
||||
for (p* body : objects) {
|
||||
// Nothing rotates until the end of the step, so everything until then can share this
|
||||
body->invInertia = ComputeInvInertiaTensor(body);
|
||||
// Nothing rotates until the end of the step, so everything until then can share this.
|
||||
// A sleeping body hasn't turned since it was last worked out.
|
||||
if (!body->isSleeping)
|
||||
body->invInertia = ComputeInvInertiaTensor(body);
|
||||
|
||||
if (!body->velocity || body->isSleeping || body->frozen) continue;
|
||||
|
||||
@@ -104,29 +106,80 @@ void Physics::Step(float dt)
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Tick and interpolation
|
||||
// ============================================================================
|
||||
void Physics::Tick(float dt, int substeps)
|
||||
{
|
||||
for (p* body : objects) {
|
||||
if (!body->velocity) continue;
|
||||
body->previousPosition = body->pointer->t.GetTranslation();
|
||||
body->previousOrientation = body->pointer->t.GetRotationQuaternion();
|
||||
body->hasPrevious = true;
|
||||
}
|
||||
|
||||
substeps = std::max(substeps, 1);
|
||||
for (int i = 0; i < substeps; i++)
|
||||
Step(dt / float(substeps));
|
||||
}
|
||||
|
||||
void Physics::ApplyInterpolation(float alpha)
|
||||
{
|
||||
alpha = glm::clamp(alpha, 0.0f, 1.0f);
|
||||
for (p* body : objects) {
|
||||
// Asleep it hasn't moved, so there's nothing to blend
|
||||
if (!body->velocity || !body->hasPrevious || body->interpolated || body->isSleeping) continue;
|
||||
|
||||
t& transform = body->pointer->t;
|
||||
body->simulatedPosition = transform.GetTranslation();
|
||||
body->simulatedOrientation = transform.GetRotationQuaternion();
|
||||
transform.TranslateTo(glm::mix(body->previousPosition, body->simulatedPosition, alpha));
|
||||
if (!body->lockRotation)
|
||||
transform.RotateToQuaternion(glm::slerp(body->previousOrientation, body->simulatedOrientation, alpha));
|
||||
body->interpolated = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Physics::RemoveInterpolation()
|
||||
{
|
||||
for (p* body : objects) {
|
||||
if (!body->interpolated) continue;
|
||||
body->pointer->t.TranslateTo(body->simulatedPosition);
|
||||
body->pointer->t.RotateToQuaternion(body->simulatedOrientation);
|
||||
body->interpolated = false;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ResolveCollisions
|
||||
// ============================================================================
|
||||
void Physics::ResolveCollisions(float dt)
|
||||
{
|
||||
FindCollisions();
|
||||
|
||||
for (Collision& col : collisions)
|
||||
Resolve(&col);
|
||||
|
||||
if (joints.empty()) return;
|
||||
|
||||
// The first contact pass nudges positions, so these have to be worked out after it
|
||||
PrepareContact(col);
|
||||
for (BallJoint* joint : joints) {
|
||||
DampJoint(*joint, dt);
|
||||
PrepareJoint(*joint, dt);
|
||||
}
|
||||
|
||||
// Joints and contacts fight each other, so iterate them together or limbs get pulled through the floor
|
||||
for (int i = 0; i < PHYSICS_JOINT_ITERATIONS; i++) {
|
||||
// One pass over the contacts leaves things resting on several points spinning a little, since
|
||||
// each push partly undoes the last. Going round several times settles them. Joints and contacts
|
||||
// fight each other too, so they get iterated together.
|
||||
const int iterations = joints.empty() ? PHYSICS_CONTACT_ITERATIONS : PHYSICS_JOINT_ITERATIONS;
|
||||
for (int i = 0; i < iterations; i++) {
|
||||
for (BallJoint* joint : joints)
|
||||
SolveJoint(*joint);
|
||||
for (Collision& col : collisions)
|
||||
Resolve(&col, false);
|
||||
SolveContact(col);
|
||||
}
|
||||
|
||||
// Positions last, so nothing above saw them half moved
|
||||
for (Collision& col : collisions) {
|
||||
CorrectContactPosition(col);
|
||||
if (col.normalImpulse > PHYSICS_MIN_IMPACT_IMPULSE)
|
||||
impacts.push_back({ col.ObjA, col.ObjB, col.POI, col.normalImpulse });
|
||||
}
|
||||
}
|
||||
|
||||
@@ -135,36 +188,70 @@ void Physics::ResolveCollisions(float dt)
|
||||
// Sort and sweep: once sorted along x, a body can only touch the ones after it
|
||||
// that start before it ends.
|
||||
// ============================================================================
|
||||
// A body's world bounds, only worked out again once its transform has changed
|
||||
static const BoundingBox& GetBounds(p* body)
|
||||
{
|
||||
const t& transform = body->pointer->t;
|
||||
if (body->boundsVersion != transform.GetVersion()) {
|
||||
body->bounds = transform.GetAABB();
|
||||
body->boundsVersion = transform.GetVersion();
|
||||
}
|
||||
return body->bounds;
|
||||
}
|
||||
|
||||
void Physics::FindCollisions()
|
||||
{
|
||||
collisions.clear();
|
||||
broadPhase.clear();
|
||||
|
||||
for (p* body : objects) {
|
||||
if (!body->collision) continue;
|
||||
BroadPhaseEntry entry;
|
||||
entry.body = body;
|
||||
entry.bounds = body->pointer->t.GetAABB();
|
||||
broadPhase.push_back(entry);
|
||||
// Everything goes in, collision or not, so the list only changes when bodies come or go
|
||||
const bool rebuilt = broadPhaseVersion != objectsVersion;
|
||||
if (rebuilt) {
|
||||
broadPhase.clear();
|
||||
for (p* body : objects) {
|
||||
BroadPhaseEntry entry;
|
||||
entry.body = body;
|
||||
broadPhase.push_back(entry);
|
||||
}
|
||||
broadPhaseVersion = objectsVersion;
|
||||
}
|
||||
for (BroadPhaseEntry& entry : broadPhase)
|
||||
entry.bounds = GetBounds(entry.body);
|
||||
|
||||
std::sort(broadPhase.begin(), broadPhase.end(), [](const BroadPhaseEntry& first, const BroadPhaseEntry& second) {
|
||||
return first.bounds.min.x < second.bounds.min.x;
|
||||
});
|
||||
// Things barely move between substeps, so last step's order is nearly sorted already and insertion
|
||||
// sort gets through it in about one pass. A fresh list gets a proper sort.
|
||||
if (rebuilt) {
|
||||
std::sort(broadPhase.begin(), broadPhase.end(), [](const BroadPhaseEntry& first, const BroadPhaseEntry& second) {
|
||||
return first.bounds.min.x < second.bounds.min.x;
|
||||
});
|
||||
}
|
||||
else {
|
||||
for (int i = 1; i < (int)broadPhase.size(); i++) {
|
||||
const BroadPhaseEntry entry = broadPhase[i];
|
||||
int j = i - 1;
|
||||
while (j >= 0 && broadPhase[j].bounds.min.x > entry.bounds.min.x) {
|
||||
broadPhase[j + 1] = broadPhase[j];
|
||||
j--;
|
||||
}
|
||||
broadPhase[j + 1] = entry;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < (int)broadPhase.size(); i++) {
|
||||
const BroadPhaseEntry& first = broadPhase[i];
|
||||
p* a = first.body;
|
||||
if (!a->collision) continue;
|
||||
|
||||
for (int j = i + 1; j < (int)broadPhase.size(); j++) {
|
||||
const BroadPhaseEntry& second = broadPhase[j];
|
||||
if (second.bounds.min.x >= first.bounds.max.x) break;
|
||||
|
||||
p* b = second.body;
|
||||
if (!b->collision) continue;
|
||||
if (!a->velocity && !b->velocity) continue;
|
||||
if (a->isSleeping && b->isSleeping) continue;
|
||||
// Two things that can't move into each other, like an item asleep on the ground
|
||||
if ((a->isSleeping || !a->velocity) && (b->isSleeping || !b->velocity) && !a->isHitbox && !b->isHitbox) continue;
|
||||
if ((a->frozen || !a->velocity) && (b->frozen || !b->velocity)) continue;
|
||||
if (a->collisionGroup != 0 && a->collisionGroup == b->collisionGroup) continue;
|
||||
if (a->collisionGroup != 0 && a->collisionGroup == b->collisionGroup) continue; if ((a->isHitbox && b->collisionGroup != 0) || (b->isHitbox && a->collisionGroup != 0)) continue;
|
||||
if (!BoundingBoxInBoundingBox(first.bounds, second.bounds)) continue;
|
||||
|
||||
if (a->collisionMesh || b->collisionMesh) {
|
||||
@@ -172,18 +259,29 @@ void Physics::FindCollisions()
|
||||
continue;
|
||||
}
|
||||
|
||||
std::optional<TInTInfo> e = TInT(a->pointer->t, b->pointer->t);
|
||||
if (e.has_value()) {
|
||||
Collision tp(a, b);
|
||||
tp.POI = e.value().POI;
|
||||
tp.CN = e.value().CN;
|
||||
tp.overlap = e.value().overlap;
|
||||
collisions.push_back(tp);
|
||||
}
|
||||
std::optional<TInTInfo> e = TInT(a->pointer->t, b->pointer->t, false);
|
||||
if (e.has_value())
|
||||
CollideBoxes(a, b, e.value());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The inverse of a transform made of a position, rotation and scale: undo the move, turn back, then unscale.
|
||||
// Much cheaper than a general 4x4 inverse.
|
||||
static glm::mat4 InverseTransform(const t& transform)
|
||||
{
|
||||
const glm::mat3 rotation = glm::mat3_cast(transform.GetRotationQuaternion());
|
||||
const glm::vec3 inverseScale = 1.0f / glm::max(glm::abs(transform.GetScale()), glm::vec3(1e-6f));
|
||||
|
||||
glm::mat3 inverse = glm::transpose(rotation);
|
||||
for (int column = 0; column < 3; column++)
|
||||
inverse[column] *= inverseScale; // scales each row, the unscale comes after the turn back
|
||||
|
||||
glm::mat4 result(inverse);
|
||||
result[3] = glm::vec4(-(inverse * transform.GetTranslation()), 1.0f);
|
||||
return result;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// CollideTriangles
|
||||
// Tests every triangle of one body against the other's BVH. Triangles have no
|
||||
@@ -273,10 +371,11 @@ void Physics::CollideTriangles(p* a, p* b)
|
||||
|
||||
const glm::mat4 queryToWorld = (queryIsA ? a : b)->pointer->t.GetMatrix();
|
||||
const glm::mat4 targetToWorld = (queryIsA ? b : a)->pointer->t.GetMatrix();
|
||||
const glm::mat4 queryToTarget = glm::inverse(targetToWorld) * queryToWorld;
|
||||
const glm::mat4 queryToTarget = InverseTransform((queryIsA ? b : a)->pointer->t) * queryToWorld;
|
||||
|
||||
// Box faces are the reliable surface. Between two meshes, b's faces are used.
|
||||
const bool surfaceIsA = !a->collisionMesh && b->collisionMesh;
|
||||
// Something that never moves, like terrain, is the surface. Otherwise box faces are the
|
||||
// reliable surface, and between two meshes, b's faces are used.
|
||||
const bool surfaceIsA = a->velocity != b->velocity ? !a->velocity : (!a->collisionMesh && b->collisionMesh);
|
||||
const bool surfaceIsQuery = surfaceIsA == queryIsA;
|
||||
|
||||
meshContacts.clear();
|
||||
@@ -340,6 +439,83 @@ void Physics::CollideTriangles(p* a, p* b)
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// CollideBoxes
|
||||
// One contact point can't hold a box flat: it tips about it every step and never settles.
|
||||
// So every corner of either box that's inside the other becomes a contact, pushing along
|
||||
// the separating axis TInT found. Edge on edge touches have no such corners and keep TInT's point.
|
||||
// ============================================================================
|
||||
void Physics::CollideBoxes(p* a, p* b, const TInTInfo& info)
|
||||
{
|
||||
static const glm::vec3 corners[8] = {
|
||||
{ 0.5f, 0.5f, 0.5f }, { 0.5f, -0.5f, 0.5f }, { 0.5f, 0.5f, -0.5f }, { 0.5f, -0.5f, -0.5f },
|
||||
{ -0.5f, 0.5f, 0.5f }, { -0.5f, -0.5f, 0.5f }, { -0.5f, 0.5f, -0.5f }, { -0.5f, -0.5f, -0.5f },
|
||||
};
|
||||
const float tolerance = 0.01f; // world units, so a corner resting right on the surface still counts
|
||||
|
||||
const t& ta = a->pointer->t;
|
||||
const t& tb = b->pointer->t;
|
||||
const glm::mat4 aToWorld = ta.GetMatrix();
|
||||
const glm::mat4 bToWorld = tb.GetMatrix();
|
||||
const glm::mat4 worldToA = InverseTransform(ta);
|
||||
const glm::mat4 worldToB = InverseTransform(tb);
|
||||
const glm::vec3 insideA = glm::vec3(0.5f) + tolerance / glm::max(ta.GetScale(), glm::vec3(1e-4f));
|
||||
const glm::vec3 insideB = glm::vec3(0.5f) + tolerance / glm::max(tb.GetScale(), glm::vec3(1e-4f));
|
||||
|
||||
// CN points from b to a, so b's surface is its furthest point along it and a's is its nearest
|
||||
glm::vec3 worldA[8], worldB[8];
|
||||
float surfaceB = -FLT_MAX;
|
||||
float surfaceA = FLT_MAX;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
worldA[i] = glm::vec3(aToWorld * glm::vec4(corners[i], 1.0f));
|
||||
worldB[i] = glm::vec3(bToWorld * glm::vec4(corners[i], 1.0f));
|
||||
surfaceA = glm::min(surfaceA, glm::dot(worldA[i], info.CN));
|
||||
surfaceB = glm::max(surfaceB, glm::dot(worldB[i], info.CN));
|
||||
}
|
||||
|
||||
meshContacts.clear();
|
||||
auto AddCorner = [&](const glm::vec3& point, float depth) {
|
||||
Collision contact(a, b);
|
||||
contact.POI = point;
|
||||
contact.CN = info.CN;
|
||||
contact.overlap = glm::clamp(depth, 0.0f, info.overlap);
|
||||
meshContacts.push_back(contact);
|
||||
};
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(glm::vec3(worldToB * glm::vec4(worldA[i], 1.0f))), insideB)))
|
||||
AddCorner(worldA[i], surfaceB - glm::dot(worldA[i], info.CN));
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(glm::vec3(worldToA * glm::vec4(worldB[i], 1.0f))), insideA)))
|
||||
AddCorner(worldB[i], glm::dot(worldB[i], info.CN) - surfaceA);
|
||||
}
|
||||
|
||||
if (meshContacts.empty()) {
|
||||
Collision contact(a, b);
|
||||
contact.POI = BoxContactPoint(ta, tb);
|
||||
contact.CN = info.CN;
|
||||
contact.overlap = info.overlap;
|
||||
collisions.push_back(contact);
|
||||
return;
|
||||
}
|
||||
|
||||
KeepSpreadContacts(meshContacts);
|
||||
for (Collision& contact : meshContacts) {
|
||||
contact.correctionWeight = 1.0f / (float)meshContacts.size();
|
||||
collisions.push_back(contact);
|
||||
}
|
||||
}
|
||||
|
||||
// Whatever was asleep on or against a body that's going away would otherwise hang there in the air
|
||||
void Physics::WakeAround(p* removed)
|
||||
{
|
||||
BoundingBox around = GetBounds(removed);
|
||||
around.min -= glm::vec3(0.05f);
|
||||
around.max += glm::vec3(0.05f);
|
||||
for (p* body : objects) {
|
||||
if (body->isSleeping && BoundingBoxInBoundingBox(GetBounds(body), around))
|
||||
body->WakeUp();
|
||||
}
|
||||
}
|
||||
|
||||
void Physics::WakeGroup(int group)
|
||||
{
|
||||
for (p* body : objects) {
|
||||
@@ -496,91 +672,130 @@ void Physics::DampJoint(BallJoint& joint, float dt)
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Resolve
|
||||
// Contacts
|
||||
// Solved the same way as the joints. Prepare works out everything that only
|
||||
// depends on where things are, then Solve runs many times a step. The impulse
|
||||
// is kept as a running total that can't go below zero, so a later pass can
|
||||
// take back an earlier pass's overshoot, but a contact can never pull.
|
||||
// ============================================================================
|
||||
void Physics::Resolve(Collision* c, bool firstPass)
|
||||
static void ApplyImpulse(p* body, const glm::vec3& impulse, const glm::vec3& r)
|
||||
{
|
||||
p* a = c->ObjA;
|
||||
p* b = c->ObjB;
|
||||
if (!body->velocity)
|
||||
return;
|
||||
body->linearvelocity += body->GetInvMass() * impulse;
|
||||
if (!body->lockRotation)
|
||||
body->angularvelocity += body->invInertia * glm::cross(r, impulse);
|
||||
}
|
||||
|
||||
a->WakeUp();
|
||||
b->WakeUp();
|
||||
static glm::vec3 RelativeVelocity(const Collision& c)
|
||||
{
|
||||
const glm::vec3 velA = c.ObjA->linearvelocity + glm::cross(c.ObjA->angularvelocity, c.rA);
|
||||
const glm::vec3 velB = c.ObjB->linearvelocity + glm::cross(c.ObjB->angularvelocity, c.rB);
|
||||
return velA - velB;
|
||||
}
|
||||
|
||||
const glm::vec3 n = c->CN;
|
||||
const glm::vec3 rA = c->POI - a->pointer->t.GetTranslation();
|
||||
const glm::vec3 rB = c->POI - b->pointer->t.GetTranslation();
|
||||
|
||||
const glm::mat3& invIA = a->invInertia;
|
||||
const glm::mat3& invIB = b->invInertia;
|
||||
|
||||
const float invMassA = a->GetInvMass();
|
||||
const float invMassB = b->GetInvMass();
|
||||
|
||||
const glm::vec3 velA = a->linearvelocity + glm::cross(a->angularvelocity, rA);
|
||||
const glm::vec3 velB = b->linearvelocity + glm::cross(b->angularvelocity, rB);
|
||||
const glm::vec3 relVel = velA - velB;
|
||||
|
||||
const float relVelN = glm::dot(relVel, n);
|
||||
if (relVelN > 0.0f) return;
|
||||
|
||||
const float e = (!firstPass || glm::abs(relVelN) < PHYSICS_RESTITUTION_THRESHOLD)
|
||||
? 0.0f
|
||||
: glm::min(a->restitution, b->restitution);
|
||||
|
||||
auto angDenom = [](const glm::mat3& invI, const glm::vec3& r, const glm::vec3& d) {
|
||||
// One over how hard it is to push the two points apart along direction
|
||||
static float InverseDenominator(const Collision& c, const glm::vec3& direction)
|
||||
{
|
||||
auto angular = [](const glm::mat3& invI, const glm::vec3& r, const glm::vec3& d) {
|
||||
return glm::dot(glm::cross(invI * glm::cross(r, d), r), d);
|
||||
};
|
||||
const float denom = c.ObjA->GetInvMass() + c.ObjB->GetInvMass()
|
||||
+ angular(c.ObjA->invInertia, c.rA, direction) + angular(c.ObjB->invInertia, c.rB, direction);
|
||||
return denom > 1e-8f ? 1.0f / denom : 0.0f;
|
||||
}
|
||||
|
||||
// ---- Normal impulse ----
|
||||
const float denomN = invMassA + invMassB + angDenom(invIA, rA, n) + angDenom(invIB, rB, n);
|
||||
const float jN = -(1.0f + e) * relVelN / denomN;
|
||||
|
||||
const glm::vec3 impulseN = jN * n;
|
||||
if (a->velocity) {
|
||||
a->linearvelocity += invMassA * impulseN;
|
||||
if (!a->lockRotation)
|
||||
a->angularvelocity += invIA * glm::cross(rA, impulseN);
|
||||
}
|
||||
if (b->velocity) {
|
||||
b->linearvelocity -= invMassB * impulseN;
|
||||
if (!b->lockRotation)
|
||||
b->angularvelocity -= invIB * glm::cross(rB, impulseN);
|
||||
// Something moving into a sleeping body wakes it. Something merely resting against it doesn't, or the two would
|
||||
// keep each other awake forever, and waking resets the timer every body needs to run down before it can sleep.
|
||||
// A push that does get through to a sleeper wakes it anyway, in UpdateSleep.
|
||||
static void WakeIfDisturbed(p* sleeper, const p* other)
|
||||
{
|
||||
if (!sleeper->isSleeping)
|
||||
return;
|
||||
// Hitboxes get moved by the animation rather than by physics, so they have no velocity to go by
|
||||
if (other->isHitbox) {
|
||||
sleeper->WakeUp();
|
||||
return;
|
||||
}
|
||||
const float linSq = glm::dot(other->linearvelocity, other->linearvelocity);
|
||||
const float angSq = glm::dot(other->angularvelocity, other->angularvelocity);
|
||||
if (linSq > PHYSICS_SLEEP_LINEAR_SQ * 4.0f || angSq > PHYSICS_SLEEP_ANGULAR_SQ * 4.0f)
|
||||
sleeper->WakeUp();
|
||||
}
|
||||
|
||||
// ---- Friction impulse ----
|
||||
glm::vec3 tangent = relVel - relVelN * n;
|
||||
const float tangLen = glm::length(tangent);
|
||||
if (tangLen > 1e-6f) {
|
||||
tangent /= tangLen;
|
||||
void Physics::PrepareContact(Collision& c)
|
||||
{
|
||||
p* a = c.ObjA;
|
||||
p* b = c.ObjB;
|
||||
WakeIfDisturbed(a, b);
|
||||
WakeIfDisturbed(b, a);
|
||||
|
||||
const float denomT = invMassA + invMassB + angDenom(invIA, rA, tangent) + angDenom(invIB, rB, tangent);
|
||||
float jT = -glm::dot(relVel, tangent) / denomT;
|
||||
c.rA = c.POI - a->pointer->t.GetTranslation();
|
||||
c.rB = c.POI - b->pointer->t.GetTranslation();
|
||||
|
||||
const float mu = (a->friction + b->friction) * 0.5f * c->frictionScale;
|
||||
jT = glm::clamp(jT, -mu * jN, mu * jN);
|
||||
// Friction works along two fixed directions across the surface
|
||||
const glm::vec3& n = c.CN;
|
||||
c.tangent1 = glm::abs(n.x) > 0.57f ? glm::vec3(n.y, -n.x, 0.0f) : glm::vec3(0.0f, n.z, -n.y);
|
||||
c.tangent1 = glm::normalize(c.tangent1);
|
||||
c.tangent2 = glm::cross(n, c.tangent1);
|
||||
|
||||
const glm::vec3 impulseT = jT * tangent;
|
||||
if (a->velocity) {
|
||||
a->linearvelocity += invMassA * impulseT;
|
||||
if (!a->lockRotation)
|
||||
a->angularvelocity += invIA * glm::cross(rA, impulseT);
|
||||
}
|
||||
if (b->velocity) {
|
||||
b->linearvelocity -= invMassB * impulseT;
|
||||
if (!b->lockRotation)
|
||||
b->angularvelocity -= invIB * glm::cross(rB, impulseT);
|
||||
}
|
||||
}
|
||||
c.inverseDenomN = InverseDenominator(c, n);
|
||||
c.inverseDenomT1 = InverseDenominator(c, c.tangent1);
|
||||
c.inverseDenomT2 = InverseDenominator(c, c.tangent2);
|
||||
c.mu = (a->friction + b->friction) * 0.5f * c.frictionScale;
|
||||
|
||||
// ---- Baumgarte positional correction ----
|
||||
if (!firstPass)
|
||||
// Bounce off at a share of the speed it came in at, but slow touches just stop, or resting things would hop
|
||||
const float approach = glm::dot(RelativeVelocity(c), n);
|
||||
const float restitution = approach < -PHYSICS_RESTITUTION_THRESHOLD ? glm::min(a->restitution, b->restitution) : 0.0f;
|
||||
c.targetSpeed = -restitution * approach;
|
||||
|
||||
c.normalImpulse = 0.0f;
|
||||
c.tangentImpulse = glm::vec2(0.0f);
|
||||
}
|
||||
|
||||
void Physics::SolveContact(Collision& c)
|
||||
{
|
||||
const glm::vec3& n = c.CN;
|
||||
|
||||
// ---- Normal ----
|
||||
const float speed = glm::dot(RelativeVelocity(c), n);
|
||||
const float total = glm::max(c.normalImpulse + (c.targetSpeed - speed) * c.inverseDenomN, 0.0f);
|
||||
const float normalStep = total - c.normalImpulse;
|
||||
c.normalImpulse = total;
|
||||
ApplyImpulse(c.ObjA, n * normalStep, c.rA);
|
||||
ApplyImpulse(c.ObjB, -n * normalStep, c.rB);
|
||||
|
||||
// ---- Friction, never more than mu times how hard the surfaces press together ----
|
||||
const glm::vec3 relVel = RelativeVelocity(c);
|
||||
glm::vec2 friction = c.tangentImpulse + glm::vec2(
|
||||
-glm::dot(relVel, c.tangent1) * c.inverseDenomT1,
|
||||
-glm::dot(relVel, c.tangent2) * c.inverseDenomT2);
|
||||
const float maxFriction = c.mu * c.normalImpulse;
|
||||
if (glm::length(friction) > maxFriction)
|
||||
friction = glm::length(friction) > 1e-8f ? glm::normalize(friction) * maxFriction : glm::vec2(0.0f);
|
||||
|
||||
const glm::vec2 frictionStep = friction - c.tangentImpulse;
|
||||
c.tangentImpulse = friction;
|
||||
const glm::vec3 frictionImpulse = c.tangent1 * frictionStep.x + c.tangent2 * frictionStep.y;
|
||||
ApplyImpulse(c.ObjA, frictionImpulse, c.rA);
|
||||
ApplyImpulse(c.ObjB, -frictionImpulse, c.rB);
|
||||
}
|
||||
|
||||
// Baumgarte: nudges overlapping bodies apart a little each step
|
||||
void Physics::CorrectContactPosition(Collision& c)
|
||||
{
|
||||
p* a = c.ObjA;
|
||||
p* b = c.ObjB;
|
||||
const float invMassA = a->GetInvMass();
|
||||
const float invMassB = b->GetInvMass();
|
||||
if (invMassA + invMassB <= 0.0f)
|
||||
return;
|
||||
|
||||
const float correction = glm::max(c->overlap - PHYSICS_BAUMGARTE_SLOP, 0.0f)
|
||||
const float correction = glm::max(c.overlap - PHYSICS_BAUMGARTE_SLOP, 0.0f)
|
||||
/ (invMassA + invMassB)
|
||||
* PHYSICS_BAUMGARTE_FACTOR
|
||||
* c->correctionWeight;
|
||||
const glm::vec3 corrVec = correction * n;
|
||||
* c.correctionWeight;
|
||||
const glm::vec3 corrVec = correction * c.CN;
|
||||
if (a->velocity) a->pointer->t.TranslateBy(invMassA * corrVec);
|
||||
if (b->velocity) b->pointer->t.TranslateBy(-invMassB * corrVec);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user