Implemented sequential impulses using baumgarte stabilisation
There is a bug with masses of static bodies not being properly set
This commit is contained in:
Diren D Bharwani
parent
33ef5e0d3d
commit
d109d06764
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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,9 +226,8 @@ 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;
|
||||
|
||||
|
|
@ -221,5 +239,6 @@ namespace SHADE
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace SHADE
|
||||
|
|
@ -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 ContactPoints& GetContactPoints () const noexcept;
|
||||
|
||||
/*---------------------------------------------------------------------------------*/
|
||||
/* Member Functions */
|
||||
|
|
@ -96,7 +108,7 @@ namespace SHADE
|
|||
/*---------------------------------------------------------------------------------*/
|
||||
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ namespace SHADE
|
|||
/*-----------------------------------------------------------------------------------*/
|
||||
|
||||
friend class SHPhysicsWorld;
|
||||
friend class SHContactSolver;
|
||||
|
||||
public:
|
||||
/*-----------------------------------------------------------------------------------*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue