johnruina/combattest
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5d7aa3b93b51cee955ea64c284e1043411b32137
combattest/Physics.cpp
T
johnruina 36e093f84c readme
2026-11-08 07:45:01 -05:00

802 lines
32 KiB
C++
Raw Blame History

#include "Physics.h"
#include <glm/gtc/constants.hpp>
#include <glm/gtc/quaternion.hpp>
// ============================================================================
// ComputeInvInertiaTensor
// Returns zero matrix for static or rotation-locked bodies.
// ============================================================================
glm::mat3 Physics::ComputeInvInertiaTensor(const p* body)
{
if (!body->velocity || body->lockRotation)
return glm::mat3(0.0f);
constexpr float k = 1.0f / 12.0f;
const glm::vec3 s = body->pointer->t.GetScale();
const glm::vec3 sq = s * s;
glm::mat3 localInvI(0.0f);
localInvI[0][0] = 1.0f / (k * body->mass * (sq.y + sq.z));
localInvI[1][1] = 1.0f / (k * body->mass * (sq.x + sq.z));
localInvI[2][2] = 1.0f / (k * body->mass * (sq.x + sq.y));
const glm::mat3 R = glm::mat3(body->pointer->t.GetRotationMatrix());
return R * localInvI * glm::transpose(R);
}
// ============================================================================
// UpdateSleep
// ============================================================================
bool Physics::UpdateSleep(p* body, float dt)
{
if (!body->velocity || body->frozen) return true;
if (body->collisionGroup != 0) return false; // groups settle as a whole, a joint can't hold a sleeping body
const float linSq = glm::dot(body->linearvelocity, body->linearvelocity);
const float angSq = glm::dot(body->angularvelocity, body->angularvelocity);
if (linSq < PHYSICS_SLEEP_LINEAR_SQ && angSq < PHYSICS_SLEEP_ANGULAR_SQ) {
body->sleepTimer += dt;
if (body->sleepTimer >= PHYSICS_SLEEP_TIME_THRESHOLD) {
body->isSleeping = true;
body->linearvelocity = glm::vec3(0.0f);
body->angularvelocity = glm::vec3(0.0f);
return true;
}
}
else {
body->sleepTimer = 0.0f;
body->isSleeping = false;
}
return false;
}
// ============================================================================
// Step
// ============================================================================
void Physics::Step(float dt)
{
// The damping constants are per 1/60 s step; rescale so drag doesn't depend on frame rate
const float steps = dt * 60.0f;
const float linearDamping = std::pow(PHYSICS_LINEAR_DAMPING, steps);
const float angularDamping = std::pow(PHYSICS_ANGULAR_DAMPING, steps);
// Integrate forces ? velocities
for (p* body : objects) {
// 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;
body->force += body->mass * gravity;
body->linearvelocity += (body->force / body->mass) * dt;
if (!body->lockRotation)
body->angularvelocity += (body->invInertia * body->torque) * dt;
body->linearvelocity *= linearDamping;
body->angularvelocity *= angularDamping;
body->force = glm::vec3(0.0f);
body->torque = glm::vec3(0.0f);
}
ResolveCollisions(dt);
// Integrate velocities ? positions
for (p* body : objects) {
if (!body->velocity) continue;
if (UpdateSleep(body, dt)) continue;
if (body->maxSpeed > 0.0f && glm::length(body->linearvelocity) > body->maxSpeed)
body->linearvelocity = glm::normalize(body->linearvelocity) * body->maxSpeed;
if (body->maxAngularSpeed > 0.0f && glm::length(body->angularvelocity) > body->maxAngularSpeed)
body->angularvelocity = glm::normalize(body->angularvelocity) * body->maxAngularSpeed;
body->pointer->t.TranslateBy(body->linearvelocity * dt);
if (!body->lockRotation) {
body->pointer->t.RotateByQuaternion(glm::quat(body->angularvelocity * dt));
if (body->collisionMesh) body->pointer->t.NormalizeRotation();
}
}
}
// ============================================================================
// 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)
PrepareContact(col);
for (BallJoint* joint : joints) {
DampJoint(*joint, dt);
PrepareJoint(*joint, dt);
}
// 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)
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 });
}
}
// ============================================================================
// FindCollisions
// 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();
// 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);
// 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;
// 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->isHitbox && b->collisionGroup != 0) || (b->isHitbox && a->collisionGroup != 0)) continue;
if (!BoundingBoxInBoundingBox(first.bounds, second.bounds)) continue;
if (a->collisionMesh || b->collisionMesh) {
CollideTriangles(a, b);
continue;
}
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
// volume, so depth comes from how far the intersecting triangle reaches behind
// the other triangle's outward facing plane.
// ============================================================================
static Triangle TransformTriangle(const Triangle& tri, const glm::mat4& matrix)
{
return {
glm::vec3(matrix * glm::vec4(tri.a, 1.0f)),
glm::vec3(matrix * glm::vec4(tri.b, 1.0f)),
glm::vec3(matrix * glm::vec4(tri.c, 1.0f))
};
}
// Keeps the deepest contact, then whichever contacts spread the rest out the most
static void KeepSpreadContacts(std::vector<Collision>& contacts)
{
if ((int)contacts.size() <= PHYSICS_MAX_MESH_CONTACTS)
return;
int deepest = 0;
for (int i = 1; i < (int)contacts.size(); i++) {
if (contacts[i].overlap > contacts[deepest].overlap)
deepest = i;
}
std::swap(contacts[0], contacts[deepest]);
for (int kept = 1; kept < PHYSICS_MAX_MESH_CONTACTS; kept++) {
int farthest = kept;
float farthestDistance = -1.0f;
for (int i = kept; i < (int)contacts.size(); i++) {
float nearest = FLT_MAX;
for (int j = 0; j < kept; j++) {
const glm::vec3 offset = contacts[i].POI - contacts[j].POI;
nearest = glm::min(nearest, glm::dot(offset, offset));
}
if (nearest > farthestDistance) {
farthestDistance = nearest;
farthest = i;
}
}
std::swap(contacts[kept], contacts[farthest]);
}
contacts.erase(contacts.begin() + PHYSICS_MAX_MESH_CONTACTS, contacts.end());
}
// Two meshes can tangle: a hand between two faces gets contacts pushing opposite ways, which cancel
// and pin it in place. So the whole pair shares one normal that always pushes them apart.
static void UnifyContactNormals(const p* a, const p* b, std::vector<Collision>& contacts)
{
glm::vec3 summed(0.0f);
float totalDepth = 0.0f;
for (const Collision& contact : contacts) {
summed += contact.CN * contact.overlap;
totalDepth += contact.overlap;
}
// Faces that mostly disagree say nothing useful, so fall back to the line between the two bodies
const glm::vec3 apart = a->pointer->t.GetTranslation() - b->pointer->t.GetTranslation();
glm::vec3 normal = summed;
if (glm::length(summed) < totalDepth * 0.3f)
normal = apart;
if (glm::length(normal) < 1e-6f)
return;
normal = glm::normalize(normal);
if (glm::dot(normal, apart) < 0.0f)
normal = -normal;
for (Collision& contact : contacts) {
contact.CN = normal;
contact.frictionScale = PHYSICS_MESH_PAIR_FRICTION;
}
}
void Physics::CollideTriangles(p* a, p* b)
{
const CollisionMesh& meshA = a->collisionMesh ? *a->collisionMesh : CollisionMesh::UnitBox();
const CollisionMesh& meshB = b->collisionMesh ? *b->collisionMesh : CollisionMesh::UnitBox();
// Loop over the smaller mesh and search the larger one
const bool queryIsA = meshA.GetTriangles().size() <= meshB.GetTriangles().size();
const CollisionMesh& queryMesh = queryIsA ? meshA : meshB;
const CollisionMesh& targetMesh = queryIsA ? meshB : meshA;
const glm::mat4 queryToWorld = (queryIsA ? a : b)->pointer->t.GetMatrix();
const glm::mat4 targetToWorld = (queryIsA ? b : a)->pointer->t.GetMatrix();
const glm::mat4 queryToTarget = InverseTransform((queryIsA ? b : a)->pointer->t) * queryToWorld;
// 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();
for (const Triangle& queryTriangle : queryMesh.GetTriangles()) {
const Triangle queryInTarget = TransformTriangle(queryTriangle, queryToTarget);
meshCandidates.clear();
targetMesh.Query(queryInTarget, meshCandidates);
for (int index : meshCandidates) {
const Triangle& targetTriangle = targetMesh.GetTriangles()[index];
if (!TriangleInTriangle(queryInTarget, targetTriangle))
continue;
const Triangle queryWorld = TransformTriangle(queryTriangle, queryToWorld);
const Triangle targetWorld = TransformTriangle(targetTriangle, targetToWorld);
const Triangle& surface = surfaceIsQuery ? queryWorld : targetWorld;
const Triangle& other = surfaceIsQuery ? targetWorld : queryWorld;
const glm::vec3 faceNormal = glm::cross(surface.b - surface.a, surface.c - surface.a);
const float faceLength = glm::length(faceNormal);
if (faceLength < 1e-8f)
continue;
const glm::vec3 normal = faceNormal / faceLength;
glm::vec3 deepestPoint = other.a;
float deepestDistance = glm::dot(other.a - surface.a, normal);
for (const glm::vec3& corner : { other.b, other.c }) {
const float distance = glm::dot(corner - surface.a, normal);
if (distance < deepestDistance) {
deepestDistance = distance;
deepestPoint = corner;
}
}
if (deepestDistance >= 0.0f)
continue;
// CN points from b to a, the same way TInT does
Collision contact(a, b);
contact.POI = deepestPoint;
contact.CN = surfaceIsA ? -normal : normal;
contact.overlap = glm::min(-deepestDistance, PHYSICS_MAX_MESH_DEPTH);
meshContacts.push_back(contact);
}
}
if (meshContacts.empty())
return;
if (a->frozen) WakeGroup(a->collisionGroup);
if (b->frozen) WakeGroup(b->collisionGroup);
if (a->collisionMesh && b->collisionMesh)
UnifyContactNormals(a, b, meshContacts);
KeepSpreadContacts(meshContacts);
for (Collision& contact : meshContacts) {
contact.correctionWeight = 1.0f / (float)meshContacts.size();
collisions.push_back(contact);
}
}
// ============================================================================
// 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) {
if (body->collisionGroup == group)
body->frozen = false;
}
}
// ============================================================================
// Joints
// Everything that only depends on where the bodies are gets worked out once in
// Prepare. Solve then just pushes velocities around, and runs many times a step.
// ============================================================================
static glm::mat3 SkewMatrix(const glm::vec3& v)
{
// Skew * x = cross(v, x). glm matrices are built from columns.
return glm::mat3(
glm::vec3(0.0f, v.z, -v.y),
glm::vec3(-v.z, 0.0f, v.x),
glm::vec3(v.y, -v.x, 0.0f));
}
// Asks for the child's spin relative to the parent around axis to close error
static void AddAngularRow(BallJoint& joint, const glm::vec3& axis, float error, float dt, bool oneSided)
{
AngularRow& row = joint.rows[joint.rowCount++];
row.axis = axis;
row.oneSided = oneSided;
row.targetSpeed = glm::clamp(error * (PHYSICS_JOINT_BIAS / dt),
-PHYSICS_JOINT_MAX_BIAS_SPEED, PHYSICS_JOINT_MAX_BIAS_SPEED);
row.inverseDenom = 1.0f / (glm::dot(axis, joint.parent->invInertia * axis) + glm::dot(axis, joint.child->invInertia * axis));
}
void Physics::PrepareJoint(BallJoint& joint, float dt)
{
p* a = joint.parent;
p* b = joint.child;
if (a->frozen && b->frozen)
return;
const glm::quat orientA = a->pointer->t.GetRotationQuaternion();
const glm::quat orientB = b->pointer->t.GetRotationQuaternion();
const glm::vec3 rA = orientA * joint.anchorParent;
const glm::vec3 rB = orientB * joint.anchorChild;
joint.worldAnchorParent = rA;
joint.worldAnchorChild = rB;
const glm::mat3 skewA = SkewMatrix(rA);
const glm::mat3 skewB = SkewMatrix(rB);
const glm::mat3 K = glm::mat3(a->GetInvMass() + b->GetInvMass())
- skewA * a->invInertia * skewA - skewB * b->invInertia * skewB;
joint.inverseK = glm::inverse(K);
const glm::vec3 drift = (b->pointer->t.GetTranslation() + rB) - (a->pointer->t.GetTranslation() + rA);
joint.bias = drift * (PHYSICS_JOINT_BIAS / dt);
if (glm::length(joint.bias) > PHYSICS_JOINT_MAX_BIAS_SPEED)
joint.bias = glm::normalize(joint.bias) * PHYSICS_JOINT_MAX_BIAS_SPEED;
joint.rowCount = 0;
// ---- Hinge ----
if (joint.isHinge) {
const glm::vec3 hingeA = orientA * joint.hingeAxisParent;
const glm::vec3 hingeB = orientB * joint.hingeAxisChild;
// Keep both hinge axes lined up, leaving only spin around the hinge free
const glm::vec3 misalignment = glm::cross(hingeB, hingeA);
const glm::vec3 side = glm::normalize(orientA * joint.hingeRefParent);
const glm::vec3 otherSide = glm::cross(hingeA, side);
AddAngularRow(joint, side, glm::dot(misalignment, side), dt, false);
AddAngularRow(joint, otherSide, glm::dot(misalignment, otherSide), dt, false);
const glm::vec3 refA = side;
const glm::vec3 refB = orientB * joint.hingeRefChild;
const float bend = std::atan2(glm::dot(glm::cross(refA, refB), hingeA), glm::dot(refA, refB));
if (bend < joint.minBend)
AddAngularRow(joint, hingeA, joint.minBend - bend, dt, true);
else if (bend > joint.maxBend)
AddAngularRow(joint, -hingeA, bend - joint.maxBend, dt, true);
return;
}
// ---- Swing cone ----
const glm::vec3 axisA = orientA * joint.axisParent;
const glm::vec3 axisB = orientB * joint.axisChild;
const float angle = glm::acos(glm::clamp(glm::dot(axisA, axisB), -1.0f, 1.0f));
if (angle > joint.maxAngle) {
const glm::vec3 swingAxis = glm::cross(axisB, axisA);
const float swingLength = glm::length(swingAxis);
if (swingLength > 1e-5f)
AddAngularRow(joint, swingAxis / swingLength, angle - joint.maxAngle, dt, true);
}
// ---- Twist limit ----
const glm::quat relative = glm::conjugate(orientA) * orientB;
const float twistAlongAxis = glm::dot(glm::vec3(relative.x, relative.y, relative.z), joint.axisChild);
float twist = 2.0f * std::atan2(twistAlongAxis, relative.w);
const float pi = glm::pi<float>();
if (twist > pi) twist -= 2.0f * pi;
else if (twist < -pi) twist += 2.0f * pi;
const glm::vec3 twistAxis = orientA * joint.axisChild;
if (twist > joint.maxTwist)
AddAngularRow(joint, -twistAxis, twist - joint.maxTwist, dt, true);
else if (twist < -joint.maxTwist)
AddAngularRow(joint, twistAxis, -joint.maxTwist - twist, dt, true);
}
void Physics::SolveJoint(BallJoint& joint)
{
p* a = joint.parent;
p* b = joint.child;
if (a->frozen && b->frozen)
return;
const glm::vec3& rA = joint.worldAnchorParent;
const glm::vec3& rB = joint.worldAnchorChild;
const glm::vec3 relVel = b->linearvelocity + glm::cross(b->angularvelocity, rB)
- a->linearvelocity - glm::cross(a->angularvelocity, rA);
const glm::vec3 impulse = joint.inverseK * (-relVel - joint.bias);
a->linearvelocity -= a->GetInvMass() * impulse;
a->angularvelocity -= a->invInertia * glm::cross(rA, impulse);
b->linearvelocity += b->GetInvMass() * impulse;
b->angularvelocity += b->invInertia * glm::cross(rB, impulse);
for (int i = 0; i < joint.rowCount; i++) {
const AngularRow& row = joint.rows[i];
const float speed = glm::dot(b->angularvelocity - a->angularvelocity, row.axis);
const float rowImpulse = (row.targetSpeed - speed) * row.inverseDenom;
if (row.oneSided && rowImpulse <= 0.0f)
continue;
a->angularvelocity -= a->invInertia * row.axis * rowImpulse;
b->angularvelocity += b->invInertia * row.axis * rowImpulse;
}
}
// Joints have no friction of their own, so without this limbs flail for a long time
void Physics::DampJoint(BallJoint& joint, float dt)
{
p* a = joint.parent;
p* b = joint.child;
if (a->frozen && b->frozen)
return;
const glm::vec3 relative = b->angularvelocity - a->angularvelocity;
const glm::vec3 removed = relative * (1.0f - std::exp(-PHYSICS_JOINT_DAMPING * dt)) * 0.5f;
a->angularvelocity += removed;
b->angularvelocity -= removed;
}
// ============================================================================
// 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.
// ============================================================================
static void ApplyImpulse(p* body, const glm::vec3& impulse, const glm::vec3& r)
{
if (!body->velocity)
return;
body->linearvelocity += body->GetInvMass() * impulse;
if (!body->lockRotation)
body->angularvelocity += body->invInertia * glm::cross(r, impulse);
}
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;
}
// 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;
}
// 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();
}
void Physics::PrepareContact(Collision& c)
{
p* a = c.ObjA;
p* b = c.ObjB;
WakeIfDisturbed(a, b);
WakeIfDisturbed(b, a);
c.rA = c.POI - a->pointer->t.GetTranslation();
c.rB = c.POI - b->pointer->t.GetTranslation();
// 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);
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;
// 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)
/ (invMassA + invMassB)
* PHYSICS_BAUMGARTE_FACTOR
* 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 View Git Blame Copy Permalink