solver_math.h

#pragma once
 
#include <algorithm>
 
#include <glm/glm.hpp>
#include <glm/gtc/constants.hpp>
 
#include "system/physics/solver/solver.h"
 
namespace Vkm::Engine::SolverMath {

inline glm::vec3 velocityAt(const PhysicsBody& body, const glm::vec3& r) {
    return body.linearVelocity + glm::cross(body.angularVelocity, r);
}

inline void applyImpulse(PhysicsBody& body, const glm::vec3& impulse, const glm::vec3& r, float sign) {
    body.linearVelocity  += impulse * (body.invMass * sign);
    body.angularVelocity += body.invInertiaWorld * glm::cross(r, impulse * sign);
}

inline float effectiveMass(
    const PhysicsBody& a,
    const PhysicsBody& b,
    const glm::vec3& rA,
    const glm::vec3& rB,
    const glm::vec3& direction
) {
    const glm::vec3 crossA = glm::cross(rA, direction);
    const glm::vec3 crossB = glm::cross(rB, direction);
    const float k = a.invMass + b.invMass
        + glm::dot(crossA, a.invInertiaWorld * crossA)
        + glm::dot(crossB, b.invInertiaWorld * crossB);
    return k > 0.0f ? 1.0f / k : 0.0f;
}

struct SoftConstraint {
    float biasRate     = 0.0f;
    float massScale    = 1.0f;
    float impulseScale = 0.0f;
};

inline SoftConstraint softness(float hertz, float damping, float dt) {
    if (dt <= 0.0f) return {};
    // Half the tick rate is what the step can carry; without damping and relax passes it would ring.
    const float clamped = std::min(hertz, 0.5f / dt);
    const float omega   = 2.0f * glm::pi<float>() * clamped;
    const float a1      = 2.0f * damping + dt * omega;
    const float a2      = dt * omega * a1;
    const float a3      = 1.0f / (1.0f + a2);
    return SoftConstraint{omega / a1, a2 * a3, a3};
}
 
} // namespace Vkm::Engine::SolverMath