Dynamitos/Seele
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

overhauled physics engine

Browse Source
This commit is contained in:
Dynamitos
2023-01-21 18:43:21 +01:00
parent 3c7346cf7b
commit 2208ab438a
164 changed files with 22606 additions and 928 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/Engine/Physics/PhysicsSystem.cpp
+156 -120
View File
@@ -1,10 +1,13 @@
#include "PhysicsSystem.h"
#include <boost/numeric/odeint.hpp>
#include <random>
using namespace Seele;
using namespace Seele::Component;
PhysicsSystem::PhysicsSystem()
PhysicsSystem::PhysicsSystem(entt::registry& registry)
: registry(registry)
, collisionSystem(registry)
{
}
@@ -14,24 +17,25 @@ PhysicsSystem::~PhysicsSystem()
}
void PhysicsSystem::update(entt::registry& registry, float deltaTime)
void PhysicsSystem::update(float deltaTime)
{
Array<Body> initialBodies;
readRigidBodies(initialBodies, registry);
readRigidBodies(initialBodies);
std::cout << "Updating " << initialBodies.size() << " bodies" << std::endl;
Array<Body> bodies = integratePhysics(initialBodies, 0, deltaTime);
writeRigidBodies(bodies, registry);
writeRigidBodies(bodies);
Array<Collision> collisions;
collisionSystem.detectCollisions(registry, collisions);
collisionSystem.detectCollisions(collisions);
if(!collisions.empty())
{
constexpr size_t numSteps = 2;
for (float t = 0; t < deltaTime; t += deltaTime / numSteps)
{
rewindCollisions(initialBodies, registry, t, t + (deltaTime / numSteps), 10);
readRigidBodies(initialBodies, registry);
rewindCollisions(initialBodies, t, t + (deltaTime / numSteps), 10);
readRigidBodies(initialBodies);
}
}
}
@@ -99,7 +103,7 @@ void PhysicsSystem::deserializeArray(Array<Body>& bodies, const Array<float>& x)
}
}
void PhysicsSystem::readRigidBodies(Array<Body>& bodies, entt::registry& registry) const
void PhysicsSystem::readRigidBodies(Array<Body>& bodies) const
{
auto view = registry.view<RigidBody, Transform, Collider>();
bodies.clear();
@@ -118,7 +122,7 @@ void PhysicsSystem::readRigidBodies(Array<Body>& bodies, entt::registry& registr
});
}
void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const
void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies) const
{
for(auto& body : bodies)
{
@@ -135,7 +139,7 @@ void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies, entt::registry&
}
}
PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity, entt::registry& registry) const
PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity) const
{
Body rigidBody;
if(registry.all_of<RigidBody, Collider, Transform>(entity))
@@ -153,7 +157,7 @@ PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity, entt::regi
}
void PhysicsSystem::writeRigidBody(const Body& body, entt::registry& registry) const
void PhysicsSystem::writeRigidBody(const Body& body) const
{
if(registry.all_of<RigidBody, Transform>(body.id))
{
@@ -193,7 +197,7 @@ Array<PhysicsSystem::Body> PhysicsSystem::integratePhysics(const Array<Body>& bo
*xdot++ = result[i].v.z;
// R(t)' = omega(t)*R(t)
Math::Quaternion qdot = 0.5f * (Math::Quaternion(0, result[i].omega) * result[i].q);
Quaternion qdot = 0.5f * (Quaternion(0, result[i].omega) * result[i].q);
*xdot++ = qdot.w;
*xdot++ = qdot.x;
*xdot++ = qdot.y;
@@ -218,7 +222,7 @@ Array<PhysicsSystem::Body> PhysicsSystem::integratePhysics(const Array<Body>& bo
void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry& registry, const float t0, const float t1, size_t remainingRecursionDepth)
void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, const float t0, const float t1, size_t remainingRecursionDepth)
{
if(remainingRecursionDepth == 0)
{
@@ -227,8 +231,8 @@ void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry
// there are collisions happening between t0 and t1
// we integrate until tc and see if they have already occured then
Array<Collision> collisions;
writeRigidBodies(integratePhysics(t0Bodies, t0, t1), registry);
collisionSystem.detectCollisions(registry, collisions);
writeRigidBodies(integratePhysics(t0Bodies, t0, t1));
collisionSystem.detectCollisions(collisions);
//std::cout << "detected " << collisions.size() << " at " << tc << std::endl;
// now we check if there has been a contact at tc
@@ -239,16 +243,17 @@ void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry
const auto&[collider1, transform1] = registry.get<Collider, Transform>(collision.a);
const auto&[collider2, transform2] = registry.get<Collider, Transform>(collision.b);
calculateContacts(collision.a, collider1.physicsMesh.transform(transform1), collision.b, collider2.physicsMesh.transform(transform2), contacts);
calculateContacts(collision.b, collider2.physicsMesh.transform(transform2), collision.a, collider1.physicsMesh.transform(transform1), contacts);
}
// we then apply forces in order to counteract interpenetration
Array<Contact> restingContacts;
for(const auto& contact : contacts)
{
Body a = readRigidBody(contact.a, registry);
Body b = readRigidBody(contact.b, registry);
Math::Vector paDot = a.ptVelocity(contact.p);
Math::Vector pbDot = b.ptVelocity(contact.p);
Body a = readRigidBody(contact.a);
Body b = readRigidBody(contact.b);
Vector paDot = a.ptVelocity(contact.p);
Vector pbDot = b.ptVelocity(contact.p);
float vrel = glm::dot(contact.n, paDot - pbDot);
if(vrel > 0.001f)
{
@@ -262,10 +267,10 @@ void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry
resolvePenetratingContact(contact, a, b);
a.updateMatrix();
b.updateMatrix();
writeRigidBody(a, registry);
writeRigidBody(b, registry);
writeRigidBody(a);
writeRigidBody(b);
}
resolveRestingContacts(restingContacts, registry);
resolveRestingContacts(restingContacts);
}
void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1, entt::entity id2, const ShapeBase& shape2, Array<Contact>& contacts) const
@@ -273,25 +278,33 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
for(size_t i = 0; i < shape1.indices.size(); i += 3)
{
// face - vertex contacts
const Math::Vector point1 = shape1.vertices[shape1.indices[i + 0]];
const Math::Vector point2 = shape1.vertices[shape1.indices[i + 1]];
const Math::Vector point3 = shape1.vertices[shape1.indices[i + 2]];
const Math::Vector v1 = point2 - point1;
const Math::Vector v2 = point3 - point1;
const Math::Vector faceNormal = glm::normalize(glm::cross(v1, v2));
auto area = [](Math::Vector ab, Math::Vector ac){
const Vector point1 = shape1.vertices[shape1.indices[i + 0]];
const Vector point2 = shape1.vertices[shape1.indices[i + 1]];
const Vector point3 = shape1.vertices[shape1.indices[i + 2]];
const Vector v1 = point2 - point1;
const Vector v2 = point3 - point1;
const Vector faceNormal = glm::normalize(glm::cross(v1, v2));
Seele::gDebugVertices.add(DebugVertex{
.position = (point1 + point2 + point3) / 3.f,
.color = Vector(1, 0, 0)
});
Seele::gDebugVertices.add(DebugVertex{
.position = faceNormal + (point1 + point2 + point3) / 3.f,
.color = Vector(1, 0, 0)
});
auto area = [](Vector ab, Vector ac){
return glm::length(glm::cross(ab, ac)) / 2.0f;
};
float faceArea = area(v1, v2);
for(size_t j = 0; j < shape2.vertices.size(); j++)
{
Math::Vector worldPos = shape2.vertices[j];
Vector worldPos = shape2.vertices[j];
float dot = glm::dot(faceNormal, worldPos - point1);
if(dot < 0.2f)
if(std::abs(dot) < 0.2f)
{
Math::Vector pa = point1 - worldPos;
Math::Vector pb = point2 - worldPos;
Math::Vector pc = point3 - worldPos;
Vector pa = point1 - worldPos;
Vector pb = point2 - worldPos;
Vector pc = point3 - worldPos;
float a1 = area(pa, pb);
float a2 = area(pb, pc);
float a3 = area(pc, pa);
@@ -311,16 +324,16 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
contacts.add(c);
}
}
//std::cout << minTemp << std::endl;
// edge - edge contacts
auto lineLineContact = [=, &contacts](Math::Vector p1, Math::Vector p2, Math::Vector p3, Math::Vector p4)
auto lineLineContact = [=, &contacts](Vector p1, Vector p2, Vector p3, Vector p4)
{
//L1 = p1 + t * p2 - p1;
//L2 = p3 + u * p4 - p3;
Math::Vector a = p1;
Math::Vector c = p3;
Math::Vector ab = p2 - p1;
Math::Vector cd = p4 - p3;
Vector a = p1;
Vector c = p3;
Vector ab = p2 - p1;
Vector cd = p4 - p3;
float tx_den = cd.z * ab.y - cd.y * ab.z;
float tx = (c.y * ab.z - a.y * ab.z - c.z * ab.y + a.z * ab.y) / tx_den;
float ty_den = cd.z * ab.x - cd.z * ab.z;
@@ -333,10 +346,10 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
&& tx >= 0.f
&& tx <= 1.f)
{
Math::Vector p = p1 + tx * (p2 - p1);
Math::Vector ea = p2 - p1;
Math::Vector eb = p4 - p3;
Math::Vector n = glm::normalize(glm::cross(eb, ea));
Vector p = p1 + tx * (p2 - p1);
Vector ea = p2 - p1;
Vector eb = p4 - p3;
Vector n = glm::normalize(glm::cross(eb, ea));
Contact contact = {
.a = id1,
.b = id2,
@@ -350,9 +363,9 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
};
for(size_t j = 0; j < shape2.indices.size(); j+=3)
{
const Math::Vector point4 = shape2.vertices[shape2.indices[j + 0]];
const Math::Vector point5 = shape2.vertices[shape2.indices[j + 1]];
const Math::Vector point6 = shape2.vertices[shape2.indices[j + 2]];
const Vector point4 = shape2.vertices[shape2.indices[j + 0]];
const Vector point5 = shape2.vertices[shape2.indices[j + 1]];
const Vector point6 = shape2.vertices[shape2.indices[j + 2]];
lineLineContact(point1, point2, point4, point5);
lineLineContact(point1, point2, point5, point6);
@@ -369,54 +382,46 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
}
}
void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts, entt::registry& registry) const
void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts) const
{
Array<Array<float>> amat(contacts.size(), Array<float>(contacts.size()));
Array<Array<float>> amat(contacts.size());
for(size_t i = 0; i < contacts.size(); ++i)
{
amat[i] = Array<float>(contacts.size());
}
Array<float> bvec(contacts.size());
computeAMatrix(contacts, amat, registry);
computeBVector(contacts, bvec, registry);
computeAMatrix(contacts, amat);
computeBVector(contacts, bvec);
Array<float> fvec(bvec);
solveQP(amat, bvec, fvec);
/*CGAL::Quadratic_program<float> qp(CGAL::SMALLER, true, 0, false, 0);
for(size_t x = 0; x < contacts.size(); ++x)
{
for(size_t y = 0; y < contacts.size(); ++y)
{
qp.set_a(x, y, amat[x][y]);
}
}
for(size_t y = 0; y < contacts.size(); ++y)
{
qp.set_b(y, bvec[y]);
}
CGAL::Quadratic_program_solution<CGAL::MP_Float> s = CGAL::solve_quadratic_program(qp, CGAL::MP_Float());
assert(s.solves_quadratic_program(qp));
auto solutionBegin = s.variable_values_begin();
for(size_t y = 0; y < contacts.size(); ++y)
{
float f = CGAL::to_double(*solutionBegin++);
Math::Vector n = contacts[y].n;
RigidBody a = readRigidBody(contacts[y].a, registry);
RigidBody b = readRigidBody(contacts[y].b, registry);
std::cout << "resting contact force: " << fvec[y] << std::endl;
float f = fvec[y];
Vector n = contacts[y].n;
Body a = readRigidBody(contacts[y].a);
Body b = readRigidBody(contacts[y].b);
a.force += f * n;
a.torque += (contacts[y].p - a.x + a.centerOfMass) * (f * n);
b.force -= f * n;
b.torque -= (contacts[y].p - b.x + b.centerOfMass) * (f * n);
writeRigidBody(a, registry);
writeRigidBody(b, registry);
}*/
writeRigidBody(a);
writeRigidBody(b);
}
}
void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, Body& b) const
{
Math::Vector paDot = a.ptVelocity(contact.p);
Math::Vector pbDot = b.ptVelocity(contact.p);
Vector paDot = a.ptVelocity(contact.p);
Vector pbDot = b.ptVelocity(contact.p);
float vrel = glm::dot(contact.n, paDot - pbDot);
Math::Vector n = contact.n;
Math::Vector ra = contact.p - a.x + a.centerOfMass;
Math::Vector rb = contact.p - b.x + b.centerOfMass;
Vector n = contact.n;
Vector ra = contact.p - a.x + a.centerOfMass;
Vector rb = contact.p - b.x + b.centerOfMass;
float numerator = -(1 + 0.5f) * vrel;
float term1 = a.inverseMass;
@@ -425,7 +430,7 @@ void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, B
float term4 = glm::dot(n, glm::cross(b.iInv * glm::cross(rb, n), rb));
float j = numerator / (term1 + term2 + term3 + term4);
Math::Vector force = j * n;
Vector force = j * n;
a.P += force;
b.P -= force;
@@ -439,7 +444,7 @@ void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, B
b.omega = b.iInv * b.L;
}
Math::Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const
Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const
{
if(c.vf)
{
@@ -447,10 +452,10 @@ Math::Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const B
}
else
{
Math::Vector eadot = glm::cross(a.omega, c.ea);
Math::Vector ebdot = glm::cross(b.omega, c.eb);
Math::Vector n1 = glm::cross(c.ea, c.eb);
Math::Vector z = glm::cross(eadot, c.eb) + glm::cross(c.ea, ebdot);
Vector eadot = glm::cross(a.omega, c.ea);
Vector ebdot = glm::cross(b.omega, c.eb);
Vector n1 = glm::cross(c.ea, c.eb);
Vector z = glm::cross(eadot, c.eb) + glm::cross(c.ea, ebdot);
float l = glm::length(n1);
n1 = glm::normalize(n1);
@@ -458,23 +463,23 @@ Math::Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const B
}
}
float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) const
float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj) const
{
if((ci.a != cj.a) && (ci.b != cj.b) &&
(ci.a != cj.b) && (ci.b != cj.a))
return 0.0f;
Body a = readRigidBody(ci.a, registry);
Body b = readRigidBody(ci.b, registry);
Math::Vector ni = ci.n;
Math::Vector nj = cj.n;
Math::Vector pi = ci.p;
Math::Vector pj = cj.p;
Math::Vector ra = pi - a.x + a.centerOfMass;
Math::Vector rb = pi - b.x + b.centerOfMass;
Body a = readRigidBody(ci.a);
Body b = readRigidBody(ci.b);
Vector ni = ci.n;
Vector nj = cj.n;
Vector pi = ci.p;
Vector pj = cj.p;
Vector ra = pi - a.x + a.centerOfMass;
Vector rb = pi - b.x + b.centerOfMass;
Math::Vector forceOnA = Math::Vector(0);
Math::Vector torqueOnA = Math::Vector(0);
Vector forceOnA = Vector(0);
Vector torqueOnA = Vector(0);
if(cj.a == ci.a)
{
forceOnA = nj;
@@ -485,8 +490,8 @@ float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj, entt::regi
forceOnA = -nj;
torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
}
Math::Vector forceOnB = Math::Vector(0);
Math::Vector torqueOnB = Math::Vector(0);
Vector forceOnB = Vector(0);
Vector torqueOnB = Vector(0);
if(cj.a == ci.b)
{
forceOnB = nj;
@@ -498,52 +503,83 @@ float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj, entt::regi
torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
}
Math::Vector aLinear = forceOnA * a.inverseMass;
Math::Vector aAngular = glm::cross((a.iInv * torqueOnA), ra);
Vector aLinear = forceOnA * a.inverseMass;
Vector aAngular = glm::cross((a.iInv * torqueOnA), ra);
Math::Vector bLinear = forceOnB * b.inverseMass;
Math::Vector bAngular = glm::cross((b.iInv * torqueOnB), rb);
Vector bLinear = forceOnB * b.inverseMass;
Vector bAngular = glm::cross((b.iInv * torqueOnB), rb);
return glm::dot(ni, ((aLinear + aAngular) - (bLinear + bAngular)));
}
void PhysicsSystem::computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat, entt::registry& registry) const
void PhysicsSystem::computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat) const
{
for(size_t x = 0; x < contacts.size(); ++x)
{
for(size_t y = 0; y < contacts.size(); ++y)
{
amat[x][y] = computeAij(contacts[x], contacts[y], registry);
amat[x][y] = computeAij(contacts[x], contacts[y]);
}
}
}
void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>& bvec, entt::registry& registry) const
void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>& bvec) const
{
for(size_t y = 0; y < contacts.size(); ++y)
{
Contact c = contacts[y];
Body a = readRigidBody(c.a, registry);
Body b = readRigidBody(c.b, registry);
Math::Vector n = c.n;
Math::Vector ra = c.p - a.x + a.centerOfMass;
Math::Vector rb = c.p - b.x + b.centerOfMass;
Body a = readRigidBody(c.a);
Body b = readRigidBody(c.b);
Vector n = c.n;
Vector ra = c.p - a.x + a.centerOfMass;
Vector rb = c.p - b.x + b.centerOfMass;
Math::Vector fExtA = a.force;
Math::Vector fExtB = b.force;
Math::Vector tExtA = a.torque;
Math::Vector tExtB = b.torque;
Vector fExtA = a.force;
Vector fExtB = b.force;
Vector tExtA = a.torque;
Vector tExtB = b.torque;
Math::Vector aExtPart = fExtA * a.inverseMass + glm::cross(a.iInv * tExtA, ra);
Math::Vector bExtPart = fExtB * b.inverseMass + glm::cross(b.iInv * tExtB, rb);
Vector aExtPart = fExtA * a.inverseMass + glm::cross(a.iInv * tExtA, ra);
Vector bExtPart = fExtB * b.inverseMass + glm::cross(b.iInv * tExtB, rb);
Math::Vector aVelPart = glm::cross(a.omega, glm::cross(a.omega, ra)) + glm::cross(a.iInv * glm::cross(a.L, a.omega), ra);
Math::Vector bVelPart = glm::cross(b.omega, glm::cross(b.omega, rb)) + glm::cross(b.iInv * glm::cross(b.L, b.omega), rb);
Vector aVelPart = glm::cross(a.omega, glm::cross(a.omega, ra)) + glm::cross(a.iInv * glm::cross(a.L, a.omega), ra);
Vector bVelPart = glm::cross(b.omega, glm::cross(b.omega, rb)) + glm::cross(b.iInv * glm::cross(b.L, b.omega), rb);
float k1 = glm::dot(n, (aExtPart + aVelPart) - (bExtPart + bVelPart));
Math::Vector ndot = computeNdot(c, a, b);
Vector ndot = computeNdot(c, a, b);
float k2 = 2.0f * glm::dot(ndot, (a.ptVelocity(c.p) - b.ptVelocity(c.p)));
bvec[y] = k1 + k2;
}
}
}
void PhysicsSystem::solveQP(const Array<Array<float>>& CI, const Array<float>& ci0, Array<float>& sol) const
{
static std::mt19937_64 generator;
static std::uniform_real_distribution<float> dist(0.01f, 0.1f);
sol.resize(CI.size());
bool solved = false;
while(!solved)
{
for(size_t i = 0; i < sol.size(); ++i)
{
sol[i] = dist(generator);
}
solved = true;
for(size_t i = 0; i < sol.size(); ++i)
{
float res = 0;
for(size_t j = 0; j < sol.size(); ++j)
{
res += CI[i][j] * sol[j] + ci0[i];
}
if(res < 0)
{
solved = false;
std::cout << "failed to solve QP" << std::endl;
continue;
}
}
return;
}
}