Improved stability of sphere vs polyhedron
Still trying to solve the polyhedron vs polyhedron issue
This commit is contained in:
Diren D Bharwani
parent
6663156405
commit
10ad5647df
|
|
@ -179,7 +179,13 @@ namespace SHADE
|
|||
|
||||
bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||
{
|
||||
return SphereVsConvex(manifold, B, A);
|
||||
const bool RESULT = SphereVsConvex(manifold, B, A);
|
||||
if (RESULT)
|
||||
{
|
||||
// Flip the normal
|
||||
manifold.normal = -manifold.normal;
|
||||
}
|
||||
return RESULT;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -238,6 +238,7 @@ namespace SHADE
|
|||
|
||||
void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept
|
||||
{
|
||||
static const float SQRT_ONE_OVER_THREE = std::sqrtf(1.0f / 3.0f);
|
||||
// Early out since exiting a collision does not require an update beyond updating the state
|
||||
if (manifold.state == SHCollisionState::EXIT)
|
||||
return;
|
||||
|
|
@ -250,13 +251,13 @@ namespace SHADE
|
|||
const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1];
|
||||
|
||||
// Compute tangents
|
||||
if (std::fabs(NORMAL.x) >= SHMath::EULER_CONSTANT)
|
||||
if (std::fabs(NORMAL.x) >= SQRT_ONE_OVER_THREE)
|
||||
tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f };
|
||||
else
|
||||
tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y };
|
||||
|
||||
tangent0 = SHVec3::Normalise(tangent0);
|
||||
tangent1 = SHVec3::Cross(NORMAL, tangent0);
|
||||
tangent1 = SHVec3::Cross(tangent0, NORMAL);
|
||||
|
||||
// Accumulate impulses
|
||||
for (uint32_t i = 0; i < manifold.numContacts; ++i)
|
||||
|
|
|
|||
|
|
@ -147,9 +147,9 @@ namespace SHADE
|
|||
* | 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 |
|
||||
* [ u v w ]| ix * x |
|
||||
* | iy * y | = [ ix * x * u + iy * y * v + iz * z * w ]
|
||||
* | iz * z |
|
||||
*
|
||||
* Simplified:
|
||||
*
|
||||
|
|
@ -171,12 +171,12 @@ namespace SHADE
|
|||
|
||||
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]);
|
||||
const SHVec3 RA_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rA);
|
||||
const SHVec3 RB_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rB);
|
||||
|
||||
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] += SHVec3::Dot(constraint.invInertiaA * RA_CROSS_T, RA_CROSS_T);
|
||||
contact.tangentMass[j] += SHVec3::Dot(constraint.invInertiaB * RB_CROSS_T, RB_CROSS_T);
|
||||
|
||||
contact.tangentMass[j] = 1.0f / contact.tangentMass[j];
|
||||
}
|
||||
|
|
@ -187,15 +187,8 @@ namespace SHADE
|
|||
* 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 = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
|
||||
const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
|
||||
|
||||
contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
|
||||
|
||||
|
||||
// Warm starting
|
||||
// Compute impulses
|
||||
SHVec3 impulse = constraint.normal * contact.normalImpulse;
|
||||
|
|
@ -208,6 +201,13 @@ namespace SHADE
|
|||
|
||||
vB += impulse * constraint.invMassB * LINEAR_LOCK_B;
|
||||
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B;
|
||||
|
||||
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 RESTITUTION_BIAS = RV_N > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
|
||||
contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
|
||||
}
|
||||
|
||||
velocityStateA.LinearVelocity = vA;
|
||||
|
|
|
|||
|
|
@ -171,12 +171,12 @@ namespace SHADE
|
|||
return angularDrag;
|
||||
}
|
||||
|
||||
const SHMatrix& SHRigidBody::GetLocalInvInertia() const noexcept
|
||||
const SHVec3& SHRigidBody::GetLocalInvInertia() const noexcept
|
||||
{
|
||||
return localInvInertia;
|
||||
}
|
||||
|
||||
const SHMatrix& SHRigidBody::GetWorldInvInertia() const noexcept
|
||||
const SHVec3& SHRigidBody::GetWorldInvInertia() const noexcept
|
||||
{
|
||||
return worldInvInertia;
|
||||
}
|
||||
|
|
@ -311,13 +311,13 @@ namespace SHADE
|
|||
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;
|
||||
localInvInertia = SHVec3::Zero;
|
||||
worldInvInertia = SHVec3::Zero;
|
||||
}
|
||||
else
|
||||
{
|
||||
invMass = 1.0f;
|
||||
localInvInertia = SHMatrix::Identity;
|
||||
localInvInertia = SHVec3 { 1.0f };
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -550,7 +550,7 @@ namespace SHADE
|
|||
// Reset angular velocity along X-axis
|
||||
angularVelocity.x = 0.0f;
|
||||
// Set inertia tensor on the x-axis to 0
|
||||
localInvInertia.m[0][0] = 0.0f;
|
||||
localInvInertia.x = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -570,7 +570,7 @@ namespace SHADE
|
|||
// Reset angular velocity along Y-axis
|
||||
angularVelocity.y = 0.0f;
|
||||
// Set inertia tensor on the y-axis to 0
|
||||
localInvInertia.m[1][1] = 0.0f;
|
||||
localInvInertia.y = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -590,7 +590,7 @@ namespace SHADE
|
|||
// Reset angular velocity along Z-axis
|
||||
angularVelocity.z = 0.0f;
|
||||
// Set inertia tensor on the z-axis to 0
|
||||
localInvInertia.m[2][2] = 0.0f;
|
||||
localInvInertia.z = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -609,7 +609,7 @@ namespace SHADE
|
|||
return;
|
||||
|
||||
accumulatedForce += force;
|
||||
accumulatedTorque += SHVec3::Cross(pos, force);
|
||||
angularVelocity += worldInvInertia * SHVec3::Cross(pos, force);
|
||||
}
|
||||
|
||||
void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
|
||||
|
|
@ -640,25 +640,48 @@ namespace SHADE
|
|||
if (bodyType == Type::STATIC)
|
||||
return;
|
||||
|
||||
const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
|
||||
const SHMatrix R = SHMatrix::Rotate(motionState.orientation);
|
||||
const SHMatrix RT = SHMatrix::Transpose(R);
|
||||
|
||||
// Compute world inertia
|
||||
worldInvInertia = SHMatrix::Transpose(ROTATION) * localInvInertia * ROTATION;
|
||||
/*
|
||||
Compute world inertia as a Vector
|
||||
|
||||
| a b c || x | | ax + by + cz |
|
||||
| d e f || y | = | dx + ey + fz |
|
||||
| g h i || z | | gx + hy + iz |
|
||||
*/
|
||||
|
||||
SHMatrix tmp = SHMatrix
|
||||
{
|
||||
SHVec4 { localInvInertia.x, 0.0f, 0.0f, 0.0f }
|
||||
, SHVec4 { 0.0f, localInvInertia.y, 0.0f, 0.0f }
|
||||
, SHVec4 { 0.0f, 0.0f, localInvInertia.z, 0.0f }
|
||||
, SHVec4 { 0.0f, 0.0f, 0.0, 1.0f }
|
||||
};
|
||||
|
||||
tmp *= RT;
|
||||
|
||||
worldInvInertia = SHVec3
|
||||
{
|
||||
R.m[0][0] * tmp.m[0][0] + RT.m[0][1] * tmp.m[1][1] + RT.m[0][2] * tmp.m[2][2]
|
||||
, R.m[1][0] * tmp.m[0][0] + RT.m[1][1] * tmp.m[1][1] + RT.m[1][2] * tmp.m[2][2]
|
||||
, R.m[2][0] * tmp.m[0][0] + RT.m[2][1] * tmp.m[1][1] + RT.m[2][2] * tmp.m[2][2]
|
||||
};
|
||||
|
||||
// Compute world centroid
|
||||
worldCentroid = (ROTATION * localCentroid) + motionState.position;
|
||||
worldCentroid = (R * localCentroid) + motionState.position;
|
||||
}
|
||||
|
||||
void SHRigidBody::ComputeMassData() noexcept
|
||||
{
|
||||
// Reset centroid
|
||||
localCentroid = SHVec3::Zero;
|
||||
localInvInertia = SHMatrix::Identity;
|
||||
localInvInertia = SHVec3{ 1.0f };
|
||||
|
||||
// In the instance the body is a particle
|
||||
if (!collider || collider->GetCollisionShapes().empty())
|
||||
{
|
||||
localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = invMass;
|
||||
localInvInertia = SHVec3{ invMass };
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -736,11 +759,9 @@ namespace SHADE
|
|||
}
|
||||
|
||||
// Set diagonals then invert
|
||||
localInvInertia.m[0][0] = J.m[0][0];
|
||||
localInvInertia.m[1][1] = J.m[1][1];
|
||||
localInvInertia.m[2][2] = J.m[2][2];
|
||||
|
||||
localInvInertia = SHMatrix::Inverse(localInvInertia);
|
||||
localInvInertia.x = totalMass == 0.0f ? 0.0f : 1.0f / J.m[0][0];
|
||||
localInvInertia.y = totalMass == 0.0f ? 0.0f : 1.0f / J.m[1][1];
|
||||
localInvInertia.z = totalMass == 0.0f ? 0.0f : 1.0f / J.m[2][2];
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -73,41 +73,41 @@ namespace SHADE
|
|||
/* Getter Functions */
|
||||
/*-----------------------------------------------------------------------------------*/
|
||||
|
||||
[[nodiscard]] Type GetType () const noexcept;
|
||||
[[nodiscard]] Type GetType () const noexcept;
|
||||
|
||||
[[nodiscard]] float GetGravityScale () const noexcept;
|
||||
[[nodiscard]] float GetGravityScale () const noexcept;
|
||||
|
||||
[[nodiscard]] float GetMass () const noexcept;
|
||||
[[nodiscard]] float GetLinearDrag () const noexcept;
|
||||
[[nodiscard]] float GetAngularDrag () const noexcept;
|
||||
[[nodiscard]] float GetMass () const noexcept;
|
||||
[[nodiscard]] float GetLinearDrag () const noexcept;
|
||||
[[nodiscard]] float GetAngularDrag () const noexcept;
|
||||
|
||||
[[nodiscard]] const SHMatrix& GetLocalInvInertia () const noexcept;
|
||||
[[nodiscard]] const SHMatrix& GetWorldInvInertia () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetLocalInvInertia () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetWorldInvInertia () const noexcept;
|
||||
|
||||
[[nodiscard]] const SHVec3& GetLocalCentroid () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetWorldCentroid () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetLocalCentroid () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetWorldCentroid () const noexcept;
|
||||
|
||||
[[nodiscard]] const SHVec3& GetForce () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetTorque () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetLinearVelocity () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetAngularVelocity () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetForce () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetTorque () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetLinearVelocity () const noexcept;
|
||||
[[nodiscard]] const SHVec3& GetAngularVelocity () const noexcept;
|
||||
|
||||
// Flags
|
||||
|
||||
[[nodiscard]] bool IsActive () const noexcept;
|
||||
[[nodiscard]] bool IsSleeping () const noexcept;
|
||||
[[nodiscard]] bool IsSleepingEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsGravityEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsAutoMassEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsTriggerInMassData () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionX () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionY () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionZ () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationX () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationY () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationZ () const noexcept;
|
||||
[[nodiscard]] bool IsActive () const noexcept;
|
||||
[[nodiscard]] bool IsSleeping () const noexcept;
|
||||
[[nodiscard]] bool IsSleepingEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsGravityEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsAutoMassEnabled () const noexcept;
|
||||
[[nodiscard]] bool IsTriggerInMassData () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionX () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionY () const noexcept;
|
||||
[[nodiscard]] bool GetFreezePositionZ () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationX () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationY () const noexcept;
|
||||
[[nodiscard]] bool GetFreezeRotationZ () const noexcept;
|
||||
|
||||
[[nodiscard]] SHMotionState& GetMotionState () noexcept;
|
||||
[[nodiscard]] SHMotionState& GetMotionState () noexcept;
|
||||
|
||||
/*-----------------------------------------------------------------------------------*/
|
||||
/* Setter Functions */
|
||||
|
|
@ -228,28 +228,28 @@ namespace SHADE
|
|||
/*-----------------------------------------------------------------------------------*/
|
||||
|
||||
// The entityID here is only meant for linking with the actual component in the engine.
|
||||
EntityID entityID;
|
||||
SHCollider* collider;
|
||||
EntityID entityID;
|
||||
SHCollider* collider;
|
||||
|
||||
Type bodyType;
|
||||
Type bodyType;
|
||||
|
||||
float gravityScale;
|
||||
float gravityScale;
|
||||
|
||||
float invMass;
|
||||
float linearDrag;
|
||||
float angularDrag;
|
||||
float invMass;
|
||||
float linearDrag;
|
||||
float angularDrag;
|
||||
|
||||
SHMatrix localInvInertia;
|
||||
SHMatrix worldInvInertia;
|
||||
SHVec3 localInvInertia; // Only store the diagonals
|
||||
SHVec3 worldInvInertia; // Only store the diagonals
|
||||
|
||||
SHVec3 localCentroid;
|
||||
SHVec3 worldCentroid;
|
||||
SHVec3 localCentroid;
|
||||
SHVec3 worldCentroid;
|
||||
|
||||
SHVec3 accumulatedForce;
|
||||
SHVec3 accumulatedTorque;
|
||||
SHVec3 accumulatedForce;
|
||||
SHVec3 accumulatedTorque;
|
||||
|
||||
SHVec3 linearVelocity;
|
||||
SHVec3 angularVelocity;
|
||||
SHVec3 linearVelocity;
|
||||
SHVec3 angularVelocity;
|
||||
|
||||
// aZ aY aX pZ pY pX 0 0 0 addTriggersToMassData inIsland autoMass enableGravity enableSleeping sleeping active
|
||||
uint16_t flags;
|
||||
|
|
|
|||
Loading…
Reference in New Issue