From b667e4df872316bebb69afb1a117f43e8f5db47b Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Thu, 22 Dec 2022 03:11:14 +0800 Subject: [PATCH] Implemented axis locking constraints --- Assets/Scenes/PhysicsSandbox.shade | 162 +++++++++++------- .../Dynamics/Constraints/SHVelocityState.h | 56 ++++++ .../src/Physics/Dynamics/SHContactManager.cpp | 12 +- .../src/Physics/Dynamics/SHContactSolver.cpp | 94 +++++----- .../src/Physics/Dynamics/SHContactSolver.h | 11 +- .../src/Physics/Dynamics/SHPhysicsWorld.cpp | 25 +-- .../src/Physics/Dynamics/SHRigidBody.cpp | 19 +- .../src/Physics/Dynamics/SHRigidBody.h | 5 +- 8 files changed, 249 insertions(+), 135 deletions(-) create mode 100644 SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 90cba8d0..1438b558 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -4,7 +4,7 @@ NumberOfChildren: 0 Components: 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} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -19,6 +19,107 @@ Interpolate: true Sleeping Enabled: true 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 Z: false Freeze Rotation X: false @@ -41,61 +142,4 @@ - 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: 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: ~ \ No newline at end of file + torqueAmount: 500 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h new file mode 100644 index 00000000..6cbb6efb --- /dev/null +++ b/SHADE_Engine/src/Physics/Dynamics/Constraints/SHVelocityState.h @@ -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 diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 3b80b3b3..83047635 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -190,14 +190,14 @@ namespace SHADE SHRigidBody* bodyA = manifold.bodyA; SHRigidBody* bodyB = manifold.bodyB; - const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second; - const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second; + const auto& STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second; + const auto& STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second; - bodyA->SetLinearVelocity(STATE_A.linearVelocity); - bodyB->SetLinearVelocity(STATE_B.linearVelocity); + bodyA->SetLinearVelocity(STATE_A.LinearVelocity); + bodyB->SetLinearVelocity(STATE_B.LinearVelocity); - bodyA->SetAngularVelocity(STATE_A.angularVelocity); - bodyB->SetAngularVelocity(STATE_B.angularVelocity); + bodyA->SetAngularVelocity(STATE_A.AngularVelocity); + bodyB->SetAngularVelocity(STATE_B.AngularVelocity); } contactSolver.Reset(); diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index c14c9eb5..ed4c9aa8 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -47,8 +47,8 @@ namespace SHADE const auto* BODY_B = manifold.bodyB; // Add velocities if it doesn't already exist - velocityStates.emplace(key.GetEntityA(), VelocityState{ BODY_A->linearVelocity, BODY_A->angularVelocity }); - velocityStates.emplace(key.GetEntityB(), VelocityState{ BODY_B->linearVelocity, BODY_B->angularVelocity }); + velocityStates.emplace(key.GetEntityA(), SHVelocityState{ BODY_A }); + velocityStates.emplace(key.GetEntityB(), SHVelocityState{ BODY_B }); // Mix friction & restitution const float FRICTION_A = SHAPE_A->GetFriction(); @@ -116,10 +116,18 @@ namespace SHADE { const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB; - SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; - SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; - SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity; - SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; + SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second; + SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second; + + 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) { @@ -172,19 +180,6 @@ namespace SHADE 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 /* * error bias = baumgarte factor / dt * penetration @@ -199,12 +194,25 @@ namespace SHADE const float RESTITUTION_BIAS = -constraint.restitution * RV_N; 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; - velocityStates[key.GetEntityA()].angularVelocity = wA; - velocityStates[key.GetEntityB()].linearVelocity = vB; - velocityStates[key.GetEntityB()].angularVelocity = wB; + velocityStateA.LinearVelocity = vA; + velocityStateA.AngularVelocity = wA; + velocityStateB.LinearVelocity = vB; + velocityStateB.AngularVelocity = wB; } } @@ -212,10 +220,18 @@ namespace SHADE { for (auto& [key, constraint] : contactConstraints) { - SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity; - SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity; - SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity; - SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity; + SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second; + SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second; + + 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) { @@ -245,11 +261,11 @@ namespace SHADE const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j]; // Apply impulses - vA -= TANGENT_IMPULSE * constraint.invMassA; - wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE); + vA -= TANGENT_IMPULSE * constraint.invMassA * LINEAR_LOCK_A; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE) * ANGULAR_LOCK_A; - vB += TANGENT_IMPULSE * constraint.invMassB; - wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE); + vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B; } // Solve normal impulse @@ -271,17 +287,17 @@ namespace SHADE const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal; // Apply impulses - vA -= NORMAL_IMPULSE * constraint.invMassA; - wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE); + vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A; - vB += NORMAL_IMPULSE * constraint.invMassB; - wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE); + vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B; } - velocityStates[key.GetEntityA()].linearVelocity = vA; - velocityStates[key.GetEntityA()].angularVelocity = wA; - velocityStates[key.GetEntityB()].linearVelocity = vB; - velocityStates[key.GetEntityB()].angularVelocity = wB; + velocityStateA.LinearVelocity = vA; + velocityStateA.AngularVelocity = wA; + velocityStateB.LinearVelocity = vB; + velocityStateB.AngularVelocity = wB; } } diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h index 7a4f2d47..56955f74 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h @@ -13,6 +13,7 @@ // Project Headers #include "Constraints/SHContactConstraint.h" +#include "Constraints/SHVelocityState.h" namespace SHADE { @@ -31,15 +32,7 @@ namespace SHADE /* Type Definitions */ /*---------------------------------------------------------------------------------*/ - struct VelocityState - { - // Velocities - - SHVec3 linearVelocity; - SHVec3 angularVelocity; - }; - - using VelocityStates = std::unordered_map; + using VelocityStates = std::unordered_map; using ContactConstraints = std::unordered_map; /*---------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp index dfa3f482..8451bd5b 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp @@ -143,29 +143,13 @@ namespace SHADE // Apply drag (exponentially applied) rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag); rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag); + + rigidBody.constrainLinearVelocities(); + rigidBody.constrainAngularVelocities(); } 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 rigidBody.motionState.hasMoved = false; @@ -178,7 +162,8 @@ namespace SHADE // Both dynamic and kinematic can sleep when their velocities are under the thresholds. else if (!rigidBody.IsSleeping()) { - ENFORCE_CONSTRAINED_VELOCITIES(rigidBody); + rigidBody.constrainLinearVelocities(); + rigidBody.constrainAngularVelocities(); rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt); rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt); diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp index b724321b..3e24fa27 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp @@ -200,6 +200,8 @@ namespace SHADE const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept { + // Check if linear velocity needs to be constrained + return linearVelocity; } @@ -378,6 +380,7 @@ namespace SHADE } linearVelocity = newLinearVelocity; + constrainLinearVelocities(); } void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept @@ -389,6 +392,7 @@ namespace SHADE } angularVelocity = newAngularVelocity; + constrainAngularVelocities(); } void SHRigidBody::SetIsActive(bool isActive) noexcept @@ -676,7 +680,20 @@ namespace SHADE const auto* FIRST_SHAPE = collider->GetCollisionShape(0); 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 \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h index 606895ee..34424a1c 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h @@ -249,7 +249,7 @@ namespace SHADE SHVec3 linearVelocity; 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; SHMotionState motionState; @@ -262,6 +262,9 @@ namespace SHADE void computeMass () noexcept; void computeInertiaTensor () noexcept; void computeMassAndInertiaTensor() noexcept; + + void constrainLinearVelocities () noexcept; + void constrainAngularVelocities () noexcept; };