From f4f6cb7eae178a333730ab6fed7f461b94c57829 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Thu, 22 Dec 2022 01:10:25 +0800 Subject: [PATCH] Fixed sequential impulses --- Assets/Scenes/PhysicsSandbox.shade | 2 +- SHADE_Engine/src/Math/Geometry/SHSphere.cpp | 2 +- .../CollisionShapes/SHCollisionShape.cpp | 7 +- .../CollisionShapes/SHCollisionShape.h | 21 ++-- .../SHSphereCollisionShape.cpp | 20 ++-- .../Physics/Collision/Contacts/SHContact.h | 2 +- .../Physics/Collision/Contacts/SHManifold.h | 7 +- .../Physics/Collision/Contacts/SHManifold.hpp | 33 ++++-- .../src/Physics/Collision/SHCollider.cpp | 2 +- .../src/Physics/Collision/SHCollider.h | 3 +- .../Constraints/SHContactConstraint.h | 7 +- .../src/Physics/Dynamics/SHContactManager.cpp | 110 ++++++++++++------ .../src/Physics/Dynamics/SHContactManager.h | 25 ++-- .../src/Physics/Dynamics/SHContactSolver.cpp | 87 +++++++------- .../src/Physics/Dynamics/SHContactSolver.h | 11 +- .../src/Physics/Dynamics/SHPhysicsWorld.cpp | 25 +--- 16 files changed, 201 insertions(+), 163 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index f2a5dd95..90cba8d0 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,7 +4,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 3, z: 0} + Translate: {x: 0.186280191, y: 4.3224473, z: 0} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true diff --git a/SHADE_Engine/src/Math/Geometry/SHSphere.cpp b/SHADE_Engine/src/Math/Geometry/SHSphere.cpp index 7dca00c0..7dbc8f41 100644 --- a/SHADE_Engine/src/Math/Geometry/SHSphere.cpp +++ b/SHADE_Engine/src/Math/Geometry/SHSphere.cpp @@ -115,7 +115,7 @@ namespace SHADE bool SHSphere::Contains(const SHSphere& rhs) const noexcept { - return BoundingSphere::Contains(rhs); + return BoundingSphere::Contains(rhs) == CONTAINS; } float SHSphere::Volume() const noexcept diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp index d3f26a8b..3696dd50 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp @@ -27,7 +27,7 @@ namespace SHADE SHCollisionShape::SHCollisionShape(SHCollisionShapeID id, Type colliderType) : id { id } , flags { 0 } - , parentTransform { nullptr } + , collider { nullptr } , collisionTag { SHCollisionTagMatrix::GetTag(0) } { flags |= 1U << SHUtilities::ConvertEnum(colliderType); @@ -135,11 +135,6 @@ namespace SHADE material = newMaterial; } - void SHCollisionShape::SetParentTransform(SHTransform& parentTF) noexcept - { - parentTransform = &parentTF; - } - void SHCollisionShape::SetPositionOffset(const SHVec3& posOffset) noexcept { transform.position = posOffset; diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h index 2cc156e1..0690b071 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h @@ -22,6 +22,12 @@ namespace SHADE { + /*-----------------------------------------------------------------------------------*/ + /* Forward Declarations */ + /*-----------------------------------------------------------------------------------*/ + + class SHRigidBody; + /*-----------------------------------------------------------------------------------*/ /* Type Definitions */ /*-----------------------------------------------------------------------------------*/ @@ -34,10 +40,11 @@ namespace SHADE /* Friends */ /*---------------------------------------------------------------------------------*/ - friend class SHCollider; - friend class SHColliderComponent; - friend class SHCollisionShapeFactory; - friend class SHCollisionSpace; + friend class SHCollider; + friend class SHColliderComponent; + friend class SHCollisionShapeFactory; + friend class SHCollisionSpace; + friend struct SHManifold; public: /*---------------------------------------------------------------------------------*/ @@ -108,8 +115,6 @@ namespace SHADE void SetDensity (float density) noexcept; void SetMaterial (const SHPhysicsMaterial& newMaterial) noexcept; - void SetParentTransform (SHTransform& parentTF) noexcept; - void SetPositionOffset (const SHVec3& posOffset) noexcept; void SetRotationOffset (const SHVec3& rotOffset) noexcept; @@ -137,13 +142,13 @@ namespace SHADE SHPhysicsMaterial material; - SHTransform* parentTransform; + SHCollider* collider; // The collider it belongs to. SHTransform transform; // Needed for conversion to euler angles SHVec3 rotationOffset; - uint8_t flags; // 0 0 0 isColliding trigger capsule sphere box + uint8_t flags; // 0 0 0 isColliding trigger capsule sphere box SHCollisionTag* collisionTag; RTTR_ENABLE() diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp index b1110489..30497f36 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp @@ -16,6 +16,7 @@ // Project Headers #include "Math/SHMathHelpers.h" #include "Math/SHMatrix.h" +#include "Physics/Collision/SHCollider.h" namespace SHADE { @@ -38,7 +39,7 @@ namespace SHADE { material = rhs.material; - parentTransform = rhs.parentTransform; + collider = rhs.collider; transform = rhs.transform; rotationOffset = rhs.rotationOffset; flags = rhs.flags; @@ -54,7 +55,7 @@ namespace SHADE { material = rhs.material; - parentTransform = rhs.parentTransform; + collider = rhs.collider; transform = rhs.transform; rotationOffset = rhs.rotationOffset; flags = rhs.flags; @@ -75,7 +76,7 @@ namespace SHADE id = rhs.id; material = rhs.material; - parentTransform = rhs.parentTransform; + collider = rhs.collider; transform = rhs.transform; rotationOffset = rhs.rotationOffset; flags = rhs.flags; @@ -101,7 +102,7 @@ namespace SHADE id = rhs.id; material = rhs.material; - parentTransform = rhs.parentTransform; + collider = rhs.collider; transform = rhs.transform; rotationOffset = rhs.rotationOffset; flags = rhs.flags; @@ -179,12 +180,14 @@ namespace SHADE void SHSphereCollisionShape::ComputeTransforms() noexcept { - const float SPHERE_SCALE = std::fabs(SHMath::Max({ parentTransform->scale.x, parentTransform->scale.y, parentTransform->scale.z })); + const SHTransform& PARENT_TRANSFORM = collider->GetTransform(); + + const float SPHERE_SCALE = std::fabs(SHMath::Max({ PARENT_TRANSFORM.scale.x, PARENT_TRANSFORM.scale.y, PARENT_TRANSFORM.scale.z })); SetScale(SPHERE_SCALE); // Recompute center - const SHQuaternion FINAL_ROT = parentTransform->orientation * transform.orientation; - const SHMatrix TRS = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(parentTransform->position); + const SHQuaternion FINAL_ROT = PARENT_TRANSFORM.orientation * transform.orientation; + const SHMatrix TRS = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(PARENT_TRANSFORM.position); Center = SHVec3::Transform(transform.position, TRS); } @@ -212,7 +215,8 @@ namespace SHADE SHMatrix SHSphereCollisionShape::ComputeWorldTransform() const noexcept { - const SHQuaternion ROTATION = parentTransform->orientation * transform.orientation; + const SHTransform& PARENT_TRANSFORM = collider->GetTransform(); + const SHQuaternion ROTATION = PARENT_TRANSFORM.orientation * transform.orientation; const SHVec3 SCALE{ Radius }; return SHMatrix::Transform diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h index 15c24ef2..0c137f2f 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h @@ -49,7 +49,7 @@ namespace SHADE uint8_t typeB; }; - uint32_t key = 0; + uint32_t key = std::numeric_limits::max(); }; /** diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h index c2b6ddb2..15741276 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h @@ -49,8 +49,11 @@ namespace SHADE // We only need 4 contact points to build a stable manifold. static constexpr int MAX_NUM_CONTACTS = 4; - SHCollisionShape* A; - SHCollisionShape* B; + SHCollisionShape* shapeA; + SHCollisionShape* shapeB; + + SHRigidBody* bodyA = nullptr; + SHRigidBody* bodyB = nullptr; uint32_t numContacts = 0; SHCollisionState state = SHCollisionState::INVALID; diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp index e43b9625..23be8c79 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp @@ -20,13 +20,18 @@ namespace SHADE /*-----------------------------------------------------------------------------------*/ inline SHManifold::SHManifold(SHCollisionShape* a, SHCollisionShape* b) noexcept - : A { a } - , B { b } - {} + : shapeA { a } + , shapeB { b } + { + bodyA = shapeA->collider->rigidBody; + bodyB = shapeB->collider->rigidBody; + } inline SHManifold::SHManifold(const SHManifold& rhs) noexcept - : A { rhs.A } - , B { rhs.B } + : shapeA { rhs.shapeA } + , shapeB { rhs.shapeB } + , bodyA { rhs.bodyA } + , bodyB { rhs.bodyB } , numContacts { rhs.numContacts } , state { rhs.state } , normal { rhs.normal } @@ -39,8 +44,10 @@ namespace SHADE } inline SHManifold::SHManifold(SHManifold&& rhs) noexcept - : A { rhs.A } - , B { rhs.B } + : shapeA { rhs.shapeA } + , shapeB { rhs.shapeB } + , bodyA { rhs.bodyA } + , bodyB { rhs.bodyB } , numContacts { rhs.numContacts } , state { rhs.state } , normal { rhs.normal } @@ -61,8 +68,10 @@ namespace SHADE if (this == &rhs) return *this; - A = rhs.A; - B = rhs.B; + shapeA = rhs.shapeA; + shapeB = rhs.shapeB; + bodyA = rhs.bodyA; + bodyB = rhs.bodyB; numContacts = rhs.numContacts; state = rhs.state; normal = rhs.normal; @@ -78,8 +87,10 @@ namespace SHADE inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept { - A = rhs.A; - B = rhs.B; + shapeA = rhs.shapeA; + shapeB = rhs.shapeB; + bodyA = rhs.bodyA; + bodyB = rhs.bodyB; numContacts = rhs.numContacts; state = rhs.state; normal = rhs.normal; diff --git a/SHADE_Engine/src/Physics/Collision/SHCollider.cpp b/SHADE_Engine/src/Physics/Collision/SHCollider.cpp index 928945c9..7d7d1a60 100644 --- a/SHADE_Engine/src/Physics/Collision/SHCollider.cpp +++ b/SHADE_Engine/src/Physics/Collision/SHCollider.cpp @@ -307,7 +307,7 @@ namespace SHADE SHSphereCollisionShape* sphere = shapeFactory->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO); // Set offsets - sphere->SetParentTransform(transform); + sphere->collider = this; sphere->SetPositionOffset(posOffset); sphere->SetRotationOffset(rotOffset); diff --git a/SHADE_Engine/src/Physics/Collision/SHCollider.h b/SHADE_Engine/src/Physics/Collision/SHCollider.h index c31be5eb..51b33cfb 100644 --- a/SHADE_Engine/src/Physics/Collision/SHCollider.h +++ b/SHADE_Engine/src/Physics/Collision/SHCollider.h @@ -40,7 +40,8 @@ namespace SHADE /* Friends */ /*---------------------------------------------------------------------------------*/ - friend class SHCollisionSpace; + friend class SHCollisionSpace; + friend struct SHManifold; public: /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h index 55e195a6..04be00b4 100644 --- a/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h +++ b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h @@ -28,10 +28,7 @@ namespace SHADE // Use the entity IDs to map resolved constraints back to the bodies - EntityID idA = MAX_EID; - EntityID idB = MAX_EID; - - uint32_t numContacts = 0; + SHCollisionKey key; // Material Data @@ -52,6 +49,8 @@ namespace SHADE SHVec3 normal; SHVec3 tangents[SHContact::NUM_TANGENTS]; SHContact contacts[SHManifold::MAX_NUM_CONTACTS]; + + uint32_t numContacts = 0; }; } // namespace SHADE diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 3ad3c0b3..3b80b3b3 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -83,6 +83,42 @@ namespace SHADE /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ + void SHContactManager::AddTrigger(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept + { + // If id not found, emplace new trigger. + auto trigger = triggers.find(key); + if (trigger == triggers.end()) + triggers.emplace(key, Trigger{ A, B, SHCollisionState::INVALID }); + } + + void SHContactManager::AddManifold(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept + { + // If id not found, emplace new manifold + auto manifold = manifolds.find(key); + if (manifold == manifolds.end()) + manifolds.emplace(key, SHManifold{ A, B }); + } + + void SHContactManager::RemoveInvalidatedTrigger(EntityID eid) noexcept + { + removeInvalidObject(triggers, eid); + } + + void SHContactManager::RemoveInvalidatedTrigger(EntityID eid, uint32_t shapeIndex) noexcept + { + removeInvalidObject(triggers, eid, shapeIndex); + } + + void SHContactManager::RemoveInvalidatedManifold(EntityID eid) noexcept + { + removeInvalidObject(manifolds, eid); + } + + void SHContactManager::RemoveInvalidatedManifold(EntityID eid, uint32_t shapeIndex) noexcept + { + removeInvalidObject(manifolds, eid, shapeIndex); + } + void SHContactManager::Update() noexcept { // Clear expired or invalid collisions. If not, test collision. @@ -92,9 +128,9 @@ namespace SHADE SHManifold& manifold = manifoldPair->second; SHManifold oldManifold = manifold; - const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.A, *manifold.B); + const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.shapeA, *manifold.shapeB); - auto& collisionState = oldManifold.state; + auto& collisionState = manifold.state; updateCollisionState(IS_COLLIDING, collisionState); const bool IS_INVALID = collisionState == SHCollisionState::INVALID; @@ -128,41 +164,45 @@ namespace SHADE } } - void SHContactManager::AddTrigger(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept + void SHContactManager::SolveCollisions(int numIterations, float dt) noexcept { - // If id not found, emplace new trigger. - auto trigger = triggers.find(key); - if (trigger == triggers.end()) - triggers.emplace(key, Trigger{ A, B, SHCollisionState::INVALID }).first; + static constexpr int SIZE_CONTACTS = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS; + + // Build constraints + for (auto& [key, manifold] : manifolds) + contactSolver.AddContact(key, manifold); + + // Solve contacts + contactSolver.SolveContacts(numIterations, dt); + + // Map impulses back to manifolds + const auto& CONTACT_CONSTRAINTS = contactSolver.GetContantConstraints(); + const auto& VELOCITY_STATES = contactSolver.GetVelocities(); + + for (auto& [key, contactConstraint] : CONTACT_CONSTRAINTS) + { + SHManifold& manifold = manifolds.find(key)->second; + + for (uint32_t i = 0; i < contactConstraint.numContacts; ++i) + manifold.contacts[i] = contactConstraint.contacts[i]; + + // Assign velocities back to the bodies + SHRigidBody* bodyA = manifold.bodyA; + SHRigidBody* bodyB = manifold.bodyB; + + const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second; + const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second; + + bodyA->SetLinearVelocity(STATE_A.linearVelocity); + bodyB->SetLinearVelocity(STATE_B.linearVelocity); + + bodyA->SetAngularVelocity(STATE_A.angularVelocity); + bodyB->SetAngularVelocity(STATE_B.angularVelocity); + } + + contactSolver.Reset(); } - void SHContactManager::AddManifold(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept - { - // If id not found, emplace new manifold - auto manifold = manifolds.find(key); - if (manifold == manifolds.end()) - manifolds.emplace(key, SHManifold{ A, B }).first; - } - - void SHContactManager::RemoveInvalidatedTrigger(EntityID eid) noexcept - { - removeInvalidObject(triggers, eid); - } - - void SHContactManager::RemoveInvalidatedTrigger(EntityID eid, uint32_t shapeIndex) noexcept - { - removeInvalidObject(triggers, eid, shapeIndex); - } - - void SHContactManager::RemoveInvalidatedManifold(EntityID eid) noexcept - { - removeInvalidObject(manifolds, eid); - } - - void SHContactManager::RemoveInvalidatedManifold(EntityID eid, uint32_t shapeIndex) noexcept - { - removeInvalidObject(manifolds, eid, shapeIndex); - } /*-----------------------------------------------------------------------------------*/ /* Private Member Functions Definitions */ @@ -191,8 +231,6 @@ namespace SHADE void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept { - manifold.state = oldManifold.state; - // Early out since exiting a collision does not require an update beyond updating the state if (manifold.state == SHCollisionState::EXIT) return; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h index 692dc2e8..c2433345 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h @@ -16,6 +16,7 @@ #include "Physics/Collision/Contacts/SHCollisionEvents.h" #include "Physics/Collision/Contacts/SHCollisionKey.h" #include "Physics/Collision/Contacts/SHManifold.h" +#include "SHContactSolver.h" namespace SHADE { @@ -64,14 +65,6 @@ namespace SHADE /* Member Functions */ /*---------------------------------------------------------------------------------*/ - /** - * @brief - * Removes any invalidated contacts and triggers, then performs narrowphase collision - * detection on existing triggers and manifolds. - * @return - */ - void Update () noexcept; - void AddTrigger (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept; void AddManifold (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept; @@ -80,6 +73,20 @@ namespace SHADE void RemoveInvalidatedManifold (EntityID eid) noexcept; void RemoveInvalidatedManifold (EntityID eid, uint32_t shapeIndex) noexcept; + /** + * @brief + * Removes any invalidated contacts and triggers, then performs narrowphase collision + * detection on existing triggers and manifolds. + */ + void Update () noexcept; + + /** + * @brief + * Builds contact constraints and solves them. Results are stored in the corresponding + * manifolds abiding by the sequential impulse method. + */ + void SolveCollisions (int numIterations, float dt) noexcept; + private: /*---------------------------------------------------------------------------------*/ /* Type Definitions */ @@ -103,6 +110,8 @@ namespace SHADE Manifolds manifolds; Triggers triggers; + SHContactSolver contactSolver; + /*---------------------------------------------------------------------------------*/ /* Member Functions */ /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index 84805c57..c14c9eb5 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -36,40 +36,39 @@ namespace SHADE /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ - void SHContactSolver::AddContact(const SHManifold& manifold, const SHRigidBody* rigidBodyA, const SHRigidBody* rigidBodyB) noexcept + void SHContactSolver::AddContact(const SHCollisionKey& key, const SHManifold& manifold) noexcept { - SHContactConstraint& newConstraint = contactConstraints.emplace_back(SHContactConstraint{}); + SHContactConstraint& newConstraint = contactConstraints.emplace(key, SHContactConstraint{}).first->second; - const auto* SHAPE_A = manifold.A; - const auto* SHAPE_B = manifold.B; + const auto* SHAPE_A = manifold.shapeA; + const auto* SHAPE_B = manifold.shapeB; - newConstraint.idA = SHAPE_A->GetEntityID(); - newConstraint.idB = SHAPE_B->GetEntityID(); + const auto* BODY_A = manifold.bodyA; + const auto* BODY_B = manifold.bodyB; // Add velocities if it doesn't already exist - velocityStates.emplace(newConstraint.idA, VelocityState{ rigidBodyA->linearVelocity, rigidBodyB->angularVelocity }); - velocityStates.emplace(newConstraint.idB, VelocityState{ rigidBodyB->linearVelocity, rigidBodyB->angularVelocity }); + velocityStates.emplace(key.GetEntityA(), VelocityState{ BODY_A->linearVelocity, BODY_A->angularVelocity }); + velocityStates.emplace(key.GetEntityB(), VelocityState{ BODY_B->linearVelocity, BODY_B->angularVelocity }); // Mix friction & restitution - const float FRICTION_A = manifold.A->GetFriction(); - const float RESTITUTION_A = manifold.A->GetBounciness(); + const float FRICTION_A = SHAPE_A->GetFriction(); + const float RESTITUTION_A = SHAPE_A->GetBounciness(); - const float FRICTION_B = manifold.B->GetFriction(); - const float RESTITUTION_B = manifold.B->GetBounciness(); + const float FRICTION_B = SHAPE_B->GetFriction(); + const float RESTITUTION_B = SHAPE_B->GetBounciness(); - newConstraint.friction = std::sqrtf(FRICTION_A * FRICTION_B); - newConstraint.restitution = std::max(RESTITUTION_A, RESTITUTION_B); + newConstraint.friction = std::sqrtf(FRICTION_A * FRICTION_B); + newConstraint.restitution = std::max(RESTITUTION_A, RESTITUTION_B); // Mass data - newConstraint.invMassA = rigidBodyA->invMass; - newConstraint.invMassB = rigidBodyB->invMass; + newConstraint.invMassA = BODY_A->invMass; + newConstraint.invInertiaA = BODY_A->worldInvInertia; + newConstraint.centerOfMassA = BODY_A->worldCentroid; - newConstraint.invInertiaA = rigidBodyA->worldInvInertia; - newConstraint.invInertiaB = rigidBodyB->worldInvInertia; - - newConstraint.centerOfMassA = rigidBodyA->worldCentroid; - newConstraint.centerOfMassB = rigidBodyB->worldCentroid; + newConstraint.invMassB = BODY_B->invMass; + newConstraint.invInertiaB = BODY_B->worldInvInertia; + newConstraint.centerOfMassB = BODY_B->worldCentroid; // Collision data @@ -91,8 +90,9 @@ namespace SHADE } } - void SHContactSolver::ClearContacts() noexcept + void SHContactSolver::Reset() noexcept { + velocityStates.clear(); contactConstraints.clear(); } @@ -112,14 +112,14 @@ namespace SHADE { const float INV_DT = 1.0f / dt; - for (auto& constraint : contactConstraints) + for (auto& [key, constraint] : contactConstraints) { const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB; - SHVec3 vA = velocityStates[constraint.idA].linearVelocity; - SHVec3 wA = velocityStates[constraint.idA].angularVelocity; - SHVec3 vB = velocityStates[constraint.idB].linearVelocity; - SHVec3 wB = velocityStates[constraint.idB].angularVelocity; + SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; + SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; + SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity; + SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; for (uint32_t i = 0; i < constraint.numContacts; ++i) { @@ -156,7 +156,7 @@ namespace SHADE contact.normalMass += SHVec3::Dot(RB_CROSS_N, constraint.invInertiaB * RB_CROSS_N); // Invert the normal mass (we want the actual mass, not the inverse mass) - contact.normalMass = contact.normalMass == 0.0f ? 0.0f : 1.0f / contact.normalMass; + contact.normalMass = 1.0f / contact.normalMass; // Effective mass along tangents (same steps as above) @@ -169,7 +169,7 @@ namespace SHADE contact.tangentMass[j] += SHVec3::Dot(RA_CROSS_T, constraint.invInertiaA * RA_CROSS_T); contact.tangentMass[j] += SHVec3::Dot(RB_CROSS_T, constraint.invInertiaB * RB_CROSS_T); - contact.tangentMass[j] = contact.tangentMass[j] == 0.0f ? 0.0f : 1.0f / contact.tangentMass[j]; + contact.tangentMass[j] = 1.0f / contact.tangentMass[j]; } // Warm starting @@ -195,28 +195,27 @@ namespace SHADE const SHVec3 RV_B = vB + SHVec3::Cross(wB, contact.rB); const float RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal); - const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * contact.penetration; + const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * std::min(0.0f, -contact.penetration + PENETRATION_SLOP); const float RESTITUTION_BIAS = -constraint.restitution * RV_N; contact.bias = ERROR_BIAS + RESTITUTION_BIAS; } - velocityStates[constraint.idA].linearVelocity = vA; - velocityStates[constraint.idA].angularVelocity = wA; - velocityStates[constraint.idB].linearVelocity = vB; - velocityStates[constraint.idB].angularVelocity = wB; - + velocityStates[key.GetEntityA()].linearVelocity = vA; + velocityStates[key.GetEntityA()].angularVelocity = wA; + velocityStates[key.GetEntityB()].linearVelocity = vB; + velocityStates[key.GetEntityB()].angularVelocity = wB; } } void SHContactSolver::solve() noexcept { - for (auto& constraint : contactConstraints) + for (auto& [key, constraint] : contactConstraints) { - SHVec3 vA = velocityStates[constraint.idA].linearVelocity; - SHVec3 wA = velocityStates[constraint.idA].angularVelocity; - SHVec3 vB = velocityStates[constraint.idB].linearVelocity; - SHVec3 wB = velocityStates[constraint.idB].angularVelocity; + SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; + SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; + SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity; + SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; for (uint32_t i = 0; i < constraint.numContacts; ++i) { @@ -279,10 +278,10 @@ namespace SHADE wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE); } - velocityStates[constraint.idA].linearVelocity = vA; - velocityStates[constraint.idA].angularVelocity = wA; - velocityStates[constraint.idB].linearVelocity = vB; - velocityStates[constraint.idB].angularVelocity = wB; + velocityStates[key.GetEntityA()].linearVelocity = vA; + velocityStates[key.GetEntityA()].angularVelocity = wA; + velocityStates[key.GetEntityB()].linearVelocity = vB; + velocityStates[key.GetEntityB()].angularVelocity = wB; } } diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h index 28d5de6f..7a4f2d47 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h @@ -13,7 +13,6 @@ // Project Headers #include "Constraints/SHContactConstraint.h" -#include "SHContactManager.h" namespace SHADE { @@ -41,7 +40,7 @@ namespace SHADE }; using VelocityStates = std::unordered_map; - using ContactConstraints = std::vector; + using ContactConstraints = std::unordered_map; /*---------------------------------------------------------------------------------*/ /* Constructors & Destructor */ @@ -66,14 +65,10 @@ namespace SHADE * Build a contact constraint from a new manifold. * @param manifold * A manifold to build a contact constraint from. - * @param rigidBodyA - * The rigid body belonging to the first collision shape. - * @param rigidBodyB - * The rigid body belonging to the second collision shape. */ - void AddContact (const SHManifold& manifold, const SHRigidBody* rigidBodyA, const SHRigidBody* rigidBodyB) noexcept; + void AddContact (const SHCollisionKey& key, const SHManifold& manifold) noexcept; - void ClearContacts () noexcept; + void Reset () noexcept; /** * @brief diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp index cb453d24..dfa3f482 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp @@ -103,32 +103,11 @@ namespace SHADE /* + * TODO: A lot of this needs to be cleaned up. * Resolve Contacts */ - // Build constraints - for (auto& [id, manifold] : contactManager.manifolds) - { - SHRigidBody* bodyA = rigidBodies[id.GetEntityA()]; - SHRigidBody* bodyB = rigidBodies[id.GetEntityB()]; - - contactSolver.AddContact(manifold, bodyA, bodyB); - } - - // Solve contacts - contactSolver.SolveContacts(settings.numVelocitySolverIterations, dt); - - // Map velocities back to bodies - const auto& VELOCITY_STATES = contactSolver.GetVelocities(); - for (auto& [id, velocityState] : VELOCITY_STATES) - { - SHRigidBody* body = rigidBodies[id]; - body->linearVelocity = velocityState.linearVelocity; - body->angularVelocity = velocityState.angularVelocity; - } - - // Clear contacts - contactSolver.ClearContacts(); + contactManager.SolveCollisions(settings.numVelocitySolverIterations, dt); /* * Integrate Velocities