diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 0ab20b38..1159f445 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -15,6 +15,7 @@ #include "SHContactManager.h" // Project Headers +#include "Math/SHMathHelpers.h" #include "Physics/Collision/Narrowphase/SHCollisionDispatch.h" namespace SHADE @@ -69,12 +70,12 @@ namespace SHADE for (auto manifoldPair = manifolds.begin(); manifoldPair != manifolds.end();) { // Test collision of every manifold. - SHManifold& oldManifold = manifoldPair->second; - SHManifold newManifold = oldManifold; + SHManifold& manifold = manifoldPair->second; + SHManifold oldManifold = manifold; - const bool IS_COLLIDING = SHCollisionDispatcher::Collide(newManifold, *newManifold.A, *newManifold.B); + const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.A, *manifold.B); - auto& collisionState = newManifold.state; + auto& collisionState = oldManifold.state; updateCollisionState(IS_COLLIDING, collisionState); const bool IS_INVALID = collisionState == SHCollisionState::INVALID; @@ -84,7 +85,7 @@ namespace SHADE continue; } - updateManifold(oldManifold, newManifold); + updateManifold(manifold, oldManifold); ++manifoldPair; } @@ -169,15 +170,56 @@ namespace SHADE } } - void SHContactManager::updateManifold(SHManifold& oldManifold, SHManifold& newManifold) noexcept + void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept { - oldManifold.state = newManifold.state; + manifold.state = oldManifold.state; // Early out since exiting a collision does not require an update beyond updating the state - if (newManifold.state == SHCollisionState::EXIT) + if (manifold.state == SHCollisionState::EXIT) return; + const SHVec3& NORMAL = manifold.normal; + SHVec3& tangent0 = manifold.tangents[0]; + SHVec3& tangent1 = manifold.tangents[1]; + const SHVec3& OLD_TANGENT_0 = oldManifold.tangents[0]; + const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1]; + + // Compute tangents + if (std::fabs(NORMAL.x) >= SHMath::EULER_CONSTANT) + tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f }; + else + tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y }; + + tangent0 = SHVec3::Normalise(tangent0); + tangent1 = SHVec3::Cross(NORMAL, tangent0); + + // Accumulate impulses + for (uint32_t i = 0; i < manifold.numContacts; ++i) + { + SHContact& contact = manifold.contacts[i]; + + // Reset impulses + contact.normalImpulse = 0.0f; + contact.tangentImpulse[0] = contact.tangentImpulse[1] = 0.0f; + + for (uint32_t j = 0; j < oldManifold.numContacts; ++j) + { + const SHContact& OLD_CONTACT = oldManifold.contacts[j]; + + if (OLD_CONTACT.featurePair.key != contact.featurePair.key) + continue; + + // 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); + + break; + } + } } } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h index 9e082db1..ecbf6dd4 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.h @@ -95,8 +95,8 @@ namespace SHADE /* Member Functions */ /*---------------------------------------------------------------------------------*/ - static void updateCollisionState (bool isColliding, SHCollisionState& state) noexcept; - static void updateManifold (SHManifold& oldManifold, SHManifold& newManifold) noexcept; + static void updateCollisionState (bool isColliding, SHCollisionState& state) noexcept; + static void updateManifold (SHManifold& manifold, const SHManifold& newManifold) noexcept; // Removal Helpers