diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index d983c22b..8323a26e 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,7 +4,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 0.0579863191, z: 0} + Translate: {x: 0, y: 3, z: 0} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -15,7 +15,7 @@ Drag: 1 Angular Drag: 1 Use Gravity: true - Gravity Scale: 10 + Gravity Scale: 1 Interpolate: true Sleeping Enabled: true Freeze Position X: false @@ -48,7 +48,7 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 0.5, z: 2} + Position: {x: 0, y: 0.5, z: 3} Pitch: 0 Yaw: 0 Roll: 0 @@ -70,7 +70,7 @@ Scale: {x: 1, y: 1, z: 1} IsActive: true RigidBody Component: - Type: Dynamic + Type: Static Auto Mass: false Mass: 1 Drag: 0.00999999978 diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp index 5dd17d35..d3f26a8b 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp @@ -37,6 +37,16 @@ namespace SHADE /* Getter Function Definitions */ /*-----------------------------------------------------------------------------------*/ + EntityID SHCollisionShape::GetEntityID() const noexcept + { + return id.GetEntityID(); + } + + uint32_t SHCollisionShape::GetIndex() const noexcept + { + return id.GetShapeIndex(); + } + float SHCollisionShape::GetFriction() const noexcept { return material.GetFriction(); diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h index 6e779eea..2cc156e1 100644 --- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h +++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h @@ -75,6 +75,9 @@ namespace SHADE /* Getter Functions */ /*---------------------------------------------------------------------------------*/ + [[nodiscard]] EntityID GetEntityID () const noexcept; + [[nodiscard]] uint32_t GetIndex () const noexcept; + // Material Properties // TODO: Remove individual setters once instanced materials are supported diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h index db55ca50..3dbb16d0 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h @@ -51,7 +51,7 @@ namespace SHADE public: static constexpr int MAX_NUM_CONTACTS = 4; - SHCollisionKey info; + SHCollisionKey info; SHCollisionState state; SHVec3 normal; SHVec3 contactPoints[MAX_NUM_CONTACTS]; diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h index c70f6e39..15c24ef2 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h @@ -27,16 +27,26 @@ namespace SHADE union SHContactFeatures { public: + /*---------------------------------------------------------------------------------*/ + /* Type Definit */ + /*---------------------------------------------------------------------------------*/ + + enum class Type : uint8_t + { + VERTEX = 0 + , FACE = 1 + }; + /*---------------------------------------------------------------------------------*/ /* Data Members */ /*---------------------------------------------------------------------------------*/ struct { - uint8_t incomingIncident; - uint8_t outgoingIncident; - uint8_t incomingReference; - uint8_t outgoingReference; + uint8_t indexA; + uint8_t indexB; + uint8_t typeA; + uint8_t typeB; }; uint32_t key = 0; @@ -56,13 +66,15 @@ namespace SHADE static constexpr int NUM_TANGENTS = 2; float penetration = 0.0f; - float bias = 0.0f; - float normalImpulse = 0.0f; - float normalMass = 0.0f; - float tangentImpulse[NUM_TANGENTS] = { 0.0f }; - float tangentMass[NUM_TANGENTS] = { 0.0f }; + float bias = 0.0f; // Restitution + Baumguarte factor + float normalImpulse = 0.0f; // Accumulated normal impulse + float normalMass = 0.0f; // Effective mass along the normal + float tangentImpulse[NUM_TANGENTS] = { 0.0f }; // Accumulated tangent impulses + float tangentMass[NUM_TANGENTS] = { 0.0f }; // Effective masses along the tangents SHVec3 position; + SHVec3 rA; // Vector from COM of A to the contact + SHVec3 rB; // Vector from COM of B to the contact SHContactFeatures featurePair; }; } diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp index f0e1374f..e43b9625 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp @@ -31,25 +31,11 @@ namespace SHADE , state { rhs.state } , normal { rhs.normal } { + static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); + memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); + for (int i = 0; i < SHContact::NUM_TANGENTS; ++i) tangents[i] = rhs.tangents[i]; - - for (int i = 0; i < MAX_NUM_CONTACTS; ++i) - { - contacts[i].penetration = rhs.contacts[i].penetration; - contacts[i].bias = rhs.contacts[i].bias; - contacts[i].normalImpulse = rhs.contacts[i].normalImpulse; - contacts[i].normalMass = rhs.contacts[i].normalMass; - - for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) - { - contacts[i].tangentImpulse[j] = rhs.contacts[i].tangentImpulse[j]; - contacts[i].tangentMass[j] = rhs.contacts[i].tangentMass[j]; - } - - contacts[i].position = rhs.contacts[i].position; - contacts[i].featurePair.key = rhs.contacts[i].featurePair.key; - } } inline SHManifold::SHManifold(SHManifold&& rhs) noexcept @@ -59,25 +45,11 @@ namespace SHADE , state { rhs.state } , normal { rhs.normal } { + static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); + memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); + for (int i = 0; i < SHContact::NUM_TANGENTS; ++i) tangents[i] = rhs.tangents[i]; - - for (int i = 0; i < MAX_NUM_CONTACTS; ++i) - { - contacts[i].penetration = rhs.contacts[i].penetration; - contacts[i].bias = rhs.contacts[i].bias; - contacts[i].normalImpulse = rhs.contacts[i].normalImpulse; - contacts[i].normalMass = rhs.contacts[i].normalMass; - - for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) - { - contacts[i].tangentImpulse[j] = rhs.contacts[i].tangentImpulse[j]; - contacts[i].tangentMass[j] = rhs.contacts[i].tangentMass[j]; - } - - contacts[i].position = rhs.contacts[i].position; - contacts[i].featurePair.key = rhs.contacts[i].featurePair.key; - } } /*-----------------------------------------------------------------------------------*/ @@ -95,26 +67,12 @@ namespace SHADE state = rhs.state; normal = rhs.normal; + static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); + memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); + for (int i = 0; i < SHContact::NUM_TANGENTS; ++i) tangents[i] = rhs.tangents[i]; - for (int i = 0; i < MAX_NUM_CONTACTS; ++i) - { - contacts[i].penetration = rhs.contacts[i].penetration; - contacts[i].bias = rhs.contacts[i].bias; - contacts[i].normalImpulse = rhs.contacts[i].normalImpulse; - contacts[i].normalMass = rhs.contacts[i].normalMass; - - for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) - { - contacts[i].tangentImpulse[j] = rhs.contacts[i].tangentImpulse[j]; - contacts[i].tangentMass[j] = rhs.contacts[i].tangentMass[j]; - } - - contacts[i].position = rhs.contacts[i].position; - contacts[i].featurePair.key = rhs.contacts[i].featurePair.key; - } - return *this; } @@ -126,26 +84,12 @@ namespace SHADE state = rhs.state; normal = rhs.normal; + static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast(MAX_NUM_CONTACTS); + memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS); + for (int i = 0; i < SHContact::NUM_TANGENTS; ++i) tangents[i] = rhs.tangents[i]; - for (int i = 0; i < MAX_NUM_CONTACTS; ++i) - { - contacts[i].penetration = rhs.contacts[i].penetration; - contacts[i].bias = rhs.contacts[i].bias; - contacts[i].normalImpulse = rhs.contacts[i].normalImpulse; - contacts[i].normalMass = rhs.contacts[i].normalMass; - - for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) - { - contacts[i].tangentImpulse[j] = rhs.contacts[i].tangentImpulse[j]; - contacts[i].tangentMass[j] = rhs.contacts[i].tangentMass[j]; - } - - contacts[i].position = rhs.contacts[i].position; - contacts[i].featurePair.key = rhs.contacts[i].featurePair.key; - } - return *this; } } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h new file mode 100644 index 00000000..55e195a6 --- /dev/null +++ b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h @@ -0,0 +1,57 @@ +/**************************************************************************************** + * \file SHContactConstraint.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Interface for a Contact Constraint. + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +****************************************************************************************/ + +#pragma once + +// Project Headers +#include "Physics/Collision/Contacts/SHManifold.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*-----------------------------------------------------------------------------------*/ + + struct SH_API SHContactConstraint + { + public: + /*---------------------------------------------------------------------------------*/ + /* Data Members */ + /*---------------------------------------------------------------------------------*/ + + // Use the entity IDs to map resolved constraints back to the bodies + + EntityID idA = MAX_EID; + EntityID idB = MAX_EID; + + uint32_t numContacts = 0; + + // Material Data + + float friction = 0.0f; + float restitution = 0.0f; + + // Mass Data + + float invMassA = 0.0f; + float invMassB = 0.0f; + SHMatrix invInertiaA; + SHMatrix invInertiaB; + SHVec3 centerOfMassA; + SHVec3 centerOfMassB; + + // Collision Data + + SHVec3 normal; + SHVec3 tangents[SHContact::NUM_TANGENTS]; + SHContact contacts[SHManifold::MAX_NUM_CONTACTS]; + }; +} // namespace SHADE + diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 1159f445..3ad3c0b3 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -60,6 +60,25 @@ namespace SHADE return collisionEvents; } + const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept + { + static ContactPoints contactPoints; + + contactPoints.clear(); + + for (auto& manifold : manifolds | std::views::values) + { + // Skip exit state manifolds + if (manifold.state == SHCollisionState::EXIT) + continue; + + for (uint32_t i = 0; i < manifold.numContacts; ++i) + contactPoints.emplace_back(manifold.contacts[i].position); + } + + return contactPoints; + } + /*-----------------------------------------------------------------------------------*/ /* Public Member Functions Definitions */ /*-----------------------------------------------------------------------------------*/ @@ -207,17 +226,17 @@ namespace SHADE { const SHContact& OLD_CONTACT = oldManifold.contacts[j]; - if (OLD_CONTACT.featurePair.key != contact.featurePair.key) - continue; + if (OLD_CONTACT.featurePair.key == contact.featurePair.key) + { + // If contact features persists, re-project old solution + contact.normalImpulse = OLD_CONTACT.normalImpulse; - // If contact features persists, re-project old solution - contact.normalImpulse = OLD_CONTACT.normalImpulse; + const SHVec3 FRICTION_FORCE = OLD_TANGENT_0 * OLD_CONTACT.tangentImpulse[0] + OLD_TANGENT_1 * OLD_CONTACT.tangentImpulse[1]; + contact.tangentImpulse[0] = SHVec3::Dot(FRICTION_FORCE, tangent0); + contact.tangentImpulse[1] = SHVec3::Dot(FRICTION_FORCE, tangent1); - const SHVec3 FRICTION_FORCE = OLD_TANGENT_0 * OLD_CONTACT.tangentImpulse[0] + OLD_TANGENT_1 * OLD_CONTACT.tangentImpulse[1]; - contact.tangentImpulse[0] = SHVec3::Dot(FRICTION_FORCE, tangent0); - contact.tangentImpulse[1] = SHVec3::Dot(FRICTION_FORCE, tangent1); - - break; + break; + } } } } diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h index ecbf6dd4..692dc2e8 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h @@ -1,8 +1,7 @@ /**************************************************************************************** * \file SHContactManager.h * \author Diren D Bharwani, diren.dbharwani, 390002520 - * \brief Interface for a Contact Manager that stores collision information and - * resolves contact constraints. + * \brief Interface for a Contact Manager that stores collision information. * * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or * disclosure of this file or its contents without the prior written consent @@ -24,8 +23,19 @@ namespace SHADE /* Type Definitions */ /*-----------------------------------------------------------------------------------*/ + /** + * @brief + * Encapsulates a class that stores collision information. + */ class SH_API SHContactManager { + private: + /*---------------------------------------------------------------------------------*/ + /* Friends */ + /*---------------------------------------------------------------------------------*/ + + friend class SHPhysicsWorld; + public: /*---------------------------------------------------------------------------------*/ /* Type Definitions */ @@ -33,6 +43,7 @@ namespace SHADE using TriggerEvents = std::vector; using CollisionEvents = std::vector; + using ContactPoints = std::vector; /*---------------------------------------------------------------------------------*/ /* Constructors & Destructor */ @@ -42,11 +53,12 @@ namespace SHADE ~SHContactManager () noexcept = default; /*---------------------------------------------------------------------------------*/ - /* Operator Overloads */ + /* Getter Functions */ /*---------------------------------------------------------------------------------*/ - const TriggerEvents& GetTriggerEvents () const noexcept; - const CollisionEvents& GetCollisionEvents () const noexcept; + const TriggerEvents& GetTriggerEvents () const noexcept; + const CollisionEvents& GetCollisionEvents () const noexcept; + const ContactPoints& GetContactPoints () const noexcept; /*---------------------------------------------------------------------------------*/ /* Member Functions */ @@ -58,7 +70,7 @@ namespace SHADE * detection on existing triggers and manifolds. * @return */ - void Update () noexcept; + void Update () noexcept; void AddTrigger (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept; void AddManifold (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept; @@ -88,15 +100,15 @@ namespace SHADE /* Data Members */ /*---------------------------------------------------------------------------------*/ - Manifolds manifolds; - Triggers triggers; + Manifolds manifolds; + Triggers triggers; /*---------------------------------------------------------------------------------*/ /* Member Functions */ /*---------------------------------------------------------------------------------*/ static void updateCollisionState (bool isColliding, SHCollisionState& state) noexcept; - static void updateManifold (SHManifold& manifold, const SHManifold& newManifold) noexcept; + static void updateManifold (SHManifold& manifold, const SHManifold& oldManifold) noexcept; // Removal Helpers diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp new file mode 100644 index 00000000..84805c57 --- /dev/null +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -0,0 +1,290 @@ +/**************************************************************************************** + * \file SHContactSolver.cpp + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Implementation for a Contact Solver. + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +***************************************************************************************/ + +#include + +// Primary Header +#include "SHContactSolver.h" + +// Project Headers +#include "Math/SHMathHelpers.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Getter Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + const SHContactSolver::VelocityStates& SHContactSolver::GetVelocities() const noexcept + { + return velocityStates; + } + + const SHContactSolver::ContactConstraints& SHContactSolver::GetContantConstraints() const noexcept + { + return contactConstraints; + } + + /*-----------------------------------------------------------------------------------*/ + /* Public Member Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + void SHContactSolver::AddContact(const SHManifold& manifold, const SHRigidBody* rigidBodyA, const SHRigidBody* rigidBodyB) noexcept + { + SHContactConstraint& newConstraint = contactConstraints.emplace_back(SHContactConstraint{}); + + const auto* SHAPE_A = manifold.A; + const auto* SHAPE_B = manifold.B; + + newConstraint.idA = SHAPE_A->GetEntityID(); + newConstraint.idB = SHAPE_B->GetEntityID(); + + // 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 }); + + // Mix friction & restitution + const float FRICTION_A = manifold.A->GetFriction(); + const float RESTITUTION_A = manifold.A->GetBounciness(); + + const float FRICTION_B = manifold.B->GetFriction(); + const float RESTITUTION_B = manifold.B->GetBounciness(); + + 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.invInertiaA = rigidBodyA->worldInvInertia; + newConstraint.invInertiaB = rigidBodyB->worldInvInertia; + + newConstraint.centerOfMassA = rigidBodyA->worldCentroid; + newConstraint.centerOfMassB = rigidBodyB->worldCentroid; + + // Collision data + + newConstraint.numContacts = manifold.numContacts; + + newConstraint.normal = manifold.normal; + + static constexpr size_t TANGENTS_SIZE = sizeof(SHVec3) * SHContact::NUM_TANGENTS; + static constexpr size_t CONTACTS_SIZE = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS; + + memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE); + memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE); + + // Compute rA & rB for contacts + for (uint32_t i = 0; i < newConstraint.numContacts; ++i) + { + newConstraint.contacts[i].rA = newConstraint.contacts[i].position - newConstraint.centerOfMassA; + newConstraint.contacts[i].rB = newConstraint.contacts[i].position - newConstraint.centerOfMassB; + } + } + + void SHContactSolver::ClearContacts() noexcept + { + contactConstraints.clear(); + } + + void SHContactSolver::SolveContacts(int numIterations, float dt) noexcept + { + preSolve(dt); + + for (int i = 0; i < numIterations; ++i) + solve(); + } + + /*-----------------------------------------------------------------------------------*/ + /* Private Member Functions Definitions */ + /*-----------------------------------------------------------------------------------*/ + + void SHContactSolver::preSolve(float dt) noexcept + { + const float INV_DT = 1.0f / dt; + + for (auto& 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; + + for (uint32_t i = 0; i < constraint.numContacts; ++i) + { + SHContact& contact = constraint.contacts[i]; + + // Calculate JM-1JT (Effective mass) + /* + * rXnT * I-1 * rXn: + * + * 1. 3x3 * 3x1 = 3x1 + * 2. 1x3 * 3x1 = 1x1 + * + * First do I-1 * rXn + * | ix 0 0 || x | | ix * x | + * | 0 iy 0 || y | = | iy * y | + * | 0 0 iz || z | | iz * z | + * + * Then dot product the result with rXnT + * | ix * x |[ u v w ] + * | iy * y | = [ ix * x * w + iy * y * v + iz * z * w ] + * | iz * z | + * + * Simplified: + * + * rXnT /dot (I-1 * rXn) + */ + + // Effective mass along Normal + const SHVec3 RA_CROSS_N = SHVec3::Cross(contact.rA, constraint.normal); + const SHVec3 RB_CROSS_N = SHVec3::Cross(contact.rB, constraint.normal); + + contact.normalMass = INV_MASS_SUM; + contact.normalMass += SHVec3::Dot(RA_CROSS_N, constraint.invInertiaA * RA_CROSS_N); + 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; + + // Effective mass along tangents (same steps as above) + + for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) + { + const SHVec3 RA_CROSS_T = SHVec3::Cross(contact.rA, constraint.tangents[j]); + const SHVec3 RB_CROSS_T = SHVec3::Cross(contact.rB, constraint.tangents[j]); + + contact.tangentMass[j] = INV_MASS_SUM; + 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]; + } + + // Warm starting + // Compute impulses + SHVec3 impulse = constraint.normal * contact.normalImpulse; + for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) + impulse += constraint.tangents[j] * contact.tangentImpulse[j]; + + // Apply impulses onto velocities + vA -= impulse * constraint.invMassA; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, impulse); + + vB += impulse * constraint.invMassB; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse); + + // Calculate bias per contact + /* + * error bias = baumgarte factor / dt * penetration + * restituion bias = restitution * (relative velocity /dot normal) + */ + + const SHVec3 RV_A = vA + SHVec3::Cross(wA, contact.rA); + 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 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; + + } + } + + void SHContactSolver::solve() noexcept + { + for (auto& 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; + + for (uint32_t i = 0; i < constraint.numContacts; ++i) + { + SHContact& contact = constraint.contacts[i]; + + // Compute relative velocity + SHVec3 velocityA = vA + SHVec3::Cross(wA, contact.rA); + SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB); + SHVec3 relativeVelocity = velocityB - velocityA; + + // Solve tangent impulse + for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) + { + // Get scalar of relative velocity along tangent + const float VT = SHVec3::Dot(relativeVelocity, constraint.tangents[j]); + + // Compute true tangent impulse + const float MAX_TANGENT_IMPULSE = constraint.friction * contact.normalImpulse; + const float OLD_TANGENT_IMPULSE = contact.tangentImpulse[j]; + + // We cannot exceed the maximum frictional force (coulumb's law) + // Compute true tangent impulse + float newTangentImpulse = -VT * contact.tangentMass[j]; + contact.tangentImpulse[j] = std::clamp(OLD_TANGENT_IMPULSE + newTangentImpulse, -MAX_TANGENT_IMPULSE, MAX_TANGENT_IMPULSE); + newTangentImpulse = contact.tangentImpulse[j] - OLD_TANGENT_IMPULSE; + + const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j]; + + // Apply impulses + vA -= TANGENT_IMPULSE * constraint.invMassA; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE); + + vB += TANGENT_IMPULSE * constraint.invMassB; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE); + } + + // Solve normal impulse + // Re-compute relative velocity + velocityA = vA + SHVec3::Cross(wA, contact.rA); + velocityB = vB + SHVec3::Cross(wB, contact.rB); + relativeVelocity = velocityB - velocityA; + + // Get scalar of relative velocity along the normal + const float VN = SHVec3::Dot(relativeVelocity, constraint.normal); + + // Compute true normal impulse + const float OLD_NORMAL_IMPULSE = contact.normalImpulse; + + float newNormalImpulse = -(VN + contact.bias) * contact.normalMass; + contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f); + newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE; + + const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal; + + // Apply impulses + vA -= NORMAL_IMPULSE * constraint.invMassA; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE); + + vB += NORMAL_IMPULSE * constraint.invMassB; + 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; + } + } + + +} // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h new file mode 100644 index 00000000..28d5de6f --- /dev/null +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h @@ -0,0 +1,108 @@ +/**************************************************************************************** + * \file SHContactSolver.h + * \author Diren D Bharwani, diren.dbharwani, 390002520 + * \brief Interface for a Contact Solver that builds contacct constraints and solves + * them. + * + * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or + * disclosure of this file or its contents without the prior written consent + * of DigiPen Institute of Technology is prohibited. +****************************************************************************************/ + +#pragma once + +// Project Headers +#include "Constraints/SHContactConstraint.h" +#include "SHContactManager.h" + +namespace SHADE +{ + /*-----------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*-----------------------------------------------------------------------------------*/ + + /** + * @brief + * Encapsulates an object that builds contact constraints and solves them. + */ + class SH_API SHContactSolver + { + public: + /*---------------------------------------------------------------------------------*/ + /* Type Definitions */ + /*---------------------------------------------------------------------------------*/ + + struct VelocityState + { + // Velocities + + SHVec3 linearVelocity; + SHVec3 angularVelocity; + }; + + using VelocityStates = std::unordered_map; + using ContactConstraints = std::vector; + + /*---------------------------------------------------------------------------------*/ + /* Constructors & Destructor */ + /*---------------------------------------------------------------------------------*/ + + SHContactSolver () noexcept = default; + ~SHContactSolver () noexcept = default; + + /*---------------------------------------------------------------------------------*/ + /* Getter Functions */ + /*---------------------------------------------------------------------------------*/ + + [[nodiscard]] const VelocityStates& GetVelocities () const noexcept; + [[nodiscard]] const ContactConstraints& GetContantConstraints () const noexcept; + + /*---------------------------------------------------------------------------------*/ + /* Member Functions */ + /*---------------------------------------------------------------------------------*/ + + /** + * @brief + * 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 ClearContacts () noexcept; + + /** + * @brief + * Solves all the contact constraints. + * @param numIterations + * The number of times to iterate over constraints when solving them. + * @param dt + * The delta time of the simulation step. + */ + void SolveContacts (int numIterations, float dt) noexcept; + + private: + /*---------------------------------------------------------------------------------*/ + /* Data Members */ + /*---------------------------------------------------------------------------------*/ + + static constexpr float BAUMGARTE_FACTOR = 0.2f; + static constexpr float PENETRATION_SLOP = 0.05f; + + VelocityStates velocityStates; + ContactConstraints contactConstraints; + + /*---------------------------------------------------------------------------------*/ + /* Member Functions */ + /*---------------------------------------------------------------------------------*/ + + void preSolve (float dt) noexcept; + void solve () noexcept; + + }; + +} // namespace SHADE diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp index bb42364d..cb453d24 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp @@ -103,9 +103,33 @@ namespace SHADE /* - * TODO: Resolve Contacts + * 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(); + /* * Integrate Velocities */ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h index 8248299a..9bc2199e 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h @@ -15,6 +15,7 @@ // Project Headers #include "Physics/Collision/SHCollisionSpace.h" #include "SHContactManager.h" +#include "SHContactSolver.h" #include "SHRigidBody.h" @@ -46,7 +47,7 @@ namespace SHADE SHVec3 gravity = SHVec3{ 0.0f, -9.81f, 0.0f }; uint16_t numVelocitySolverIterations = 10; - uint16_t numPositionSolverIterations = 5; + uint16_t numPositionSolverIterations = 5; // Unused until PGS is implemented bool sleepingEnabled = true; }; @@ -128,6 +129,7 @@ namespace SHADE RigidBodies rigidBodies; SHContactManager contactManager; + SHContactSolver contactSolver; /*---------------------------------------------------------------------------------*/ /* Function Members */ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp index aa8027c7..542b6082 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp @@ -296,7 +296,18 @@ namespace SHADE return; bodyType = newType; - invMass = newType == Type::DYNAMIC ? 1.0f : 0.0f; + + if (bodyType != Type::DYNAMIC) + { + invMass = 0.0f; + localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = 0.0f; + worldInvInertia.m[0][0] = worldInvInertia.m[1][1] = worldInvInertia.m[2][2] = 0.0f; + } + else + { + invMass = 1.0f; + localInvInertia = SHMatrix::Identity; + } } void SHRigidBody::SetGravityScale(float newGravityScale) noexcept @@ -598,6 +609,9 @@ namespace SHADE void SHRigidBody::ComputeWorldData() noexcept { + if (bodyType == Type::STATIC) + return; + const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation); // Compute world inertia diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h index ab30bbcf..606895ee 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h @@ -40,6 +40,7 @@ namespace SHADE /*-----------------------------------------------------------------------------------*/ friend class SHPhysicsWorld; + friend class SHContactSolver; public: /*-----------------------------------------------------------------------------------*/