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

Implemented a custom physics engine #316

Merged
direnbharwani merged 95 commits from SHPhysics into main 2023-01-23 15:55:45 +08:00
Conversation 0 Commits 95 Files Changed 144 +599 -103
15 changed files with 599 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d109d06764 - Show all commits

8
Assets/Scenes/PhysicsSandbox.shade
View File

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

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

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

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

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

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

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

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

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

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

@ -31,25 +31,11 @@ namespace SHADE
, state { rhs.state }
, normal { rhs.normal }
{
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(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<size_t>(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<size_t>(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<size_t>(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

57
SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h Normal file
View File

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

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

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

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

@ -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<SHTriggerEvent>;
using CollisionEvents = std::vector<SHCollisionEvent>;
using ContactPoints = std::vector<SHVec3>;
/*---------------------------------------------------------------------------------*/
/* 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

290
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp Normal file
View File

@ -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 <SHpch.h>
// 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

108
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h Normal file
View File

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

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

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

4
SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h
View File

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

16
SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp
View File

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

1
SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h
View File

@ -40,6 +40,7 @@ namespace SHADE
/*-----------------------------------------------------------------------------------*/
friend class SHPhysicsWorld;
friend class SHContactSolver;
public:
/*-----------------------------------------------------------------------------------*/