Implemented sequential impulses using baumgarte stabilisation
There is a bug with masses of static bodies not being properly set
This commit is contained in:
parent
33ef5e0d3d
commit
d109d06764
|
@ -4,7 +4,7 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Transform Component:
|
Transform Component:
|
||||||
Translate: {x: 0, y: 0.0579863191, z: 0}
|
Translate: {x: 0, y: 3, z: 0}
|
||||||
Rotate: {x: -0, y: 0, z: -0}
|
Rotate: {x: -0, y: 0, z: -0}
|
||||||
Scale: {x: 1, y: 1, z: 1}
|
Scale: {x: 1, y: 1, z: 1}
|
||||||
IsActive: true
|
IsActive: true
|
||||||
|
@ -15,7 +15,7 @@
|
||||||
Drag: 1
|
Drag: 1
|
||||||
Angular Drag: 1
|
Angular Drag: 1
|
||||||
Use Gravity: true
|
Use Gravity: true
|
||||||
Gravity Scale: 10
|
Gravity Scale: 1
|
||||||
Interpolate: true
|
Interpolate: true
|
||||||
Sleeping Enabled: true
|
Sleeping Enabled: true
|
||||||
Freeze Position X: false
|
Freeze Position X: false
|
||||||
|
@ -48,7 +48,7 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Camera Component:
|
Camera Component:
|
||||||
Position: {x: 0, y: 0.5, z: 2}
|
Position: {x: 0, y: 0.5, z: 3}
|
||||||
Pitch: 0
|
Pitch: 0
|
||||||
Yaw: 0
|
Yaw: 0
|
||||||
Roll: 0
|
Roll: 0
|
||||||
|
@ -70,7 +70,7 @@
|
||||||
Scale: {x: 1, y: 1, z: 1}
|
Scale: {x: 1, y: 1, z: 1}
|
||||||
IsActive: true
|
IsActive: true
|
||||||
RigidBody Component:
|
RigidBody Component:
|
||||||
Type: Dynamic
|
Type: Static
|
||||||
Auto Mass: false
|
Auto Mass: false
|
||||||
Mass: 1
|
Mass: 1
|
||||||
Drag: 0.00999999978
|
Drag: 0.00999999978
|
||||||
|
|
|
@ -37,6 +37,16 @@ namespace SHADE
|
||||||
/* Getter Function Definitions */
|
/* 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
|
float SHCollisionShape::GetFriction() const noexcept
|
||||||
{
|
{
|
||||||
return material.GetFriction();
|
return material.GetFriction();
|
||||||
|
|
|
@ -75,6 +75,9 @@ namespace SHADE
|
||||||
/* Getter Functions */
|
/* Getter Functions */
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
|
[[nodiscard]] EntityID GetEntityID () const noexcept;
|
||||||
|
[[nodiscard]] uint32_t GetIndex () const noexcept;
|
||||||
|
|
||||||
// Material Properties
|
// Material Properties
|
||||||
// TODO: Remove individual setters once instanced materials are supported
|
// TODO: Remove individual setters once instanced materials are supported
|
||||||
|
|
||||||
|
|
|
@ -27,16 +27,26 @@ namespace SHADE
|
||||||
union SHContactFeatures
|
union SHContactFeatures
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
/* Type Definit */
|
||||||
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
|
enum class Type : uint8_t
|
||||||
|
{
|
||||||
|
VERTEX = 0
|
||||||
|
, FACE = 1
|
||||||
|
};
|
||||||
|
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Data Members */
|
/* Data Members */
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
struct
|
struct
|
||||||
{
|
{
|
||||||
uint8_t incomingIncident;
|
uint8_t indexA;
|
||||||
uint8_t outgoingIncident;
|
uint8_t indexB;
|
||||||
uint8_t incomingReference;
|
uint8_t typeA;
|
||||||
uint8_t outgoingReference;
|
uint8_t typeB;
|
||||||
};
|
};
|
||||||
|
|
||||||
uint32_t key = 0;
|
uint32_t key = 0;
|
||||||
|
@ -56,13 +66,15 @@ namespace SHADE
|
||||||
static constexpr int NUM_TANGENTS = 2;
|
static constexpr int NUM_TANGENTS = 2;
|
||||||
|
|
||||||
float penetration = 0.0f;
|
float penetration = 0.0f;
|
||||||
float bias = 0.0f;
|
float bias = 0.0f; // Restitution + Baumguarte factor
|
||||||
float normalImpulse = 0.0f;
|
float normalImpulse = 0.0f; // Accumulated normal impulse
|
||||||
float normalMass = 0.0f;
|
float normalMass = 0.0f; // Effective mass along the normal
|
||||||
float tangentImpulse[NUM_TANGENTS] = { 0.0f };
|
float tangentImpulse[NUM_TANGENTS] = { 0.0f }; // Accumulated tangent impulses
|
||||||
float tangentMass[NUM_TANGENTS] = { 0.0f };
|
float tangentMass[NUM_TANGENTS] = { 0.0f }; // Effective masses along the tangents
|
||||||
|
|
||||||
SHVec3 position;
|
SHVec3 position;
|
||||||
|
SHVec3 rA; // Vector from COM of A to the contact
|
||||||
|
SHVec3 rB; // Vector from COM of B to the contact
|
||||||
SHContactFeatures featurePair;
|
SHContactFeatures featurePair;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
|
@ -31,25 +31,11 @@ namespace SHADE
|
||||||
, state { rhs.state }
|
, state { rhs.state }
|
||||||
, normal { rhs.normal }
|
, 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)
|
for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
|
||||||
tangents[i] = rhs.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
|
inline SHManifold::SHManifold(SHManifold&& rhs) noexcept
|
||||||
|
@ -59,25 +45,11 @@ namespace SHADE
|
||||||
, state { rhs.state }
|
, state { rhs.state }
|
||||||
, normal { rhs.normal }
|
, 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)
|
for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
|
||||||
tangents[i] = rhs.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;
|
state = rhs.state;
|
||||||
normal = rhs.normal;
|
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)
|
for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
|
||||||
tangents[i] = rhs.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;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -126,26 +84,12 @@ namespace SHADE
|
||||||
state = rhs.state;
|
state = rhs.state;
|
||||||
normal = rhs.normal;
|
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)
|
for (int i = 0; i < SHContact::NUM_TANGENTS; ++i)
|
||||||
tangents[i] = rhs.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;
|
return *this;
|
||||||
}
|
}
|
||||||
} // namespace SHADE
|
} // 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;
|
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 */
|
/* Public Member Functions Definitions */
|
||||||
/*-----------------------------------------------------------------------------------*/
|
/*-----------------------------------------------------------------------------------*/
|
||||||
|
@ -207,9 +226,8 @@ namespace SHADE
|
||||||
{
|
{
|
||||||
const SHContact& OLD_CONTACT = oldManifold.contacts[j];
|
const SHContact& OLD_CONTACT = oldManifold.contacts[j];
|
||||||
|
|
||||||
if (OLD_CONTACT.featurePair.key != contact.featurePair.key)
|
if (OLD_CONTACT.featurePair.key == contact.featurePair.key)
|
||||||
continue;
|
{
|
||||||
|
|
||||||
// If contact features persists, re-project old solution
|
// If contact features persists, re-project old solution
|
||||||
contact.normalImpulse = OLD_CONTACT.normalImpulse;
|
contact.normalImpulse = OLD_CONTACT.normalImpulse;
|
||||||
|
|
||||||
|
@ -221,5 +239,6 @@ namespace SHADE
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace SHADE
|
} // namespace SHADE
|
|
@ -1,8 +1,7 @@
|
||||||
/****************************************************************************************
|
/****************************************************************************************
|
||||||
* \file SHContactManager.h
|
* \file SHContactManager.h
|
||||||
* \author Diren D Bharwani, diren.dbharwani, 390002520
|
* \author Diren D Bharwani, diren.dbharwani, 390002520
|
||||||
* \brief Interface for a Contact Manager that stores collision information and
|
* \brief Interface for a Contact Manager that stores collision information.
|
||||||
* resolves contact constraints.
|
|
||||||
*
|
*
|
||||||
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
|
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
|
||||||
* disclosure of this file or its contents without the prior written consent
|
* disclosure of this file or its contents without the prior written consent
|
||||||
|
@ -24,8 +23,19 @@ namespace SHADE
|
||||||
/* Type Definitions */
|
/* Type Definitions */
|
||||||
/*-----------------------------------------------------------------------------------*/
|
/*-----------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief
|
||||||
|
* Encapsulates a class that stores collision information.
|
||||||
|
*/
|
||||||
class SH_API SHContactManager
|
class SH_API SHContactManager
|
||||||
{
|
{
|
||||||
|
private:
|
||||||
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
/* Friends */
|
||||||
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
|
friend class SHPhysicsWorld;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Type Definitions */
|
/* Type Definitions */
|
||||||
|
@ -33,6 +43,7 @@ namespace SHADE
|
||||||
|
|
||||||
using TriggerEvents = std::vector<SHTriggerEvent>;
|
using TriggerEvents = std::vector<SHTriggerEvent>;
|
||||||
using CollisionEvents = std::vector<SHCollisionEvent>;
|
using CollisionEvents = std::vector<SHCollisionEvent>;
|
||||||
|
using ContactPoints = std::vector<SHVec3>;
|
||||||
|
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Constructors & Destructor */
|
/* Constructors & Destructor */
|
||||||
|
@ -42,11 +53,12 @@ namespace SHADE
|
||||||
~SHContactManager () noexcept = default;
|
~SHContactManager () noexcept = default;
|
||||||
|
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Operator Overloads */
|
/* Getter Functions */
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
const TriggerEvents& GetTriggerEvents () const noexcept;
|
const TriggerEvents& GetTriggerEvents () const noexcept;
|
||||||
const CollisionEvents& GetCollisionEvents () const noexcept;
|
const CollisionEvents& GetCollisionEvents () const noexcept;
|
||||||
|
const ContactPoints& GetContactPoints () const noexcept;
|
||||||
|
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Member Functions */
|
/* Member Functions */
|
||||||
|
@ -96,7 +108,7 @@ namespace SHADE
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
static void updateCollisionState (bool isColliding, SHCollisionState& state) noexcept;
|
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
|
// 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
|
* Integrate Velocities
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
// Project Headers
|
// Project Headers
|
||||||
#include "Physics/Collision/SHCollisionSpace.h"
|
#include "Physics/Collision/SHCollisionSpace.h"
|
||||||
#include "SHContactManager.h"
|
#include "SHContactManager.h"
|
||||||
|
#include "SHContactSolver.h"
|
||||||
#include "SHRigidBody.h"
|
#include "SHRigidBody.h"
|
||||||
|
|
||||||
|
|
||||||
|
@ -46,7 +47,7 @@ namespace SHADE
|
||||||
|
|
||||||
SHVec3 gravity = SHVec3{ 0.0f, -9.81f, 0.0f };
|
SHVec3 gravity = SHVec3{ 0.0f, -9.81f, 0.0f };
|
||||||
uint16_t numVelocitySolverIterations = 10;
|
uint16_t numVelocitySolverIterations = 10;
|
||||||
uint16_t numPositionSolverIterations = 5;
|
uint16_t numPositionSolverIterations = 5; // Unused until PGS is implemented
|
||||||
bool sleepingEnabled = true;
|
bool sleepingEnabled = true;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -128,6 +129,7 @@ namespace SHADE
|
||||||
|
|
||||||
RigidBodies rigidBodies;
|
RigidBodies rigidBodies;
|
||||||
SHContactManager contactManager;
|
SHContactManager contactManager;
|
||||||
|
SHContactSolver contactSolver;
|
||||||
|
|
||||||
/*---------------------------------------------------------------------------------*/
|
/*---------------------------------------------------------------------------------*/
|
||||||
/* Function Members */
|
/* Function Members */
|
||||||
|
|
|
@ -296,7 +296,18 @@ namespace SHADE
|
||||||
return;
|
return;
|
||||||
|
|
||||||
bodyType = newType;
|
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
|
void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
|
||||||
|
@ -598,6 +609,9 @@ namespace SHADE
|
||||||
|
|
||||||
void SHRigidBody::ComputeWorldData() noexcept
|
void SHRigidBody::ComputeWorldData() noexcept
|
||||||
{
|
{
|
||||||
|
if (bodyType == Type::STATIC)
|
||||||
|
return;
|
||||||
|
|
||||||
const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
|
const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
|
||||||
|
|
||||||
// Compute world inertia
|
// Compute world inertia
|
||||||
|
|
|
@ -40,6 +40,7 @@ namespace SHADE
|
||||||
/*-----------------------------------------------------------------------------------*/
|
/*-----------------------------------------------------------------------------------*/
|
||||||
|
|
||||||
friend class SHPhysicsWorld;
|
friend class SHPhysicsWorld;
|
||||||
|
friend class SHContactSolver;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/*-----------------------------------------------------------------------------------*/
|
/*-----------------------------------------------------------------------------------*/
|
||||||
|
|
Loading…
Reference in New Issue