stuff
This commit is contained in:
+427
-15
@@ -1,5 +1,8 @@
|
||||
#include "Physics.h"
|
||||
|
||||
#include <glm/gtc/constants.hpp>
|
||||
#include <glm/gtc/quaternion.hpp>
|
||||
|
||||
// ============================================================================
|
||||
// ComputeInvInertiaTensor
|
||||
// Returns zero matrix for static or rotation-locked bodies.
|
||||
@@ -27,7 +30,8 @@ glm::mat3 Physics::ComputeInvInertiaTensor(const p* body)
|
||||
// ============================================================================
|
||||
bool Physics::UpdateSleep(p* body, float dt)
|
||||
{
|
||||
if (!body->velocity) return true;
|
||||
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);
|
||||
@@ -53,21 +57,27 @@ bool Physics::UpdateSleep(p* body, float dt)
|
||||
// ============================================================================
|
||||
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) {
|
||||
if (!body->velocity || body->isSleeping) continue;
|
||||
// Nothing rotates until the end of the step, so everything until then can share this
|
||||
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 += (ComputeInvInertiaTensor(body) * body->torque) * dt;
|
||||
body->angularvelocity += (body->invInertia * body->torque) * 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;
|
||||
body->linearvelocity *= std::pow(PHYSICS_LINEAR_DAMPING, steps);
|
||||
body->angularvelocity *= std::pow(PHYSICS_ANGULAR_DAMPING, steps);
|
||||
body->linearvelocity *= linearDamping;
|
||||
body->angularvelocity *= angularDamping;
|
||||
|
||||
body->force = glm::vec3(0.0f);
|
||||
body->torque = glm::vec3(0.0f);
|
||||
@@ -80,17 +90,415 @@ void Physics::Step(float dt)
|
||||
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)
|
||||
if (!body->lockRotation) {
|
||||
body->pointer->t.RotateByQuaternion(glm::quat(body->angularvelocity * dt));
|
||||
if (body->collisionMesh) body->pointer->t.NormalizeRotation();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 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
|
||||
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++) {
|
||||
for (BallJoint* joint : joints)
|
||||
SolveJoint(*joint);
|
||||
for (Collision& col : collisions)
|
||||
Resolve(&col, false);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// FindCollisions
|
||||
// Sort and sweep: once sorted along x, a body can only touch the ones after it
|
||||
// that start before it ends.
|
||||
// ============================================================================
|
||||
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);
|
||||
}
|
||||
|
||||
std::sort(broadPhase.begin(), broadPhase.end(), [](const BroadPhaseEntry& first, const BroadPhaseEntry& second) {
|
||||
return first.bounds.min.x < second.bounds.min.x;
|
||||
});
|
||||
|
||||
for (int i = 0; i < (int)broadPhase.size(); i++) {
|
||||
const BroadPhaseEntry& first = broadPhase[i];
|
||||
p* a = first.body;
|
||||
|
||||
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 (!a->velocity && !b->velocity) continue;
|
||||
if (a->isSleeping && b->isSleeping) continue;
|
||||
if ((a->frozen || !a->velocity) && (b->frozen || !b->velocity)) continue;
|
||||
if (a->collisionGroup != 0 && a->collisionGroup == b->collisionGroup) 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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 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 = glm::inverse(targetToWorld) * queryToWorld;
|
||||
|
||||
// Box faces are the reliable surface. Between two meshes, b's faces are used.
|
||||
const bool surfaceIsA = !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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Resolve
|
||||
// ============================================================================
|
||||
void Physics::Resolve(Collision* c)
|
||||
void Physics::Resolve(Collision* c, bool firstPass)
|
||||
{
|
||||
p* a = c->ObjA;
|
||||
p* b = c->ObjB;
|
||||
@@ -102,8 +510,8 @@ void Physics::Resolve(Collision* c)
|
||||
const glm::vec3 rA = c->POI - a->pointer->t.GetTranslation();
|
||||
const glm::vec3 rB = c->POI - b->pointer->t.GetTranslation();
|
||||
|
||||
const glm::mat3 invIA = ComputeInvInertiaTensor(a);
|
||||
const glm::mat3 invIB = ComputeInvInertiaTensor(b);
|
||||
const glm::mat3& invIA = a->invInertia;
|
||||
const glm::mat3& invIB = b->invInertia;
|
||||
|
||||
const float invMassA = a->GetInvMass();
|
||||
const float invMassB = b->GetInvMass();
|
||||
@@ -115,7 +523,7 @@ void Physics::Resolve(Collision* c)
|
||||
const float relVelN = glm::dot(relVel, n);
|
||||
if (relVelN > 0.0f) return;
|
||||
|
||||
const float e = (glm::abs(relVelN) < PHYSICS_RESTITUTION_THRESHOLD)
|
||||
const float e = (!firstPass || glm::abs(relVelN) < PHYSICS_RESTITUTION_THRESHOLD)
|
||||
? 0.0f
|
||||
: glm::min(a->restitution, b->restitution);
|
||||
|
||||
@@ -148,7 +556,7 @@ void Physics::Resolve(Collision* c)
|
||||
const float denomT = invMassA + invMassB + angDenom(invIA, rA, tangent) + angDenom(invIB, rB, tangent);
|
||||
float jT = -glm::dot(relVel, tangent) / denomT;
|
||||
|
||||
const float mu = (a->friction + b->friction) * 0.5f;
|
||||
const float mu = (a->friction + b->friction) * 0.5f * c->frictionScale;
|
||||
jT = glm::clamp(jT, -mu * jN, mu * jN);
|
||||
|
||||
const glm::vec3 impulseT = jT * tangent;
|
||||
@@ -165,10 +573,14 @@ void Physics::Resolve(Collision* c)
|
||||
}
|
||||
|
||||
// ---- Baumgarte positional correction ----
|
||||
if (!firstPass)
|
||||
return;
|
||||
|
||||
const float correction = glm::max(c->overlap - PHYSICS_BAUMGARTE_SLOP, 0.0f)
|
||||
/ (invMassA + invMassB)
|
||||
* PHYSICS_BAUMGARTE_FACTOR;
|
||||
* PHYSICS_BAUMGARTE_FACTOR
|
||||
* c->correctionWeight;
|
||||
const glm::vec3 corrVec = correction * n;
|
||||
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