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

160
Assets/Scenes/PhysicsSandbox.shade
View File

@ -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
@ -42,60 +143,3 @@
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: ~

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

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

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

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

@ -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<EntityID, VelocityState>;
using VelocityStates = std::unordered_map<EntityID, SHVelocityState>;
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)
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);

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

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

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

@ -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;
@ -263,6 +263,9 @@ namespace SHADE
void computeInertiaTensor () noexcept;
void computeMassAndInertiaTensor() noexcept;
void constrainLinearVelocities () noexcept;
void constrainAngularVelocities () noexcept;
};
} // namespace SHADE