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

Implemented axis locking constraints

This commit is contained in:
Diren D Bharwani 2022-12-22 03:11:14 +08:00
parent f4f6cb7eae
commit b667e4df87
8 changed files with 249 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
Assets/Scenes/PhysicsSandbox.shade
View File

@ -4,7 +4,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: 0.186280191, y: 4.3224473, 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
@ -19,6 +19,107 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: true
Colliders:
- Is Trigger: false
Type: Sphere
Radius: 1
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts:
- Type: PhysicsTestObj
Enabled: true
forceAmount: 50
torqueAmount: 500
- EID: 1
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Camera Component:
Position: {x: 0, y: 0.5, z: 5}
Pitch: 0
Yaw: 0
Roll: 0
Width: 1920
Height: 1080
Near: 0.00999999978
Far: 10000
Perspective: true
IsActive: true
Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: -2.5, z: 0}
Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: false
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: false
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: true
Colliders:
- Is Trigger: false
Type: Sphere
Radius: 5
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~
- EID: 3
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 5, z: 0}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Dynamic
Auto Mass: false
Mass: 1
Drag: 1
Angular Drag: 1
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: false Freeze Position Y: false
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
@ -41,61 +142,4 @@
- Type: PhysicsTestObj - Type: PhysicsTestObj
Enabled: true Enabled: true
forceAmount: 50 forceAmount: 50
torqueAmount: 500 torqueAmount: 500
- EID: 1
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Camera Component:
Position: {x: 0, y: 0.5, z: 3}
Pitch: 0
Yaw: 0
Roll: 0
Width: 1920
Height: 1080
Near: 0.00999999978
Far: 10000
Perspective: true
IsActive: true
Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 1, z: 0}
Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: false
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: false
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: true
Colliders:
- Is Trigger: false
Type: Sphere
Radius: 1
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~

56
SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************************
* \file SHVelocityState.h
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Interface for a Velocity State for constraint solving.
*
* \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/Dynamics/SHRigidBody.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Type Definitions */
/*-----------------------------------------------------------------------------------*/
struct SH_API SHVelocityState
{
public:
SHVec3 LinearVelocity;
SHVec3 AngularVelocity;
SHVec3 LinearLockFactor;
SHVec3 AngularLockFactor;
/*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */
/*---------------------------------------------------------------------------------*/
SHVelocityState (const SHRigidBody* rigidBody) noexcept
{
LinearVelocity = rigidBody->GetLinearVelocity();
AngularVelocity = rigidBody->GetAngularVelocity();
LinearLockFactor = SHVec3
{
rigidBody->GetFreezePositionX() ? 0.0f : 1.0f
, rigidBody->GetFreezePositionY() ? 0.0f : 1.0f
, rigidBody->GetFreezePositionZ() ? 0.0f : 1.0f
};
AngularLockFactor = SHVec3
{
rigidBody->GetFreezeRotationX() ? 0.0f : 1.0f
, rigidBody->GetFreezeRotationY() ? 0.0f : 1.0f
, rigidBody->GetFreezeRotationZ() ? 0.0f : 1.0f
};
}
};
} // namespace SHADE

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

@ -190,14 +190,14 @@ namespace SHADE
SHRigidBody* bodyA = manifold.bodyA; SHRigidBody* bodyA = manifold.bodyA;
SHRigidBody* bodyB = manifold.bodyB; SHRigidBody* bodyB = manifold.bodyB;
const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second; const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second;
const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second; const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second;
bodyA->SetLinearVelocity(STATE_A.linearVelocity); bodyA->SetLinearVelocity(STATE_A.LinearVelocity);
bodyB->SetLinearVelocity(STATE_B.linearVelocity); bodyB->SetLinearVelocity(STATE_B.LinearVelocity);
bodyA->SetAngularVelocity(STATE_A.angularVelocity); bodyA->SetAngularVelocity(STATE_A.AngularVelocity);
bodyB->SetAngularVelocity(STATE_B.angularVelocity); bodyB->SetAngularVelocity(STATE_B.AngularVelocity);
} }
contactSolver.Reset(); contactSolver.Reset();

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

@ -47,8 +47,8 @@ namespace SHADE
const auto* BODY_B = manifold.bodyB; const auto* BODY_B = manifold.bodyB;
// Add velocities if it doesn't already exist // Add velocities if it doesn't already exist
velocityStates.emplace(key.GetEntityA(), VelocityState{ BODY_A->linearVelocity, BODY_A->angularVelocity }); velocityStates.emplace(key.GetEntityA(), SHVelocityState{ BODY_A });
velocityStates.emplace(key.GetEntityB(), VelocityState{ BODY_B->linearVelocity, BODY_B->angularVelocity }); velocityStates.emplace(key.GetEntityB(), SHVelocityState{ BODY_B });
// Mix friction & restitution // Mix friction & restitution
const float FRICTION_A = SHAPE_A->GetFriction(); const float FRICTION_A = SHAPE_A->GetFriction();
@ -116,10 +116,18 @@ namespace SHADE
{ {
const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB; const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB;
SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; SHVec3 vA = velocityStateA.LinearVelocity;
SHVec3 wA = velocityStateA.AngularVelocity;
SHVec3 vB = velocityStateB.LinearVelocity;
SHVec3 wB = velocityStateB.AngularVelocity;
const SHVec3& LINEAR_LOCK_A = velocityStateA.LinearLockFactor;
const SHVec3& ANGULAR_LOCK_A = velocityStateA.AngularLockFactor;
const SHVec3& LINEAR_LOCK_B = velocityStateB.LinearLockFactor;
const SHVec3& ANGULAR_LOCK_B = velocityStateB.AngularLockFactor;
for (uint32_t i = 0; i < constraint.numContacts; ++i) for (uint32_t i = 0; i < constraint.numContacts; ++i)
{ {
@ -172,19 +180,6 @@ namespace SHADE
contact.tangentMass[j] = 1.0f / contact.tangentMass[j]; contact.tangentMass[j] = 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 // Calculate bias per contact
/* /*
* error bias = baumgarte factor / dt * penetration * error bias = baumgarte factor / dt * penetration
@ -199,12 +194,25 @@ namespace SHADE
const float RESTITUTION_BIAS = -constraint.restitution * RV_N; const float RESTITUTION_BIAS = -constraint.restitution * RV_N;
contact.bias = ERROR_BIAS + RESTITUTION_BIAS; contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
// 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 * LINEAR_LOCK_A;
wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, impulse) * ANGULAR_LOCK_A;
vB += impulse * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B;
} }
velocityStates[key.GetEntityA()].linearVelocity = vA; velocityStateA.LinearVelocity = vA;
velocityStates[key.GetEntityA()].angularVelocity = wA; velocityStateA.AngularVelocity = wA;
velocityStates[key.GetEntityB()].linearVelocity = vB; velocityStateB.LinearVelocity = vB;
velocityStates[key.GetEntityB()].angularVelocity = wB; velocityStateB.AngularVelocity = wB;
} }
} }
@ -212,10 +220,18 @@ namespace SHADE
{ {
for (auto& [key, constraint] : contactConstraints) for (auto& [key, constraint] : contactConstraints)
{ {
SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; SHVec3 vA = velocityStateA.LinearVelocity;
SHVec3 wA = velocityStateA.AngularVelocity;
SHVec3 vB = velocityStateB.LinearVelocity;
SHVec3 wB = velocityStateB.AngularVelocity;
const SHVec3& LINEAR_LOCK_A = velocityStateA.LinearLockFactor;
const SHVec3& ANGULAR_LOCK_A = velocityStateA.AngularLockFactor;
const SHVec3& LINEAR_LOCK_B = velocityStateB.LinearLockFactor;
const SHVec3& ANGULAR_LOCK_B = velocityStateB.AngularLockFactor;
for (uint32_t i = 0; i < constraint.numContacts; ++i) for (uint32_t i = 0; i < constraint.numContacts; ++i)
{ {
@ -245,11 +261,11 @@ namespace SHADE
const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j]; const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j];
// Apply impulses // Apply impulses
vA -= TANGENT_IMPULSE * constraint.invMassA; vA -= TANGENT_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE); wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE) * ANGULAR_LOCK_A;
vB += TANGENT_IMPULSE * constraint.invMassB; vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE); wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
} }
// Solve normal impulse // Solve normal impulse
@ -271,17 +287,17 @@ namespace SHADE
const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal; const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
// Apply impulses // Apply impulses
vA -= NORMAL_IMPULSE * constraint.invMassA; vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE); wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
vB += NORMAL_IMPULSE * constraint.invMassB; vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE); wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
} }
velocityStates[key.GetEntityA()].linearVelocity = vA; velocityStateA.LinearVelocity = vA;
velocityStates[key.GetEntityA()].angularVelocity = wA; velocityStateA.AngularVelocity = wA;
velocityStates[key.GetEntityB()].linearVelocity = vB; velocityStateB.LinearVelocity = vB;
velocityStates[key.GetEntityB()].angularVelocity = wB; velocityStateB.AngularVelocity = wB;
} }
} }

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

@ -13,6 +13,7 @@
// Project Headers // Project Headers
#include "Constraints/SHContactConstraint.h" #include "Constraints/SHContactConstraint.h"
#include "Constraints/SHVelocityState.h"
namespace SHADE namespace SHADE
{ {
@ -31,15 +32,7 @@ namespace SHADE
/* Type Definitions */ /* Type Definitions */
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
struct VelocityState using VelocityStates = std::unordered_map<EntityID, SHVelocityState>;
{
// Velocities
SHVec3 linearVelocity;
SHVec3 angularVelocity;
};
using VelocityStates = std::unordered_map<EntityID, VelocityState>;
using ContactConstraints = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>; using ContactConstraints = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/

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

@ -143,29 +143,13 @@ namespace SHADE
// Apply drag (exponentially applied) // Apply drag (exponentially applied)
rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag); rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag); rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag);
rigidBody.constrainLinearVelocities();
rigidBody.constrainAngularVelocities();
} }
void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
{ {
static const auto ENFORCE_CONSTRAINED_VELOCITIES = [](SHRigidBody& rigidBody)
{
// Enforce linear constraints
rigidBody.linearVelocity = SHVec3
{
rigidBody.GetFreezePositionX() ? 0.0f : rigidBody.linearVelocity.x
, rigidBody.GetFreezePositionY() ? 0.0f : rigidBody.linearVelocity.y
, rigidBody.GetFreezePositionZ() ? 0.0f : rigidBody.linearVelocity.z
};
// Enforce angular constraints
rigidBody.angularVelocity = SHVec3
{
rigidBody.GetFreezeRotationX() ? 0.0f : rigidBody.angularVelocity.x
, rigidBody.GetFreezeRotationY() ? 0.0f : rigidBody.angularVelocity.y
, rigidBody.GetFreezeRotationZ() ? 0.0f : rigidBody.angularVelocity.z
};
};
// Always reset movement flag // Always reset movement flag
rigidBody.motionState.hasMoved = false; rigidBody.motionState.hasMoved = false;
@ -178,7 +162,8 @@ namespace SHADE
// Both dynamic and kinematic can sleep when their velocities are under the thresholds. // Both dynamic and kinematic can sleep when their velocities are under the thresholds.
else if (!rigidBody.IsSleeping()) else if (!rigidBody.IsSleeping())
{ {
ENFORCE_CONSTRAINED_VELOCITIES(rigidBody); rigidBody.constrainLinearVelocities();
rigidBody.constrainAngularVelocities();
rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt); rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt); rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt);

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

@ -200,6 +200,8 @@ namespace SHADE
const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
{ {
// Check if linear velocity needs to be constrained
return linearVelocity; return linearVelocity;
} }
@ -378,6 +380,7 @@ namespace SHADE
} }
linearVelocity = newLinearVelocity; linearVelocity = newLinearVelocity;
constrainLinearVelocities();
} }
void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
@ -389,6 +392,7 @@ namespace SHADE
} }
angularVelocity = newAngularVelocity; angularVelocity = newAngularVelocity;
constrainAngularVelocities();
} }
void SHRigidBody::SetIsActive(bool isActive) noexcept void SHRigidBody::SetIsActive(bool isActive) noexcept
@ -676,7 +680,20 @@ namespace SHADE
const auto* FIRST_SHAPE = collider->GetCollisionShape(0); const auto* FIRST_SHAPE = collider->GetCollisionShape(0);
localInvInertia = SHMatrix::Inverse(FIRST_SHAPE->GetInertiaTensor(1.0f / invMass)); localInvInertia = SHMatrix::Inverse(FIRST_SHAPE->GetInertiaTensor(1.0f / invMass));
} }
}
void SHRigidBody::constrainLinearVelocities() noexcept
{
linearVelocity.x = GetFreezePositionX() ? 0.0f : linearVelocity.x;
linearVelocity.y = GetFreezePositionY() ? 0.0f : linearVelocity.y;
linearVelocity.z = GetFreezePositionZ() ? 0.0f : linearVelocity.z;
}
void SHRigidBody::constrainAngularVelocities() noexcept
{
angularVelocity.x = GetFreezeRotationX() ? 0.0f : angularVelocity.x;
angularVelocity.y = GetFreezeRotationY() ? 0.0f : angularVelocity.y;
angularVelocity.z = GetFreezeRotationZ() ? 0.0f : angularVelocity.z;
} }
} // namespace SHADE } // namespace SHADE

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

@ -249,7 +249,7 @@ namespace SHADE
SHVec3 linearVelocity; SHVec3 linearVelocity;
SHVec3 angularVelocity; SHVec3 angularVelocity;
// aZ aY aX pZ pY pX 0 0 0 0 inIsland autoMass enableGravity enableSleeping sleeping active // aZ aY aX rotLockActive pZ pY pX posLockActive 0 0 inIsland autoMass enableGravity enableSleeping sleeping active
uint16_t flags; uint16_t flags;
SHMotionState motionState; SHMotionState motionState;
@ -262,6 +262,9 @@ namespace SHADE
void computeMass () noexcept; void computeMass () noexcept;
void computeInertiaTensor () noexcept; void computeInertiaTensor () noexcept;
void computeMassAndInertiaTensor() noexcept; void computeMassAndInertiaTensor() noexcept;
void constrainLinearVelocities () noexcept;
void constrainAngularVelocities () noexcept;
}; };