Formatted EVERYTHING

src/Engine/Physics/BVH.cpp

@@ -2,13 +2,10 @@
 
 using namespace Seele;
 
-void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
-{
+void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps) {
     overlaps.clear();
-    for(const auto& node : dynamicNodes)
-    {
-        if(!node.isLeaf)
-        {
+    for (const auto& node : dynamicNodes) {
+        if (!node.isLeaf) {
             continue;
         }
         traverseStaticTree(node.box, node.owner, staticRoot, overlaps);
@@ -16,60 +13,46 @@ void BVH::findOverlaps(Array<Pair<entt::entity, entt::entity>>& overlaps)
     }
 }
 
-void BVH::updateDynamicCollider(entt::entity entity, AABB aabb)
-{
-    
-    for(auto& node : dynamicNodes)
-    {
-        if(node.owner == entity)
-        {
-            if(!node.box.contains(aabb))
-            {
+void BVH::updateDynamicCollider(entt::entity entity, AABB aabb) {
+
+    for (auto& node : dynamicNodes) {
+        if (node.owner == entity) {
+            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)
-{
+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)
-    {
+    if (collider.type == Component::ColliderType::STATIC) {
         addStaticCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
     }
 }
 
-void BVH::visualize()
-{
+void BVH::visualize() {
     Array<DebugVertex> verts;
-    for (const auto& node : staticNodes)
-    {
+    for (const auto& node : staticNodes) {
         node.box.visualize(verts);
     }
-    for (const auto& node : dynamicNodes)
-    {
+    for (const auto& node : dynamicNodes) {
         node.box.visualize(verts);
     }
     addDebugVertices(verts);
 }
 
-void BVH::traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIndex, Array<Pair<entt::entity, entt::entity>>& overlaps)
-{
+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))
-    {
+    if (!aabb.intersects(node.box)) {
         return;
     }
-    if(node.isLeaf && node.owner != source)
-    {
+    if (node.isLeaf && node.owner != source) {
         overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
         return;
     }
@@ -77,19 +60,15 @@ void BVH::traverseStaticTree(const AABB& aabb, entt::entity source, int32 nodeIn
     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)
-    {
+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))
-    {
+    if (!aabb.intersects(node.box)) {
         return;
     }
-    if(node.isLeaf && node.owner != source)
-    {
+    if (node.isLeaf && node.owner != source) {
         overlaps.add(Pair<entt::entity, entt::entity>(source, node.owner));
         return;
     }
@@ -97,147 +76,117 @@ void BVH::traverseDynamicTree(const AABB& aabb, entt::entity source, int32 nodeI
     traverseDynamicTree(aabb, source, node.right, overlaps);
 }
 
-void BVH::reinsertCollider(entt::entity entity, AABB aabb)
-{
-    
+void BVH::reinsertCollider(entt::entity entity, AABB aabb) {
+
     removeCollider(entity);
-    
+
     addDynamicCollider(entity, aabb);
-    
 }
 
-void BVH::removeCollider(entt::entity entity)
-{
+void BVH::removeCollider(entt::entity entity) {
     int32 nodeIndex = -1;
-    for (size_t i = 0; i < dynamicNodes.size(); i++)
-    {
-        if(dynamicNodes[i].isLeaf && dynamicNodes[i].owner == entity)
-        {
+    for (size_t i = 0; i < dynamicNodes.size(); i++) {
+        if (dynamicNodes[i].isLeaf && dynamicNodes[i].owner == entity) {
             nodeIndex = i;
             break;
         }
     }
-    if(nodeIndex == -1)
-    {
+    if (nodeIndex == -1) {
         return;
     }
     int32 parentIndex = dynamicNodes[nodeIndex].parentIndex;
-    if(parentIndex == -1)
-    {
+    if (parentIndex == -1) {
         // its the root node
         dynamicRoot = -1;
         freeNode(nodeIndex);
-        
+
         return;
     }
     const Node& parent = dynamicNodes[parentIndex];
     int32 siblingIndex;
-    
-    if(parent.left == nodeIndex)
-    {
+
+    if (parent.left == nodeIndex) {
         siblingIndex = parent.right;
-    }
-    else
-    {
+    } 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 point to the sibling directly instead of the parent
-    if(parent.parentIndex != -1)
-    {
+    if (parent.parentIndex != -1) {
         Node& grandParent = dynamicNodes[parent.parentIndex];
-        if(grandParent.left == parentIndex)
-        {
+        if (grandParent.left == parentIndex) {
             grandParent.left = siblingIndex;
-        }
-        else
-        {
+        } else {
             grandParent.right = siblingIndex;
         }
         dynamicNodes[siblingIndex].parentIndex = parent.parentIndex;
-    }
-    else
-    {
+    } else {
         // if the shared parent was the root, we need a new root, which is the remaining sibling
         dynamicRoot = siblingIndex;
         dynamicNodes[siblingIndex].parentIndex = -1;
     }
-    
+
     freeNode(nodeIndex);
-    
+
     freeNode(parentIndex);
-    
 }
 
-void BVH::addDynamicCollider(entt::entity entity, AABB 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 {
+    aabb = AABB{
         .min = center + (aabb.min - center) * 1.1f,
         .max = center + (aabb.max - center) * 1.1f,
     };
     int32 leafIndex = allocateNode();
-    Node newNode = Node {
+    Node newNode = Node{
         .box = aabb,
         .isLeaf = true,
         .isValid = true,
         .owner = entity,
     };
     dynamicNodes[leafIndex] = newNode;
-    
 
-    if (dynamicRoot == -1)
-    {
+    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 {
+    dynamicNodes[newParent] = Node{
         .box = aabb.combine(dynamicNodes[bestSibling].box),
         .parentIndex = oldParent,
         .isLeaf = false,
         .isValid = true,
     };
-    
 
-    if(oldParent != -1)
-    {
-        if(dynamicNodes[oldParent].left == bestSibling)
-        {
+    if (oldParent != -1) {
+        if (dynamicNodes[oldParent].left == bestSibling) {
             dynamicNodes[oldParent].left = newParent;
-        }
-        else
-        {
+        } else {
             dynamicNodes[oldParent].right = newParent;
         }
         dynamicNodes[newParent].left = bestSibling;
         dynamicNodes[newParent].right = leafIndex;
         dynamicNodes[bestSibling].parentIndex = newParent;
         dynamicNodes[leafIndex].parentIndex = newParent;
-        
-    }
-    else
-    {
+
+    } 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)
-    {
+    while (index != -1) {
         int32 left = dynamicNodes[index].left;
         int32 right = dynamicNodes[index].right;
 
@@ -246,9 +195,8 @@ void BVH::addDynamicCollider(entt::entity entity, AABB aabb)
     }
 }
 
-void BVH::addStaticCollider(entt::entity entity, AABB boundingBox)
-{
-    staticCollider.add(AABBCenter {
+void BVH::addStaticCollider(entt::entity entity, AABB boundingBox) {
+    staticCollider.add(AABBCenter{
         .bb = boundingBox,
         .center = (boundingBox.min + boundingBox.max) / 2.0f,
         .id = entity,
@@ -257,18 +205,14 @@ void BVH::addStaticCollider(entt::entity entity, AABB boundingBox)
     staticRoot = splitNode(staticCollider);
 }
 
-void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result)
-{
-    if(nodeIndex == -1)
-    {
+void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& result) {
+    if (nodeIndex == -1) {
         return;
     }
     float lowerBound = lowerBoundCost(newNode, nodeIndex);
-    if(lowerBound < bestCost)
-    {
+    if (lowerBound < bestCost) {
         float cost = siblingCost(newNode, nodeIndex);
-        if(cost < bestCost)
-        {
+        if (cost < bestCost) {
             bestCost = cost;
             result = nodeIndex;
         }
@@ -277,14 +221,12 @@ void BVH::findSibling(Node newNode, int32 nodeIndex, float& bestCost, int32& res
     }
 }
 
-float BVH::siblingCost(Node newNode, int32 siblingIndex)
-{
+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)
-    {
+    while (parentIndex != -1) {
         AABB parentBox = dynamicNodes[parentIndex].box;
         cost += parentBox.combine(newBox).surfaceArea() - parentBox.surfaceArea();
         parentIndex = dynamicNodes[parentIndex].parentIndex;
@@ -292,11 +234,9 @@ float BVH::siblingCost(Node newNode, int32 siblingIndex)
     return cost;
 }
 
-float BVH::lowerBoundCost(Node newNode, int32 branchIndex)
-{
+float BVH::lowerBoundCost(Node newNode, int32 branchIndex) {
     float cost = newNode.box.surfaceArea();
-    while(branchIndex != -1)
-    {
+    while (branchIndex != -1) {
         AABB box = dynamicNodes[branchIndex].box;
         cost += box.combine(newNode.box).surfaceArea() - box.surfaceArea();
         branchIndex = dynamicNodes[branchIndex].parentIndex;
@@ -304,10 +244,8 @@ float BVH::lowerBoundCost(Node newNode, int32 branchIndex)
     return cost;
 }
 
-int32 BVH::splitNode(Array<AABBCenter> aabbs)
-{
-    if(aabbs.size() == 1)
-    {
+int32 BVH::splitNode(Array<AABBCenter> aabbs) {
+    if (aabbs.size() == 1) {
         int32 leafIndex = static_cast<int32>(staticNodes.size());
         Node& leaf = staticNodes.add();
         leaf.isLeaf = true;
@@ -318,8 +256,7 @@ int32 BVH::splitNode(Array<AABBCenter> aabbs)
         return leafIndex;
     }
     AABB rootBox;
-    for(size_t i = 0; i < aabbs.size(); ++i)
-    {
+    for (size_t i = 0; i < aabbs.size(); ++i) {
         rootBox.adjust(aabbs[i].bb.min);
         rootBox.adjust(aabbs[i].bb.max);
     }
@@ -327,32 +264,25 @@ int32 BVH::splitNode(Array<AABBCenter> aabbs)
     float ylen = rootBox.max.y - rootBox.min.y;
     float zlen = rootBox.max.z - rootBox.min.z;
     int32 longestAxis;
-    if(xlen >= ylen && xlen >= zlen)
-    {
+    if (xlen >= ylen && xlen >= zlen) {
         longestAxis = 0;
     }
-    if(ylen >= xlen && ylen >= zlen)
-    {
+    if (ylen >= xlen && ylen >= zlen) {
         longestAxis = 1;
     }
-    if(zlen >= xlen && zlen >= ylen)
-    {
+    if (zlen >= xlen && zlen >= ylen) {
         longestAxis = 2;
     }
-    struct
-    {
-        bool operator()(const AABBCenter& lhs, const AABBCenter& rhs)
-        { 
-            return lhs.center[longestAxis] < rhs.center[longestAxis];
-        }
+    struct {
+        bool operator()(const AABBCenter& lhs, const AABBCenter& rhs) { return lhs.center[longestAxis] < rhs.center[longestAxis]; }
         int32 longestAxis;
     } compare = {longestAxis};
-    
+
     std::sort(aabbs.begin(), aabbs.end(), compare);
-    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());
+    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;
@@ -360,12 +290,9 @@ int32 BVH::splitNode(Array<AABBCenter> aabbs)
     rootNode.right = splitNode(std::move(right));
     return rootIndex;
 }
-int32 BVH::allocateNode()
-{
-    for (size_t i = 0; i < dynamicNodes.size(); i++)
-    {
-        if(!dynamicNodes[i].isValid)
-        {
+int32 BVH::allocateNode() {
+    for (size_t i = 0; i < dynamicNodes.size(); i++) {
+        if (!dynamicNodes[i].isValid) {
             return i;
         }
     }
@@ -373,26 +300,21 @@ int32 BVH::allocateNode()
     dynamicNodes.add();
     return newLeaf;
 }
-void BVH::freeNode(int32 nodeIndex)
-{
+void BVH::freeNode(int32 nodeIndex) {
     dynamicNodes[nodeIndex].isValid = false;
     dynamicNodes[nodeIndex].parentIndex = -1;
 }
 
-void BVH::validateBVH() const
-{
-    if(dynamicRoot != -1)
-    {
+void BVH::validateBVH() const {
+    if (dynamicRoot != -1) {
         assert(dynamicNodes[dynamicRoot].parentIndex == -1);
     }
-    for(size_t i = 0; i < dynamicNodes.size(); ++i)
-    {
+    for (size_t i = 0; i < dynamicNodes.size(); ++i) {
         int32 nodeIdx = i;
         size_t counter = dynamicNodes.size();
-        if(!dynamicNodes[nodeIdx].isValid)
+        if (!dynamicNodes[nodeIdx].isValid)
             continue;
-        while(dynamicNodes[nodeIdx].parentIndex != -1)
-        {
+        while (dynamicNodes[nodeIdx].parentIndex != -1) {
             nodeIdx = dynamicNodes[nodeIdx].parentIndex;
             assert(counter-- > 0 && dynamicNodes[nodeIdx].isValid);
         }

src/Engine/Physics/BVH.h

@@ -1,28 +1,26 @@
 #pragma once
-#include <entt/entt.hpp>
-#include "Containers/Array.h"
-#include "Math/AABB.h"
 #include "Component/Collider.h"
+#include "Containers/Array.h"
 #include "Containers/Pair.h"
+#include "Math/AABB.h"
+#include <entt/entt.hpp>
 
-namespace Seele
-{
-class BVH
-{
-public:
+
+namespace Seele {
+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
-    {
+
+  private:
+    struct AABBCenter {
         AABB bb;
         Vector center;
         entt::entity id;
     };
-    struct Node
-    {
+    struct Node {
         AABB box;
         int32 parentIndex = -1;
         int32 left = -1;

src/Engine/Physics/CollisionSystem.cpp

@@ -3,25 +3,17 @@
 using namespace Seele;
 using namespace Seele::Component;
 
-CollisionSystem::CollisionSystem(entt::registry& registry)
-    : registry(registry)
-{
+CollisionSystem::CollisionSystem(entt::registry& registry) : registry(registry) {
     registry.on_construct<Component::Collider>().connect<&BVH::colliderCallback>(bvh);
 }
 
-CollisionSystem::~CollisionSystem()
-{
-    
-}
+CollisionSystem::~CollisionSystem() {}
 
-void CollisionSystem::detectCollisions(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)
-        {
+    for (auto&& [entity, collider, transform] : view.each()) {
+        if (collider.type == ColliderType::DYNAMIC) {
             bvh.updateDynamicCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
         }
         collider.physicsMesh.transform(transform).visualize();
@@ -29,11 +21,9 @@ void CollisionSystem::detectCollisions(Array<Collision>& collisions)
     bvh.visualize();
     Array<Pair<entt::entity, entt::entity>> overlaps;
     bvh.findOverlaps(overlaps);
-    for(auto pair : overlaps)
-    {
-        if(checkCollision(pair))
-        {
-            collisions.add(Collision {
+    for (auto pair : overlaps) {
+        if (checkCollision(pair)) {
+            collisions.add(Collision{
                 .a = pair.key,
                 .b = pair.value,
             });
@@ -41,42 +31,36 @@ void CollisionSystem::detectCollisions(Array<Collision>& collisions)
     }
 }
 
-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);
+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);
     ShapeBase shape1 = collider1.physicsMesh.transform(transform1);
     ShapeBase shape2 = collider2.physicsMesh.transform(transform2);
     Witness witness;
-    if(cachedWitness.exists(pair))
-    {
+    if (cachedWitness.exists(pair)) {
         witness = cachedWitness[pair];
     }
-    if(witnessValid(witness, shape1, shape2))
-    {
+    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)
-{
+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)
-    {
+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];
@@ -84,48 +68,39 @@ bool CollisionSystem::createWitness(Witness& result, const ShapeBase& source, co
         result.point4Index = UINT32_MAX;
         bool valid = witnessValid(result, source, other);
         // if not, it is a valid separating plane, so no collision
-        if (valid)
-        {
+        if (valid) {
             return false;
         }
     }
-    for (size_t i = 0; i < source.indices.size(); i += 3)
-    {
-        auto findEdgePlane = [this, &source, &other, &result](uint32_t point1Index, uint32_t point2Index)
-        {
+    for (size_t i = 0; i < source.indices.size(); i += 3) {
+        auto findEdgePlane = [this, &source, &other, &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) })
-                {
+            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))
-                    {
+                    if (witnessValid(result, source, other)) {
                         return true;
                     }
                 }
             }
             return false;
         };
-        if(findEdgePlane(source.indices[i], source.indices[i+1]))
-        {
+        if (findEdgePlane(source.indices[i], source.indices[i + 1])) {
             return false;
         }
-        if(findEdgePlane(source.indices[i+1], source.indices[i+2]))
-        {
+        if (findEdgePlane(source.indices[i + 1], source.indices[i + 2])) {
             return false;
         }
-        if(findEdgePlane(source.indices[i+2], source.indices[i]))
-        {
+        if (findEdgePlane(source.indices[i + 2], source.indices[i])) {
             return false;
         }
     }
@@ -133,23 +108,18 @@ bool CollisionSystem::createWitness(Witness& result, const ShapeBase& source, co
     return true;
 }
 
-bool CollisionSystem::witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2)
-{
+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)
-        {
+    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)
-        {
+    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;
@@ -157,4 +127,3 @@ bool CollisionSystem::witnessValid(const Witness& witness, const Component::Shap
     }
     return true;
 }
-

src/Engine/Physics/CollisionSystem.h

@@ -1,26 +1,24 @@
 #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"
+#include "Containers/Array.h"
+#include "Containers/Map.h"
+#include <entt/entt.hpp>
 
-namespace Seele
-{
-struct Collision
-{
+
+namespace Seele {
+struct Collision {
     entt::entity a, b;
 };
-class CollisionSystem
-{
-public:
+class CollisionSystem {
+  public:
     CollisionSystem(entt::registry& registry);
     virtual ~CollisionSystem();
     void detectCollisions(Array<Collision>& collisions);
-private:
-    struct Witness
-    {
+
+  private:
+    struct Witness {
         Vector p;
         Vector n;
         // for finding p

src/Engine/Physics/PhysicsSystem.cpp

@@ -1,47 +1,36 @@
 #include "PhysicsSystem.h"
-#include <random>
 #include <iostream>
+#include <random>
+
 
 using namespace Seele;
 using namespace Seele::Component;
 
-PhysicsSystem::PhysicsSystem(entt::registry& registry)
-    : registry(registry)
-    , collisionSystem(registry)
-{
+PhysicsSystem::PhysicsSystem(entt::registry& registry) : registry(registry), collisionSystem(registry) {}
 
-}
+PhysicsSystem::~PhysicsSystem() {}
 
-PhysicsSystem::~PhysicsSystem()
-{
-    
-}
-
-void PhysicsSystem::update(float deltaTime)
-{
+void PhysicsSystem::update(float deltaTime) {
     Array<Body> initialBodies;
     readRigidBodies(initialBodies);
 
-    //std::cout << "Updating " << initialBodies.size() << " bodies" << std::endl;
+    // std::cout << "Updating " << initialBodies.size() << " bodies" << std::endl;
     Array<Body> bodies = integratePhysics(initialBodies, 0, deltaTime);
     writeRigidBodies(bodies);
 
     Array<Collision> collisions;
     collisionSystem.detectCollisions(collisions);
-    
-    if(!collisions.empty())
-    {
+
+    if (!collisions.empty()) {
         constexpr size_t numSteps = 2;
-        for (float t = 0; t < deltaTime; t += deltaTime / numSteps)
-        {
+        for (float t = 0; t < deltaTime; t += deltaTime / numSteps) {
             rewindCollisions(initialBodies, t, t + (deltaTime / numSteps), 10);
             readRigidBodies(initialBodies);
         }
     }
 }
 
-void PhysicsSystem::serializeRB(const Body& rb, float* y) const
-{
+void PhysicsSystem::serializeRB(const Body& rb, float* y) const {
     *y++ = rb.x.x;
     *y++ = rb.x.y;
     *y++ = rb.x.z;
@@ -60,8 +49,7 @@ void PhysicsSystem::serializeRB(const Body& rb, float* y) const
     *y++ = rb.L.z;
 }
 
-void PhysicsSystem::deserializeRB(Body& rb, const float* y) const
-{
+void PhysicsSystem::deserializeRB(Body& rb, const float* y) const {
     rb.x.x = *y++;
     rb.x.y = *y++;
     rb.x.z = *y++;
@@ -85,49 +73,37 @@ void PhysicsSystem::deserializeRB(Body& rb, const float* y) const
     rb.omega = rb.iInv * rb.L;
 }
 
-void PhysicsSystem::serializeArray(const Array<Body>& bodies, Array<float>& x) const
-{
+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));
+    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
-{
+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));
+    for (uint32_t i = 0; i < bodies.size(); ++i) {
+        deserializeRB(bodies[i], x.data() + (i * FLOATS_PER_RB));
     }
 }
 
-void PhysicsSystem::readRigidBodies(Array<Body>& bodies) const
-{
+void PhysicsSystem::readRigidBodies(Array<Body>& bodies) 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)
-    {
+    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)
-    {
+    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) const
-{
-    for(auto& body : bodies)
-    {
-        if(registry.all_of<RigidBody, Transform>(body.id))
-        {
+void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies) 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);
@@ -139,16 +115,12 @@ void PhysicsSystem::writeRigidBodies(const Array<Body>& bodies) const
     }
 }
 
-PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity) const
-{
+PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity) const {
     Body rigidBody;
-    if(registry.all_of<RigidBody, Collider, Transform>(entity))
-    {
+    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
-    {
+    } else {
         const auto& [collider, transform] = registry.get<Collider, Transform>(entity);
         rigidBody = Body(entity, collider, transform);
     }
@@ -156,11 +128,8 @@ PhysicsSystem::Body PhysicsSystem::readRigidBody(entt::entity entity) const
     return rigidBody;
 }
 
-
-void PhysicsSystem::writeRigidBody(const Body& body) const
-{
-    if(registry.all_of<RigidBody, Transform>(body.id))
-    {
+void PhysicsSystem::writeRigidBody(const Body& body) 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);
@@ -168,34 +137,29 @@ void PhysicsSystem::writeRigidBody(const Body& body) const
         physics.angularMomentum = body.L;
         physics.force = body.force;
         physics.torque = body.torque;
-    }
-    else
-    {
+    } 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<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)
-    {
+    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++)
-        {
+        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)
             Quaternion qdot = 0.5f * (Quaternion(0, result[i].omega) * result[i].q);
             *xdot++ = qdot.w;
@@ -214,54 +178,48 @@ Array<PhysicsSystem::Body> PhysicsSystem::integratePhysics(const Array<Body>& bo
             *xdot++ = result[i].torque.z;
         }
     };
-    //TODO
-    //boost::numeric::odeint::runge_kutta4<Array<float>, float> stepper;
-    //boost::numeric::odeint::integrate_const(stepper, dxdt, buffer, t0, tdelta, tdelta);
+    // TODO
+    // boost::numeric::odeint::runge_kutta4<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, const float t0, const float t1, size_t remainingRecursionDepth)
-{
-    if(remainingRecursionDepth == 0)
-    {
-        //std::cout << "reached max recursion depth" << std::endl;
+void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, 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));
     collisionSystem.detectCollisions(collisions);
-    
-    //std::cout << "detected " << collisions.size() << " at " << tc << std::endl;
-    // now we check if there has been a contact at tc
+
+    // 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);
-        calculateContacts(collision.b, collider2.physicsMesh.transform(transform2), collision.a, collider1.physicsMesh.transform(transform1), contacts);
+    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);
+        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)
-    {
+    for (const auto& contact : contacts) {
         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)
-        {
+        if (vrel > 0.001f) {
             continue;
         }
-        if(vrel > -0.001f)
-        {
+        if (vrel > -0.001f) {
             restingContacts.add(contact);
             continue;
         }
@@ -274,10 +232,9 @@ void PhysicsSystem::rewindCollisions(const Array<Body>& t0Bodies, const float t0
     resolveRestingContacts(restingContacts);
 }
 
-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)
-    {
+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 Vector point1 = shape1.vertices[shape1.indices[i + 0]];
         const Vector point2 = shape1.vertices[shape1.indices[i + 1]];
@@ -285,24 +242,14 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
         const Vector v1 = point2 - point1;
         const Vector v2 = point3 - point1;
         const Vector faceNormal = glm::normalize(glm::cross(v1, v2));
-        addDebugVertex(DebugVertex{
-            .position = (point1 + point2 + point3) / 3.f,
-            .color = Vector(1, 0, 0)
-        });
-        addDebugVertex(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;
-        };
+        addDebugVertex(DebugVertex{.position = (point1 + point2 + point3) / 3.f, .color = Vector(1, 0, 0)});
+        addDebugVertex(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++)
-        {
+        for (size_t j = 0; j < shape2.vertices.size(); j++) {
             Vector worldPos = shape2.vertices[j];
             float dot = glm::dot(faceNormal, worldPos - point1);
-            if(std::abs(dot) < 0.2f)
-            {
+            if (std::abs(dot) < 0.2f) {
                 Vector pa = point1 - worldPos;
                 Vector pb = point2 - worldPos;
                 Vector pc = point3 - worldPos;
@@ -310,27 +257,19 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
                 float a2 = area(pb, pc);
                 float a3 = area(pc, pa);
 
-                if(std::abs(a1 + a2 + a3 - faceArea) > 0.2f)
-                {
+                if (std::abs(a1 + a2 + a3 - faceArea) > 0.2f) {
                     continue;
                 }
 
-                Contact c = {
-                    .a = id2,
-                    .b = id1,
-                    .p = worldPos,
-                    .n = faceNormal,
-                    .vf = true
-                };
+                Contact c = {.a = id2, .b = id1, .p = worldPos, .n = faceNormal, .vf = true};
                 contacts.add(c);
             }
         }
-        //std::cout << minTemp << std::endl;
-        // edge - edge contacts
-        auto lineLineContact = [=, &contacts](Vector p1, Vector p2, Vector p3, Vector p4)
-        {
-            //L1 = p1 + t * p2 - p1;
-            //L2 = p3 + u * p4 - p3;
+        // std::cout << minTemp << std::endl;
+        //  edge - edge contacts
+        auto lineLineContact = [=, &contacts](Vector p1, Vector p2, Vector p3, Vector p4) {
+            // L1 = p1 + t * p2 - p1;
+            // L2 = p3 + u * p4 - p3;
             Vector a = p1;
             Vector c = p3;
             Vector ab = p2 - p1;
@@ -341,12 +280,7 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
             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)
-            {
+            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) {
                 // Vector p = p1 + tx * (p2 - p1);
                 // Vector ea = p2 - p1;
                 // Vector eb = p4 - p3;
@@ -362,8 +296,7 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
                 // };
             }
         };
-        for(size_t j = 0; j < shape2.indices.size(); j+=3)
-        {
+        for (size_t j = 0; j < shape2.indices.size(); j += 3) {
             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]];
@@ -383,11 +316,9 @@ void PhysicsSystem::calculateContacts(entt::entity id1, const ShapeBase& shape1,
     }
 }
 
-void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts) const
-{
+void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts) const {
     Array<Array<float>> amat(contacts.size());
-    for(size_t i = 0; i < contacts.size(); ++i)
-    {
+    for (size_t i = 0; i < contacts.size(); ++i) {
         amat[i] = Array<float>(contacts.size());
     }
     Array<float> bvec(contacts.size());
@@ -398,8 +329,7 @@ void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts) const
     Array<float> fvec(bvec);
     solveQP(amat, bvec, fvec);
 
-    for(size_t y = 0; y < contacts.size(); ++y)
-    {
+    for (size_t y = 0; y < contacts.size(); ++y) {
         std::cout << "resting contact force: " << fvec[y] << std::endl;
         float f = fvec[y];
         Vector n = contacts[y].n;
@@ -415,8 +345,7 @@ void PhysicsSystem::resolveRestingContacts(const Array<Contact>& contacts) const
     }
 }
 
-void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, Body& b) const
-{
+void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, Body& b) const {
     Vector paDot = a.ptVelocity(contact.p);
     Vector pbDot = b.ptVelocity(contact.p);
     float vrel = glm::dot(contact.n, paDot - pbDot);
@@ -445,14 +374,10 @@ void PhysicsSystem::resolvePenetratingContact(const Contact& contact, Body& a, B
     b.omega = b.iInv * b.L;
 }
 
-Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const
-{
-    if(c.vf)
-    {
+Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b) const {
+    if (c.vf) {
         return glm::cross(b.omega, c.n);
-    }
-    else
-    {
+    } else {
         Vector eadot = glm::cross(a.omega, c.ea);
         Vector ebdot = glm::cross(b.omega, c.eb);
         Vector n1 = glm::cross(c.ea, c.eb);
@@ -464,10 +389,8 @@ Vector PhysicsSystem::computeNdot(const Contact& c, const Body& a, const Body& b
     }
 }
 
-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))
+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);
@@ -481,25 +404,19 @@ float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj) const
 
     Vector forceOnA = Vector(0);
     Vector torqueOnA = Vector(0);
-    if(cj.a == ci.a)
-    {
+    if (cj.a == ci.a) {
         forceOnA = nj;
         torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
-    }
-    else if(cj.b == ci.a)
-    {
+    } else if (cj.b == ci.a) {
         forceOnA = -nj;
         torqueOnA = glm::cross((pj - a.x + a.centerOfMass), nj);
     }
     Vector forceOnB = Vector(0);
     Vector torqueOnB = Vector(0);
-    if(cj.a == ci.b)
-    {
+    if (cj.a == ci.b) {
         forceOnB = nj;
         torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
-    }
-    else if(cj.b == ci.b)
-    {
+    } else if (cj.b == ci.b) {
         forceOnB = -nj;
         torqueOnB = glm::cross((pj - b.x + b.centerOfMass), nj);
     }
@@ -513,21 +430,16 @@ float PhysicsSystem::computeAij(const Contact& ci, const Contact& cj) const
     return glm::dot(ni, ((aLinear + aAngular) - (bLinear + bAngular)));
 }
 
-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)
-        {
+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]);
         }
     }
 }
 
-void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>& bvec) const
-{
-    for(size_t y = 0; y < contacts.size(); ++y)
-    {
+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);
         Body b = readRigidBody(c.b);
@@ -539,7 +451,7 @@ void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>&
         Vector fExtB = b.force;
         Vector tExtA = a.torque;
         Vector tExtB = b.torque;
-        
+
         Vector aExtPart = fExtA * a.inverseMass + glm::cross(a.iInv * tExtA, ra);
         Vector bExtPart = fExtB * b.inverseMass + glm::cross(b.iInv * tExtB, rb);
 
@@ -554,28 +466,22 @@ void PhysicsSystem::computeBVector(const Array<Contact>& contacts, Array<float>&
     }
 }
 
-void PhysicsSystem::solveQP(const Array<Array<float>>& CI, const Array<float>& ci0, Array<float>& sol) const
-{
+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)
-        {
+    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)
-        {
+        for (size_t i = 0; i < sol.size(); ++i) {
             float res = 0;
-            for(size_t j = 0; j < sol.size(); ++j)
-            {
+            for (size_t j = 0; j < sol.size(); ++j) {
                 res += CI[i][j] * sol[j] + ci0[i];
             }
-            if(res < 0)
-            {
+            if (res < 0) {
                 solved = false;
                 std::cout << "failed to solve QP" << std::endl;
                 continue;

src/Engine/Physics/PhysicsSystem.h

@@ -1,22 +1,21 @@
 #pragma once
-#include <entt/entt.hpp>
-#include "MinimalEngine.h"
-#include "Component/Transform.h"
-#include "Component/RigidBody.h"
-#include "Component/Collider.h"
 #include "CollisionSystem.h"
+#include "Component/Collider.h"
+#include "Component/RigidBody.h"
+#include "Component/Transform.h"
+#include "MinimalEngine.h"
+#include <entt/entt.hpp>
 
-namespace Seele
-{
-class PhysicsSystem
-{
-public:
+
+namespace Seele {
+class PhysicsSystem {
+  public:
     PhysicsSystem(entt::registry& registry);
     ~PhysicsSystem();
     void update(float deltaTime);
-private:
-    struct Body
-    {
+
+  private:
+    struct Body {
         entt::entity id = entt::entity();
         float inverseMass = 0.0f;
         Vector centerOfMass = Vector();
@@ -35,63 +34,36 @@ private:
         Vector force = Vector();
         Vector torque = Vector();
         Matrix4 matrix = Matrix4();
-        Vector ptVelocity(Vector p) const {return v + glm::cross(omega, p - x + centerOfMass);}
+        Vector ptVelocity(Vector p) const { return v + glm::cross(omega, p - x + centerOfMass); }
         void updateMatrix() {
             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()
-        {}
-        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(Vector())
-            , omega(Vector())
-            , force(physics.force)
-            , torque(physics.torque)
-        {
+        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(Vector()), omega(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(Vector(0))
-            , L(Vector(0))
-            , iInv(glm::mat3())
-            , R(glm::mat3())
-            , v(Vector())
-            , omega(Vector())
-            , force(Vector(0))
-            , torque(Vector(0))
-        {
+            : 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(Vector(0)), L(Vector(0)),
+              iInv(glm::mat3()), R(glm::mat3()), v(Vector()), omega(Vector()), force(Vector(0)), torque(Vector(0)) {
             R = glm::mat3_cast(glm::normalize(q));
             iInv = R * iBodyInv * glm::transpose(R);
             omega = iInv * L;
         }
     };
-    struct Contact
-    {
+    struct Contact {
         entt::entity a, b;
         glm::vec3 p;
         glm::vec3 n;
@@ -105,26 +77,27 @@ private:
     bool pause = false;
 
     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) const;
-    
+
     void writeRigidBodies(const Array<Body>& bodies) const;
-    
+
     Body readRigidBody(entt::entity entity) 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, 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 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) const;