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/BVH.cpp
+122 -32
View File
@@ -7,6 +7,10 @@ void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
overlaps.clear();
for(const auto& node : dynamicNodes)
{
if(!node.isLeaf)
{
continue;
}
traverseStaticTree(node.box, node.owner, staticRoot, overlaps);
traverseDynamicTree(node.box, node.owner, dynamicRoot, overlaps);
}
@@ -14,39 +18,48 @@ void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
void BVH::updateDynamicCollider(entt::entity entity, AABB aabb)
{
for(auto& aabbcenter : dynamicCollider)
for(auto& node : dynamicNodes)
{
if(aabbcenter.id == entity)
if(node.owner == entity)
{
if(!aabbcenter.bb.contains(aabb))
if(!node.box.contains(aabb))
{
// moved out of extended bounds
reinsertCollider(entity, aabb);
}
return;
}
}
// new collider
addDynamicCollider(entity, aabb);
}
void BVH::colliderCallback(entt::registry& registry, entt::entity entity)
{
Component::Collider& collider = registry.get<Component::Collider>(entity);
Component::Transform& transform = registry.get<Component::Transform>(entity);
if(collider.type == Component::ColliderType::STATIC)
{
addStaticCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
}
}
void BVH::visualize()
{
for(const auto& node : staticNodes)
{
node.box.visualize(gDebugVertices);
}
for(const auto& node : dynamicNodes)
{
node.box.visualize(gDebugVertices);
}
}
void BVH::traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
{
const Node& node = staticNodes[nodeIndex];
if(!aabb.intersects(node.box))
{
return;
}
if(node.isLeaf)
{
overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
return;
}
traverseStaticTree(aabb, source, node.left, overlaps);
traverseStaticTree(aabb, source, node.right, overlaps);
}
void BVH::traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
{
const Node& node = staticNodes[nodeIndex];
if(!aabb.intersects(node.box))
@@ -62,7 +75,36 @@ void BVH::traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeI
traverseStaticTree(aabb, source, node.right, overlaps);
}
void BVH::traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
{
if(nodeIndex == -1)
{
return;
}
const Node& node = dynamicNodes[nodeIndex];
if(!aabb.intersects(node.box))
{
return;
}
if(node.isLeaf && node.owner != source)
{
overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
return;
}
traverseDynamicTree(aabb, source, node.left, overlaps);
traverseDynamicTree(aabb, source, node.right, overlaps);
}
void BVH::reinsertCollider(entt::entity entity, AABB aabb)
{
removeCollider(entity);
addDynamicCollider(entity, aabb);
}
void BVH::removeCollider(entt::entity entity)
{
int32 nodeIndex = -1;
for (int32 i = 0; i < dynamicNodes.size(); i++)
@@ -75,11 +117,21 @@ void BVH::reinsertCollider(entt::entity entity, AABB aabb)
}
if(nodeIndex == -1)
{
return;
}
int32 parentIndex = dynamicNodes[nodeIndex].parentIndex;
if(parentIndex == -1)
{
// its the root node
dynamicRoot = -1;
freeNode(nodeIndex);
return;
}
const Node& parent = dynamicNodes[parentIndex];
int32 siblingIndex;
if(parent.left == nodeIndex)
{
siblingIndex = parent.right;
@@ -88,21 +140,34 @@ void BVH::reinsertCollider(entt::entity entity, AABB aabb)
{
siblingIndex = parent.left;
}
// the node to remove and their sibling share a parent
// now that the node is removed, the parent is also useless
// so the grandparent should point32 to the sibling directly instead of the parent
Node& grandParent = dynamicNodes[parent.parentIndex];
if(grandParent.left == parentIndex)
// so the grandparent should point to the sibling directly instead of the parent
if(parent.parentIndex != -1)
{
grandParent.left = siblingIndex;
Node& grandParent = dynamicNodes[parent.parentIndex];
if(grandParent.left == parentIndex)
{
grandParent.left = siblingIndex;
}
else
{
grandParent.right = siblingIndex;
}
dynamicNodes[siblingIndex].parentIndex = parent.parentIndex;
}
else
{
grandParent.right = siblingIndex;
// if the shared parent was the root, we need a new root, which is the remaining sibling
dynamicRoot = siblingIndex;
dynamicNodes[siblingIndex].parentIndex = -1;
}
freeNode(parentIndex);
freeNode(nodeIndex);
addDynamicCollider(entity, aabb);
freeNode(parentIndex);
}
void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
@@ -113,18 +178,15 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
.min = center + (aabb.min - center) * 1.1f,
.max = center + (aabb.max - center) * 1.1f,
};
dynamicCollider.add(AABBCenter {
.bb = aabb,
.center = center,
.id = entity
});
int32 leafIndex = allocateNode();
Node newNode = Node {
.box = aabb,
.isLeaf = true,
.isValid = true,
.owner = entity,
};
dynamicNodes[leafIndex] = newNode;
if (dynamicRoot == -1)
{
@@ -135,6 +197,7 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
int32 bestSibling;
float bestCost = std::numeric_limits<float>::max();
findSibling(newNode, dynamicRoot, bestCost, bestSibling);
int32 oldParent = dynamicNodes[bestSibling].parentIndex;
int32 newParent = allocateNode();
@@ -142,7 +205,9 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
.box = aabb.combine(dynamicNodes[bestSibling].box),
.parentIndex = oldParent,
.isLeaf = false,
.isValid = true,
};
if(oldParent != -1)
{
@@ -158,6 +223,7 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
dynamicNodes[newParent].right = leafIndex;
dynamicNodes[bestSibling].parentIndex = newParent;
dynamicNodes[leafIndex].parentIndex = newParent;
}
else
{
@@ -166,6 +232,7 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
dynamicNodes[bestSibling].parentIndex = newParent;
dynamicNodes[leafIndex].parentIndex = newParent;
dynamicRoot = newParent;
}
int32 index = dynamicNodes[leafIndex].parentIndex;
while(index != -1)
@@ -186,6 +253,7 @@ void BVH::addStaticCollider(entt::entity entity, AABB boundingBox)
.id = entity,
});
staticNodes.clear();
staticRoot = splitNode(staticCollider);
}
void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result)
@@ -218,6 +286,7 @@ float BVH::siblingCost(Node newNode, int32 siblingIndex)
{
AABB parentBox = dynamicNodes[parentIndex].box;
cost += parentBox.combine(newBox).surfaceArea() - parentBox.surfaceArea();
parentIndex = dynamicNodes[parentIndex].parentIndex;
}
return cost;
}
@@ -240,6 +309,7 @@ int32 BVH::splitNode(Array<AABBCenter> aabbs)
{
int32 leafIndex = static_cast<int32>(staticNodes.size());
Node& leaf = staticNodes.add();
leaf.isLeaf = true;
leaf.box = aabbs[0].bb;
leaf.left = -1;
leaf.right = -1;
@@ -296,5 +366,25 @@ int32 BVH::allocateNode()
void BVH::freeNode(int32 nodeIndex)
{
dynamicNodes[nodeIndex].isValid = false;
dynamicNodes[nodeIndex].parentIndex = -1;
}
void BVH::validateBVH() const
{
if(dynamicRoot != -1)
{
assert(dynamicNodes[dynamicRoot].parentIndex == -1);
}
for(int32 i = 0; i < dynamicNodes.size(); ++i)
{
int32 nodeIdx = i;
size_t counter = dynamicNodes.size();
if(!dynamicNodes[nodeIdx].isValid)
continue;
while(dynamicNodes[nodeIdx].parentIndex != -1)
{
nodeIdx = dynamicNodes[nodeIdx].parentIndex;
assert(counter-- > 0 && dynamicNodes[nodeIdx].isValid);
}
}
}
src/Engine/Physics/BVH.h
+4 -2
View File
@@ -12,11 +12,13 @@ class BVH
public:
void findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps);
void updateDynamicCollider(entt::entity entity, AABB aabb);
void colliderCallback(entt::registry& registry, entt::entity entity);
void visualize();
private:
struct AABBCenter
{
AABB bb;
Math::Vector center;
Vector center;
entt::entity id;
};
struct Node
@@ -41,9 +43,9 @@ private:
int32 splitNode(Array<AABBCenter> aabbs);
int32 allocateNode();
void freeNode(int32 nodeIndex);
void validateBVH() const;
Array<Node> dynamicNodes;
Array<Node> staticNodes;
Array<AABBCenter> dynamicCollider;
Array<AABBCenter> staticCollider;
int32 staticRoot = -1;
int32 dynamicRoot = -1;
src/Engine/Physics/CollisionSystem.cpp
+9 -7
View File
@@ -3,9 +3,10 @@
using namespace Seele;
using namespace Seele::Component;
CollisionSystem::CollisionSystem()
CollisionSystem::CollisionSystem(entt::registry& registry)
: registry(registry)
{
registry.on_construct<Component::Collider>().connect<&BVH::colliderCallback>(bvh);
}
CollisionSystem::~CollisionSystem()
@@ -13,22 +14,24 @@ CollisionSystem::~CollisionSystem()
}
void CollisionSystem::detectCollisions(const entt::registry& registry, Array<Collision>& collisions)
void CollisionSystem::detectCollisions(Array<Collision>& collisions)
{
collisions.clear();
auto view = registry.view<Collider, Transform>();
for(auto && [entity, collider, transform] : view.each())
{
if(collider.type == ColliderType::DYNAMIC && transform.isDirty())
if(collider.type == ColliderType::DYNAMIC)
{
bvh.updateDynamicCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
}
collider.physicsMesh.transform(transform).visualize();
}
bvh.visualize();
Array<Pair<entt::entity, entt::entity>> overlaps;
bvh.findOverlaps(overlaps);
for(auto pair : overlaps)
{
if(checkCollision(registry, pair))
if(checkCollision(pair))
{
collisions.add(Collision {
.a = pair.key,
@@ -38,7 +41,7 @@ void CollisionSystem::detectCollisions(const entt::registry& registry, Array<Col
}
}
bool CollisionSystem::checkCollision(const entt::registry& registry, Pair<entt::entity, entt::entity> pair)
bool CollisionSystem::checkCollision(Pair<entt::entity, entt::entity> pair)
{
const auto&[collider1, transform1] = registry.get<Collider, Transform>(pair.key);
const auto&[collider2, transform2] = registry.get<Collider, Transform>(pair.value);
@@ -64,7 +67,6 @@ void CollisionSystem::updateWitness(Witness& result, const glm::vec3& point, con
result.p = point;
}
bool CollisionSystem::createWitness(Witness& result, const ShapeBase& source, const ShapeBase& other)
{
for (size_t i = 0; i < source.indices.size(); i += 3)
src/Engine/Physics/CollisionSystem.h
+7 -6
View File
@@ -15,14 +15,14 @@ struct Collision
class CollisionSystem
{
public:
CollisionSystem();
CollisionSystem(entt::registry& registry);
virtual ~CollisionSystem();
void detectCollisions(const entt::registry& registry, Array<Collision>& collisions);
void detectCollisions(Array<Collision>& collisions);
private:
struct Witness
{
Math::Vector p;
Math::Vector n;
Vector p;
Vector n;
// for finding p
uint32_t point1Index;
// and the face where the plane lies on
@@ -33,11 +33,12 @@ private:
entt::entity other;
};
BVH bvh;
entt::registry& registry;
Map<Pair<entt::entity, entt::entity>, Witness> cachedWitness;
bool checkCollision(const entt::registry& registry, Pair<entt::entity, entt::entity> pair);
bool checkCollision(Pair<entt::entity, entt::entity> pair);
void updateWitness(Witness& witness, const Math::Vector& point, const Math::Vector& v1, const Math::Vector& v2);
void updateWitness(Witness& witness, const Vector& point, const Vector& v1, const Vector& v2);
// returns true if a collision was found
bool createWitness(Witness& witness, const Component::ShapeBase& source, const Component::ShapeBase& other);
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;
}
}
src/Engine/Physics/PhysicsSystem.h
+43 -40
View File
@@ -11,35 +11,35 @@ namespace Seele
class PhysicsSystem
{
public:
PhysicsSystem();
PhysicsSystem(entt::registry& registry);
~PhysicsSystem();
void update(entt::registry& registry, float deltaTime);
void update(float deltaTime);
private:
struct Body
{
entt::entity id;
float inverseMass;
Math::Vector centerOfMass;
Math::Matrix3 iBody, iBodyInv;
Math::Vector scale;
Vector centerOfMass;
Matrix3 iBody, iBodyInv;
Vector scale;
Math::Vector x;
Math::Quaternion q;
Math::Vector P;
Math::Vector L;
Vector x;
Quaternion q;
Vector P;
Vector L;
Math::Matrix3 iInv;
Math::Matrix3 R;
Math::Vector v;
Math::Vector omega;
Math::Vector force;
Math::Vector torque;
Math::Matrix4 matrix;
Math::Vector ptVelocity(Math::Vector p) const {return v + glm::cross(omega, p - x + centerOfMass);}
Matrix3 iInv;
Matrix3 R;
Vector v;
Vector omega;
Vector force;
Vector torque;
Matrix4 matrix;
Vector ptVelocity(Vector p) const {return v + glm::cross(omega, p - x + centerOfMass);}
void updateMatrix() {
Math::Matrix4 scaleMatrix = glm::scale(Math::Matrix4(1), scale);
Math::Matrix4 rotationMatrix = glm::mat4_cast(q);
Math::Matrix4 translationMatrix = glm::translate(Math::Matrix4(1), x);
Matrix4 scaleMatrix = glm::scale(Matrix4(1), scale);
Matrix4 rotationMatrix = glm::mat4_cast(q);
Matrix4 translationMatrix = glm::translate(Matrix4(1), x);
matrix = translationMatrix * rotationMatrix * scaleMatrix;
}
Body()
@@ -57,8 +57,8 @@ private:
, L(physics.angularMomentum)
, iInv(glm::mat3())
, R(glm::mat3())
, v(Math::Vector())
, omega(Math::Vector())
, v(Vector())
, omega(Vector())
, force(physics.force)
, torque(physics.torque)
{
@@ -76,14 +76,14 @@ private:
, scale(transform.getScale())
, x(transform.getPosition())
, q(transform.getRotation())
, P(Math::Vector(0))
, L(Math::Vector(0))
, P(Vector(0))
, L(Vector(0))
, iInv(glm::mat3())
, R(glm::mat3())
, v(Math::Vector())
, omega(Math::Vector())
, force(Math::Vector(0))
, torque(Math::Vector(0))
, v(Vector())
, omega(Vector())
, force(Vector(0))
, torque(Vector(0))
{
R = glm::mat3_cast(glm::normalize(q));
iInv = R * iBodyInv * glm::transpose(R);
@@ -99,9 +99,10 @@ private:
glm::vec3 eb;
bool vf;
};
static constexpr size_t FLOATS_PER_RB = sizeof(Body) / sizeof(float);
static constexpr size_t FLOATS_PER_RB = 13;
entt::registry& registry;
CollisionSystem collisionSystem;
bool pause = false;
void serializeRB(const Body& rb, float* y) const;
@@ -111,30 +112,32 @@ private:
void deserializeArray(Array<Body>& bodies, const Array<float>& x) const;
void readRigidBodies(Array<Body>& bodies, entt::registry& registry) const;
void readRigidBodies(Array<Body>& bodies) const;
void writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const;
void writeRigidBodies(const Array<Body>& bodies) const;
Body readRigidBody(entt::entity entity, entt::registry& regsitry) const;
Body readRigidBody(entt::entity entity) const;
void writeRigidBody(const Body& body, entt::registry& registry) const;
void writeRigidBody(const Body& body) const;
Array<Body> integratePhysics(const Array<Body>& bodies, const float t0, const float tdelta) const;
void rewindCollisions(const Array<Body>& t0Bodies, entt::registry& registry, const float t0, const float t1, size_t remainingDepth);
void rewindCollisions(const Array<Body>& t0Bodies, const float t0, const float t1, size_t remainingDepth);
void calculateContacts(entt::entity id1, const Component::ShapeBase& shape1, entt::entity id2, const Component::ShapeBase& shape2, Array<Contact>& contacts) const;
void resolveRestingContacts(const Array<Contact>& contacts, entt::registry& registry) const;
void resolveRestingContacts(const Array<Contact>& contacts) const;
void resolvePenetratingContact(const Contact& c, Body& a, Body& b) const;
Math::Vector computeNdot(const Contact& c, const Body& a, const Body& b) const;
Vector computeNdot(const Contact& c, const Body& a, const Body& b) const;
float computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) const;
float computeAij(const Contact& ci, const Contact& cj) const;
void computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat, entt::registry& registry) const;
void computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat) const;
void computeBVector(const Array<Contact>& contacts, Array<float>& avec, entt::registry& registry) const;
void computeBVector(const Array<Contact>& contacts, Array<float>& avec) const;
void solveQP(const Array<Array<float>>& A, const Array<float>& b, Array<float>& f) const;
};
} // namespace Seele