#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);
}