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

Implementing basic physics

Browse Source
This commit is contained in:
Dynamitos
2022-11-29 16:34:42 +01:00
parent 3d3e264867
commit 1e04da963d
39 changed files with 1999 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/Engine/Physics/BVH.cpp
+300
View File
@@ -0,0 +1,300 @@
#include "BVH.h"
using namespace Seele;
void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
{
overlaps.clear();
for(const auto& node : dynamicNodes)
{
traverseStaticTree(node.box, node.owner, staticRoot, overlaps);
traverseDynamicTree(node.box, node.owner, dynamicRoot, overlaps);
}
}
void BVH::updateDynamicCollider(entt::entity entity, AABB aabb)
{
for(auto& aabbcenter : dynamicCollider)
{
if(aabbcenter.id == entity)
{
if(!aabbcenter.bb.contains(aabb))
{
// moved out of extended bounds
reinsertCollider(entity, aabb);
}
return;
}
}
// new collider
addDynamicCollider(entity, aabb);
}
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))
{
return;
}
if(node.isLeaf && node.owner != source)
{
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::reinsertCollider(entt::entity entity, AABB aabb)
{
int32 nodeIndex = -1;
for (int32 i = 0; i < dynamicNodes.size(); i++)
{
if(dynamicNodes[i].isLeaf && dynamicNodes[i].owner == entity)
{
nodeIndex = i;
break;
}
}
if(nodeIndex == -1)
{
return;
}
int32 parentIndex = dynamicNodes[nodeIndex].parentIndex;
const Node& parent = dynamicNodes[parentIndex];
int32 siblingIndex;
if(parent.left == nodeIndex)
{
siblingIndex = parent.right;
}
else
{
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)
{
grandParent.left = siblingIndex;
}
else
{
grandParent.right = siblingIndex;
}
freeNode(parentIndex);
freeNode(nodeIndex);
addDynamicCollider(entity, aabb);
}
void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
{
auto center = (aabb.max + aabb.min) / 2.0f;
// enlarge box slightly to buffer movement
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,
.owner = entity,
};
dynamicNodes[leafIndex] = newNode;
if (dynamicRoot == -1)
{
dynamicRoot = leafIndex;
return;
}
int32 bestSibling;
float bestCost = std::numeric_limits<float>::max();
findSibling(newNode, dynamicRoot, bestCost, bestSibling);
int32 oldParent = dynamicNodes[bestSibling].parentIndex;
int32 newParent = allocateNode();
dynamicNodes[newParent] = Node {
.box = aabb.combine(dynamicNodes[bestSibling].box),
.parentIndex = oldParent,
.isLeaf = false,
};
if(oldParent != -1)
{
if(dynamicNodes[oldParent].left == bestSibling)
{
dynamicNodes[oldParent].left = newParent;
}
else
{
dynamicNodes[oldParent].right = newParent;
}
dynamicNodes[newParent].left = bestSibling;
dynamicNodes[newParent].right = leafIndex;
dynamicNodes[bestSibling].parentIndex = newParent;
dynamicNodes[leafIndex].parentIndex = newParent;
}
else
{
dynamicNodes[newParent].left = bestSibling;
dynamicNodes[newParent].right = leafIndex;
dynamicNodes[bestSibling].parentIndex = newParent;
dynamicNodes[leafIndex].parentIndex = newParent;
dynamicRoot = newParent;
}
int32 index = dynamicNodes[leafIndex].parentIndex;
while(index != -1)
{
int32 left = dynamicNodes[index].left;
int32 right = dynamicNodes[index].right;
dynamicNodes[index].box = dynamicNodes[left].box.combine(dynamicNodes[right].box);
index = dynamicNodes[index].parentIndex;
}
}
void BVH::addStaticCollider(entt::entity entity, AABB boundingBox)
{
staticCollider.add(AABBCenter {
.bb = boundingBox,
.center = (boundingBox.min + boundingBox.max) / 2.0f,
.id = entity,
});
staticNodes.clear();
}
void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result)
{
if(nodeIndex == -1)
{
return;
}
float lowerBound = lowerBoundCost(newNode, nodeIndex);
if(lowerBound < bestCost)
{
float cost = siblingCost(newNode, nodeIndex);
if(cost < bestCost)
{
bestCost = cost;
result = nodeIndex;
}
findSibling(newNode, dynamicNodes[nodeIndex].left, bestCost, result);
findSibling(newNode, dynamicNodes[nodeIndex].right, bestCost, result);
}
}
float BVH::siblingCost(Node newNode, int32 siblingIndex)
{
const Node& sibling = dynamicNodes[siblingIndex];
AABB newBox = sibling.box.combine(newNode.box);
float cost = newBox.surfaceArea();
int32 parentIndex = sibling.parentIndex;
while(parentIndex != -1)
{
AABB parentBox = dynamicNodes[parentIndex].box;
cost += parentBox.combine(newBox).surfaceArea() - parentBox.surfaceArea();
}
return cost;
}
float BVH::lowerBoundCost(Node newNode, int32 branchIndex)
{
float cost = newNode.box.surfaceArea();
while(branchIndex != -1)
{
AABB box = dynamicNodes[branchIndex].box;
cost += box.combine(newNode.box).surfaceArea() - box.surfaceArea();
branchIndex = dynamicNodes[branchIndex].parentIndex;
}
return cost;
}
int32 BVH::splitNode(Array<AABBCenter> aabbs)
{
if(aabbs.size() == 1)
{
int32 leafIndex = static_cast<int32>(staticNodes.size());
Node& leaf = staticNodes.add();
leaf.box = aabbs[0].bb;
leaf.left = -1;
leaf.right = -1;
leaf.owner = aabbs[0].id;
return leafIndex;
}
AABB rootBox;
for(size_t i = 0; i < aabbs.size(); ++i)
{
rootBox.adjust(aabbs[i].bb.min);
rootBox.adjust(aabbs[i].bb.max);
}
float xlen = rootBox.max.x - rootBox.min.x;
float ylen = rootBox.max.y - rootBox.min.y;
float zlen = rootBox.max.z - rootBox.min.z;
int32 longestAxis;
if(xlen >= ylen && xlen >= zlen)
{
longestAxis = 0;
}
if(ylen >= xlen && ylen >= zlen)
{
longestAxis = 1;
}
if(zlen >= xlen && zlen >= ylen)
{
longestAxis = 2;
}
std::sort(aabbs.begin(), aabbs.end(), [longestAxis](AABBCenter lhs, AABBCenter rhs){ return lhs.center[longestAxis] < rhs.center[longestAxis];});
Array<AABBCenter> left((aabbs.size()+1)/2);
Array<AABBCenter> right(aabbs.size()/2);
std::copy(aabbs.begin(), aabbs.begin()+left.size(), left.begin());
std::copy(aabbs.begin()+left.size(), aabbs.end(), right.begin());
int32 rootIndex = static_cast<int32>(staticNodes.size());
Node& rootNode = staticNodes.add();
rootNode.box = rootBox;
rootNode.left = splitNode(std::move(left));
rootNode.right = splitNode(std::move(right));
return rootIndex;
}
int32 BVH::allocateNode()
{
for (int32 i = 0; i < dynamicNodes.size(); i++)
{
if(!dynamicNodes[i].isValid)
{
return i;
}
}
int32 newLeaf = static_cast<int32>(dynamicNodes.size());
dynamicNodes.add();
return newLeaf;
}
void BVH::freeNode(int32 nodeIndex)
{
dynamicNodes[nodeIndex].isValid = false;
}
src/Engine/Physics/BVH.h
+51
View File
@@ -0,0 +1,51 @@
#pragma once
#include <entt/entt.hpp>
#include "Containers/Array.h"
#include "Component/AABB.h"
#include "Component/Collider.h"
#include "Containers/Pair.h"
namespace Seele
{
class BVH
{
public:
void findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps);
void updateDynamicCollider(entt::entity entity, AABB aabb);
private:
struct AABBCenter
{
AABB bb;
Math::Vector center;
entt::entity id;
};
struct Node
{
AABB box;
int32 parentIndex = -1;
int32 left = -1;
int32 right = -1;
bool isLeaf;
bool isValid = true;
entt::entity owner;
};
void traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps);
void traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps);
void reinsertCollider(entt::entity, AABB aabb);
void removeCollider(entt::entity entity);
void addDynamicCollider(entt::entity entity, AABB aabb);
void addStaticCollider(entt::entity entity, AABB aabb);
void findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result);
float siblingCost(Node newNode, int32 siblingIndex);
float lowerBoundCost(Node newNode, int32 branchIndex);
int32 splitNode(Array<AABBCenter> aabbs);
int32 allocateNode();
void freeNode(int32 nodeIndex);
Array<Node> dynamicNodes;
Array<Node> staticNodes;
Array<AABBCenter> dynamicCollider;
Array<AABBCenter> staticCollider;
int32 staticRoot = -1;
int32 dynamicRoot = -1;
};
} // namespace Seele
src/Engine/Physics/CMakeLists.txt
+15
View File
@@ -0,0 +1,15 @@
target_sources(Engine
PRIVATE
BVH.h
BVH.cpp
CollisionSystem.h
CollisionSystem.cpp
PhysicsSystem.h
PhysicsSystem.cpp)
target_sources(Engine
PUBLIC FILE_SET HEADERS
FILES
BVH.h
CollisionSystem.h
PhysicsSystem.h)
src/Engine/Physics/CollisionSystem.cpp
+158
View File
@@ -0,0 +1,158 @@
#include "CollisionSystem.h"
using namespace Seele;
using namespace Seele::Component;
CollisionSystem::CollisionSystem()
{
}
CollisionSystem::~CollisionSystem()
{
}
void CollisionSystem::detectCollisions(const entt::registry& registry, 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())
{
bvh.updateDynamicCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
}
}
Array<Pair<entt::entity, entt::entity>> overlaps;
bvh.findOverlaps(overlaps);
for(auto pair : overlaps)
{
if(checkCollision(registry, pair))
{
collisions.add(Collision {
.a = pair.key,
.b = pair.value,
});
}
}
}
bool CollisionSystem::checkCollision(const entt::registry& registry, 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);
ShapeBase shape1 = collider1.physicsMesh.transform(transform1);
ShapeBase shape2 = collider2.physicsMesh.transform(transform2);
Witness witness;
if(cachedWitness.exists(pair))
{
witness = cachedWitness[pair];
}
if(witnessValid(witness, shape1, shape2))
{
return false;
}
return createWitness(witness, shape1, shape2);
}
void CollisionSystem::updateWitness(Witness& result, const glm::vec3& point, const glm::vec3& v1, const glm::vec3& v2)
{
const glm::vec3 faceNormal = glm::normalize(glm::cross(v1, v2));
result.n = faceNormal;
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)
{
const glm::vec3 point1 = source.vertices[source.indices[i + 0]];
const glm::vec3 point2 = source.vertices[source.indices[i + 1]];
const glm::vec3 point3 = source.vertices[source.indices[i + 2]];
const glm::vec3 v1 = point2 - point1;
const glm::vec3 v2 = point3 - point1;
updateWitness(result, point1, v1, v2);
result.point1Index = source.indices[i + 0];
result.point2Index = source.indices[i + 1];
result.point3Index = source.indices[i + 2];
result.point4Index = UINT32_MAX;
bool valid = witnessValid(result, source, other);
// if not, it is a valid separating plane, so no collision
if (valid)
{
return false;
}
}
for (size_t i = 0; i < source.indices.size(); i += 3)
{
auto findEdgePlane = [=, &result](uint32_t point1Index, uint32_t point2Index)
{
const glm::vec3 point1 = source.vertices[point1Index];
const glm::vec3 point2 = source.vertices[point2Index];
result.point1Index = point1Index;
result.point2Index = point2Index;
const glm::vec3 d1 = point2 - point1;
for (size_t j = 0; j < other.indices.size(); j += 3)
{
for (const auto& [point3Index, point4Index] : { std::pair(j+1, j), std::pair(j+2, j+1), std::pair(j, j+2) })
{
result.point3Index = other.indices[point3Index];
result.point4Index = other.indices[point4Index];
const glm::vec3 point3 = other.vertices[result.point3Index];
const glm::vec3 point4 = other.vertices[result.point4Index];
const glm::vec3 d2 = point3 - point4;
updateWitness(result, point1, d2, d1);
if (witnessValid(result, source, other))
{
return true;
}
}
}
return false;
};
if(findEdgePlane(source.indices[i], source.indices[i+1]))
{
return false;
}
if(findEdgePlane(source.indices[i+1], source.indices[i+2]))
{
return false;
}
if(findEdgePlane(source.indices[i+2], source.indices[i]))
{
return false;
}
}
// no separating plane was found, collision
return true;
}
bool CollisionSystem::witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2)
{
const float e = 0.0001f;
for (size_t i = 0; i < shape1.vertices.size(); i++)
{
if(glm::dot(witness.n, shape1.vertices[i] - witness.p) > e)
{
// something intersecting the separating plane
// it is not valid anymore
return false;
}
}
for (size_t i = 0; i < shape2.vertices.size(); i++)
{
if(glm::dot(witness.n, shape2.vertices[i] - witness.p) < -e)
{
// something intersecting the separating plane
// it is not valid anymore
return false;
}
}
return true;
}
src/Engine/Physics/CollisionSystem.h
+48
View File
@@ -0,0 +1,48 @@
#pragma once
#include <entt/entt.hpp>
#include "BVH.h"
#include "Containers/Map.h"
#include "Containers/Array.h"
#include "Component/Collider.h"
#include "Component/Transform.h"
namespace Seele
{
struct Collision
{
entt::entity a, b;
};
class CollisionSystem
{
public:
CollisionSystem();
virtual ~CollisionSystem();
void detectCollisions(const entt::registry& registry, Array<Collision>& collisions);
private:
struct Witness
{
Math::Vector p;
Math::Vector n;
// for finding p
uint32_t point1Index;
// and the face where the plane lies on
uint32_t point2Index;
uint32_t point3Index;
uint32_t point4Index = UINT32_MAX;
entt::entity source;
entt::entity other;
};
BVH bvh;
Map<Pair<entt::entity, entt::entity>, Witness> cachedWitness;
bool checkCollision(const entt::registry& registry, Pair<entt::entity, entt::entity> pair);
void updateWitness(Witness& witness, const Math::Vector& point, const Math::Vector& v1, const Math::Vector& v2);
// returns true if a collision was found
bool createWitness(Witness& witness, const Component::ShapeBase& source, const Component::ShapeBase& other);
// returns true if a collision was found
bool witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2);
};
} // namespace Seele
src/Engine/Physics/PhysicsSystem.cpp
+549
View File
@@ -0,0 +1,549 @@
#include "PhysicsSystem.h"
#include <boost/numeric/odeint.hpp>
using namespace Seele;
using namespace Seele::Component;
PhysicsSystem::PhysicsSystem()
{
}
PhysicsSystem::~PhysicsSystem()
{
}
void PhysicsSystem::update(entt::registry& registry, float deltaTime)
{
Array<Body> initialBodies;
readRigidBodies(initialBodies, registry);
Array<Body> bodies = integratePhysics(initialBodies, 0, deltaTime);
writeRigidBodies(bodies, registry);
Array<Collision> collisions;
collisionSystem.detectCollisions(registry, 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);
}
}
}
void PhysicsSystem::serializeRB(const Body& rb, float* y) const
{
*y++ = rb.x.x;
*y++ = rb.x.y;
*y++ = rb.x.z;
*y++ = rb.q.w;
*y++ = rb.q.x;
*y++ = rb.q.y;
*y++ = rb.q.z;
*y++ = rb.P.x;
*y++ = rb.P.y;
*y++ = rb.P.z;
*y++ = rb.L.x;
*y++ = rb.L.y;
*y++ = rb.L.z;
}
void PhysicsSystem::deserializeRB(Body& rb, const float* y) const
{
rb.x.x = *y++;
rb.x.y = *y++;
rb.x.z = *y++;
rb.q.w = *y++;
rb.q.x = *y++;
rb.q.y = *y++;
rb.q.z = *y++;
rb.P.x = *y++;
rb.P.y = *y++;
rb.P.z = *y++;
rb.L.x = *y++;
rb.L.y = *y++;
rb.L.z = *y++;
rb.v = rb.P * rb.inverseMass;
rb.R = glm::mat3_cast(glm::normalize(rb.q));
rb.iInv = rb.R * rb.iBodyInv * glm::transpose(rb.R);
rb.omega = rb.iInv * rb.L;
}
void PhysicsSystem::serializeArray(const Array<Body>& bodies, Array<float>& x) const
{
x.resize(bodies.size() * FLOATS_PER_RB);
for(uint32_t i = 0; i < bodies.size(); ++i)
{
serializeRB(bodies[i], x.data()+(i*FLOATS_PER_RB));
}
}
void PhysicsSystem::deserializeArray(Array<Body>& bodies, const Array<float>& x) const
{
bodies.resize(x.size() / FLOATS_PER_RB);
for(uint32_t i = 0; i < bodies.size(); ++i)
{
deserializeRB(bodies[i], x.data()+(i*FLOATS_PER_RB));
}
}
void PhysicsSystem::readRigidBodies(Array<Body>& bodies, entt::registry& registry) const
{
auto view = registry.view<RigidBody, Transform, Collider>();
bodies.clear();
bodies.reserve(view.size_hint());
view
.each([&bodies](entt::entity id, RigidBody& rb, Transform& transform, Collider& collider)
{
Body& rigidBody = bodies.add(Body(id, rb, collider, transform));
rigidBody.updateMatrix();
});
registry.view<Collider, Transform>(entt::exclude<RigidBody>)
.each([&bodies](entt::entity id, Collider& collider, Transform& transform)
{
Body& rigidBody = bodies.add(Body(id, collider, transform));
rigidBody.updateMatrix();
});
}
void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const
{
for(auto& body : bodies)
{
if(registry.all_of<RigidBody, Transform>(body.id))
{
auto [physics, transform] = registry.get<RigidBody, Transform>(body.id);
transform.setPosition(body.x);
transform.setRotation(body.q);
physics.linearMomentum = body.P;
physics.angularMomentum = body.L;
physics.force = body.force;
physics.torque = body.torque;
}
}
}
PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity, entt::registry& registry) const
{
Body rigidBody;
if(registry.all_of<RigidBody, Collider, Transform>(entity))
{
const auto& [physics, collider, transform] = registry.get<RigidBody, Collider, Transform>(entity);
rigidBody = Body(entity, physics, collider, transform);
}
else
{
const auto& [collider, transform] = registry.get<Collider, Transform>(entity);
rigidBody = Body(entity, collider, transform);
}
rigidBody.updateMatrix();
return rigidBody;
}
void PhysicsSystem::writeRigidBody(const Body& body, entt::registry& registry) const
{
if(registry.all_of<RigidBody, Transform>(body.id))
{
const auto& [physics, transform] = registry.get<RigidBody, Transform>(body.id);
transform.setPosition(body.x);
transform.setRotation(body.q);
physics.linearMomentum = body.P;
physics.angularMomentum = body.L;
physics.force = body.force;
physics.torque = body.torque;
}
else
{
auto& transform = registry.get<Transform>(body.id);
assert(transform.getPosition() == body.x);
assert(transform.getRotation() == body.q);
}
}
Array<PhysicsSystem::Body> PhysicsSystem::integratePhysics(const Array<Body>& bodies, const float t0, const float tdelta) const
{
Array<Body> result;
Array<float> buffer;
result.resize(bodies.size());
buffer.resize(bodies.size() * FLOATS_PER_RB);
std::memcpy(result.data(), bodies.data(), result.size() * sizeof(Body));
serializeArray(bodies, buffer);
auto dxdt = [this, &result](const Array<float>& x, Array<float>& x2, const float)
{
deserializeArray(result, x);
float* xdot = x2.data();
for (size_t i = 0; i < result.size(); i++)
{
// x(t)' = v(t)
*xdot++ = result[i].v.x;
*xdot++ = result[i].v.y;
*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);
*xdot++ = qdot.w;
*xdot++ = qdot.x;
*xdot++ = qdot.y;
*xdot++ = qdot.z;
// P(t)' = F(t)
*xdot++ = result[i].force.x;
*xdot++ = result[i].force.y;
*xdot++ = result[i].force.z;
// L(t)' = tau(t)
*xdot++ = result[i].torque.x;
*xdot++ = result[i].torque.y;
*xdot++ = result[i].torque.z;
}
};
boost::numeric::odeint::stepper_rk4<Array<float>, float> stepper;
boost::numeric::odeint::integrate_const(stepper, dxdt, buffer, t0, tdelta, tdelta);
deserializeArray(result, buffer);
return result;
}
void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, entt::registry& registry, const float t0, const float t1, size_t remainingRecursionDepth)
{
if(remainingRecursionDepth == 0)
{
//std::cout << "reached max recursion depth" << std::endl;
}
// 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);
//std::cout << "detected " << collisions.size() << " at " << tc << std::endl;
// now we check if there has been a contact at tc
Array<Contact> contacts;
// collision occured at [tc; t1]
for (auto &&collision : collisions)
{
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);
}
// 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);
float vrel = glm::dot(contact.n, paDot - pbDot);
if(vrel > 0.001f)
{
continue;
}
if(vrel > -0.001f)
{
restingContacts.add(contact);
continue;
}
resolvePenetratingContact(contact, a, b);
a.updateMatrix();
b.updateMatrix();
writeRigidBody(a, registry);
writeRigidBody(b, registry);
}
resolveRestingContacts(restingContacts, registry);
}
void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1, entt::entity id2, const ShapeBase& shape2, Array<Contact>& contacts) const
{
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){
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];
float dot = glm::dot(faceNormal, worldPos - point1);
if(dot < 0.2f)
{
Math::Vector pa = point1 - worldPos;
Math::Vector pb = point2 - worldPos;
Math::Vector pc = point3 - worldPos;
float a1 = area(pa, pb);
float a2 = area(pb, pc);
float a3 = area(pc, pa);
if(std::abs(a1 + a2 + a3 - faceArea) > 0.2f)
{
continue;
}
Contact c = {
.a = id2,
.b = id1,
.p = worldPos,
.n = faceNormal,
.vf = true
};
contacts.add(c);
}
}
// edge - edge contacts
auto lineLineContact = [=, &contacts](Math::Vector p1, Math::Vector p2, Math::Vector p3, Math::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;
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;
float ty = (c.x * ab.z - a.x * ab.z - c.z * ab.x + a.z * ab.x) / ty_den;
float tz_den = cd.y * ab.x - cd.x * ab.y;
float tz = (c.x * ab.y - a.x * ab.y - c.y * ab.x + a.y * ab.x) / tz_den;
if (std::abs(tx - ty) < 0.1f
&& std::abs(ty - tz) < 0.1f
&& std::abs(tz - tx) < 0.1f
&& 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));
Contact contact = {
.a = id1,
.b = id2,
.p = p,
.n = n,
.ea = ea,
.eb = eb,
.vf = false
};
}
};
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]];
lineLineContact(point1, point2, point4, point5);
lineLineContact(point1, point2, point5, point6);
lineLineContact(point1, point2, point6, point4);
lineLineContact(point2, point3, point4, point5);
lineLineContact(point2, point3, point5, point6);
lineLineContact(point2, point3, point6, point4);
lineLineContact(point3, point1, point4, point5);
lineLineContact(point3, point1, point5, point6);
lineLineContact(point3, point1, point6, point4);
}
}
}
void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts, entt::registry& registry) const
{
Array<Array<float>> amat(contacts.size(), Array<float>(contacts.size()));
Array<float> bvec(contacts.size());
computeAMatrix(contacts, amat, registry);
computeBVector(contacts, bvec, registry);
/*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);
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);
}*/
}
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);
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;
float numerator = -(1 + 0.5f) * vrel;
float term1 = a.inverseMass;
float term2 = b.inverseMass;
float term3 = glm::dot(n, glm::cross(a.iInv * glm::cross(ra, n), ra));
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;
a.P += force;
b.P -= force;
a.L += glm::cross(ra, force);
b.L -= glm::cross(rb, force);
a.v = a.P * a.inverseMass;
b.v = b.P * b.inverseMass;
a.omega = a.iInv * a.L;
b.omega = b.iInv * b.L;
}
Math::Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const
{
if(c.vf)
{
return glm::cross(b.omega, c.n);
}
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);
float l = glm::length(n1);
n1 = glm::normalize(n1);
return (z - glm::cross(glm::cross(z, n1), n1)) / l;
}
}
float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) 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;
Math::Vector forceOnA = Math::Vector(0);
Math::Vector torqueOnA = Math::Vector(0);
if(cj.a == ci.a)
{
forceOnA = nj;
torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
}
else if(cj.b == ci.a)
{
forceOnA = -nj;
torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
}
Math::Vector forceOnB = Math::Vector(0);
Math::Vector torqueOnB = Math::Vector(0);
if(cj.a == ci.b)
{
forceOnB = nj;
torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
}
else if(cj.b == ci.b)
{
forceOnB = -nj;
torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
}
Math::Vector aLinear = forceOnA * a.inverseMass;
Math::Vector aAngular = glm::cross((a.iInv * torqueOnA), ra);
Math::Vector bLinear = forceOnB * b.inverseMass;
Math::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
{
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);
}
}
}
void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>& bvec, entt::registry& registry) 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;
Math::Vector fExtA = a.force;
Math::Vector fExtB = b.force;
Math::Vector tExtA = a.torque;
Math::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);
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);
float k1 = glm::dot(n, (aExtPart + aVelPart) - (bExtPart + bVelPart));
Math::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;
}
}
src/Engine/Physics/PhysicsSystem.h
+140
View File
@@ -0,0 +1,140 @@
#pragma once
#include <entt/entt.hpp>
#include "MinimalEngine.h"
#include "Component/Transform.h"
#include "Component/RigidBody.h"
#include "Component/Collider.h"
#include "CollisionSystem.h"
namespace Seele
{
class PhysicsSystem
{
public:
PhysicsSystem();
~PhysicsSystem();
void update(entt::registry& registry, float deltaTime);
private:
struct Body
{
entt::entity id;
float inverseMass;
Math::Vector centerOfMass;
Math::Matrix3 iBody, iBodyInv;
Math::Vector scale;
Math::Vector x;
Math::Quaternion q;
Math::Vector P;
Math::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);}
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);
matrix = translationMatrix * rotationMatrix * scaleMatrix;
}
Body()
{}
Body(entt::entity id, const Component::RigidBody& physics, const Component::Collider& collider, const Component::Transform& transform)
: id(id)
, inverseMass(1 / (physics.mass * glm::length(transform.getScale())))
, centerOfMass(collider.physicsMesh.centerOfMass)
, iBody(collider.physicsMesh.bodyInertia)
, iBodyInv(glm::inverse(collider.physicsMesh.bodyInertia))
, scale(transform.getScale())
, x(transform.getPosition())
, q(transform.getRotation())
, P(physics.linearMomentum)
, L(physics.angularMomentum)
, iInv(glm::mat3())
, R(glm::mat3())
, v(Math::Vector())
, omega(Math::Vector())
, force(physics.force)
, torque(physics.torque)
{
v = P * inverseMass;
R = glm::mat3_cast(glm::normalize(q));
iInv = R * iBodyInv * glm::transpose(R);
omega = iInv * L;
}
Body(entt::entity id, const Component::Collider& collider, const Component::Transform& transform)
: id(id)
, inverseMass(0)
, centerOfMass(collider.physicsMesh.centerOfMass)
, iBody(glm::mat3(0))
, iBodyInv(glm::mat3(0))
, scale(transform.getScale())
, x(transform.getPosition())
, q(transform.getRotation())
, P(Math::Vector(0))
, L(Math::Vector(0))
, iInv(glm::mat3())
, R(glm::mat3())
, v(Math::Vector())
, omega(Math::Vector())
, force(Math::Vector(0))
, torque(Math::Vector(0))
{
R = glm::mat3_cast(glm::normalize(q));
iInv = R * iBodyInv * glm::transpose(R);
omega = iInv * L;
}
};
struct Contact
{
entt::entity a, b;
glm::vec3 p;
glm::vec3 n;
glm::vec3 ea;
glm::vec3 eb;
bool vf;
};
static constexpr size_t FLOATS_PER_RB = sizeof(Body) / sizeof(float);
CollisionSystem collisionSystem;
void serializeRB(const Body& rb, float* y) const;
void deserializeRB(Body& rb, const float* y) const;
void serializeArray(const Array<Body>& bodies, Array<float>& x) const;
void deserializeArray(Array<Body>& bodies, const Array<float>& x) const;
void readRigidBodies(Array<Body>& bodies, entt::registry& registry) const;
void writeRigidBodies(const Array<Body>& bodies, entt::registry& registry) const;
Body readRigidBody(entt::entity entity, entt::registry& regsitry) const;
void writeRigidBody(const Body& body, entt::registry& registry) 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 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 resolvePenetratingContact(const Contact& c, Body& a, Body& b) const;
Math::Vector computeNdot(const Contact& c, const Body& a, const Body& b) const;
float computeAij(const Contact& ci, const Contact& cj, entt::registry& registry) const;
void computeAMatrix(const Array<Contact>& contacts, Array<Array<float>>& amat, entt::registry& registry) const;
void computeBVector(const Array<Contact>& contacts, Array<float>& avec, entt::registry& registry) const;
};
} // namespace Seele