xiaoqi
/
SHADE_Y3
1
0
Fork 0
Code Issues 12 Pull Requests Packages Projects Releases 9 Wiki Activity

Fixed sequential impulses

This commit is contained in:
Diren D Bharwani 2022-12-22 01:10:25 +08:00
parent 92ed8a29ff
commit f4f6cb7eae
16 changed files with 201 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Assets/Scenes/PhysicsSandbox.shade
View File

@ -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

2
SHADE_Engine/src/Math/Geometry/SHSphere.cpp
View File

@ -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

7
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp
View File

@ -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;

11
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h
View File

@ -22,6 +22,12 @@
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Forward Declarations */
/*-----------------------------------------------------------------------------------*/
class SHRigidBody;
/*-----------------------------------------------------------------------------------*/
/* Type Definitions */
/*-----------------------------------------------------------------------------------*/
@ -38,6 +44,7 @@ namespace SHADE
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,7 +142,7 @@ namespace SHADE
SHPhysicsMaterial material;
SHTransform* parentTransform;
SHCollider* collider; // The collider it belongs to.
SHTransform transform;
// Needed for conversion to euler angles

20
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp
View File

@ -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

2
SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h
View File

@ -49,7 +49,7 @@ namespace SHADE
uint8_t typeB;
};
uint32_t key = 0;
uint32_t key = std::numeric_limits<uint32_t>::max();
};
/**

7
SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h
View File

@ -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;

33
SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp
View File

@ -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;

2
SHADE_Engine/src/Physics/Collision/SHCollider.cpp
View File

@ -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);

1
SHADE_Engine/src/Physics/Collision/SHCollider.h
View File

@ -41,6 +41,7 @@ namespace SHADE
/*---------------------------------------------------------------------------------*/
friend class SHCollisionSpace;
friend struct SHManifold;
public:
/*---------------------------------------------------------------------------------*/

7
SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h
View File

@ -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

106
SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp
View File

@ -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);
}
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;
contactSolver.Reset();
}
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;

25
SHADE_Engine/src/Physics/Dynamics/SHContactManager.h
View File

@ -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 */
/*---------------------------------------------------------------------------------*/

83
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
View File

@ -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);
// 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;
}
}

11
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h
View File

@ -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<EntityID, VelocityState>;
using ContactConstraints = std::vector<SHContactConstraint>;
using ContactConstraints = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
/*---------------------------------------------------------------------------------*/
/* 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

25
SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp
View File

@ -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