From e293094b6d544937adc4d4aecb7ff9fd739a9512 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sat, 4 Mar 2023 17:31:50 +0800 Subject: [PATCH] Another attempt to fix invalid error bias --- Assets/Scenes/SS_Playground.shade | 269 +++++++++++++++++- Assets/Scripts/Tests/MovementTest.cs | 9 +- .../Narrowphase/SHConvexVsConvex.cpp | 8 +- .../src/Physics/Dynamics/SHContactSolver.cpp | 64 +++-- 4 files changed, 307 insertions(+), 43 deletions(-) diff --git a/Assets/Scenes/SS_Playground.shade b/Assets/Scenes/SS_Playground.shade index 7aae078f..7e8c8c8f 100644 --- a/Assets/Scenes/SS_Playground.shade +++ b/Assets/Scenes/SS_Playground.shade @@ -4,8 +4,8 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 0, y: 2, z: 5} - Pitch: 0 + Position: {x: 0, y: 10, z: 0} + Pitch: -90 Yaw: 0 Roll: 0 Width: 1920 @@ -21,9 +21,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0, y: 0, z: 0} - Rotate: {x: -0, y: 0, z: -0} - Scale: {x: 5, y: 0.999998808, z: 5} + Translate: {x: -5.3379097, y: 5, z: -0.258950353} + Rotate: {x: -0, y: 0, z: 1.57079601} + Scale: {x: 9.99821568, y: 0.999887228, z: 10} IsActive: true Collider Component: Colliders: @@ -44,7 +44,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.141888797, y: 5, z: 0} + Translate: {x: 0, y: 5, z: 0} Rotate: {x: -0, y: 0, z: 0} Scale: {x: 0.999999702, y: 0.999999702, z: 1} IsActive: true @@ -80,6 +80,259 @@ Scripts: - Type: MovementTest Enabled: true - forceAmount: 25 + forceAmount: 50 torqueAmount: 50 - rayDistance: 1 \ No newline at end of file + rayDistance: 1 + rayYOffset: -0.100000001 +- EID: 131077 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -0.00715482235, y: 0, z: 0} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 10, y: 0.999998808, z: 10} + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 8 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -0.0296445489, y: 5.5, z: 5.51512575} + Rotate: {x: 1.57079601, y: 0, z: -0} + Scale: {x: 9.99866009, y: 0.999816775, z: 9.9984293} + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 65543 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 5.43979311, y: 5, z: 0.0634104013} + Rotate: {x: -0, y: 0, z: 1.57079601} + Scale: {x: 9.99855232, y: 0.999908268, z: 10} + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 131074 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -0.0296445489, y: 5.5, z: -5.14873552} + Rotate: {x: 1.57079601, y: 0, z: -0} + Scale: {x: 9.99866009, y: 0.999806404, z: 9.99826336} + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 131076 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: -2.60022187, y: 5, z: -3.02165651} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 0.999999702, y: 0.999999702, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + 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 + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 65542 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 1.58913052, y: 4.99788427, z: 1.11987209} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 0.999999702, y: 0.999999702, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + 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 + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 9 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 2.48236442, y: 5, z: -3.18853354} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 0.999999702, y: 0.999999702, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + 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 + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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: ~ +- EID: 10 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: 5, z: 2.37419057} + Rotate: {x: -0, y: 0, z: -0} + Scale: {x: 0.999999702, y: 0.999999702, z: 1} + IsActive: true + RigidBody Component: + Type: Dynamic + Auto Mass: false + Mass: 1 + Drag: 0.00999999978 + Angular Drag: 0.00999999978 + 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 + Freeze Rotation Y: false + Freeze Rotation Z: false + IsActive: true + Collider Component: + Colliders: + - Is Trigger: false + Collision Tag: 0 + Type: Box + Half Extents: {x: 1, y: 1, z: 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 diff --git a/Assets/Scripts/Tests/MovementTest.cs b/Assets/Scripts/Tests/MovementTest.cs index c1c18ff4..d9d09a32 100644 --- a/Assets/Scripts/Tests/MovementTest.cs +++ b/Assets/Scripts/Tests/MovementTest.cs @@ -28,6 +28,7 @@ public class MovementTest : Script public float forceAmount = 50.0f; public float torqueAmount = 100.0f; public float rayDistance = 1.0f; + public float rayYOffset = -0.1f; protected override void awake() { @@ -50,9 +51,10 @@ public class MovementTest : Script } Vector3 dirNor = tf.Forward; - List rayList1 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535); - List rayList2 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535); - List rayList3 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, dirNor), rayDistance, false, (ushort)65535); + Vector3 offset = new Vector3(0.0f, rayYOffset, 0.0f); + List rayList1 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535); + List rayList2 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535); + List rayList3 = Physics.ColliderRaycast(GameObject, new Ray(offset, dirNor), rayDistance, false, (ushort)65535); } protected override void fixedUpdate() @@ -62,7 +64,6 @@ public class MovementTest : Script // Apply force in the direction the box is facing // AKA Transform's forward body.AddForce(tf.Forward * forceAmount); - Debug.Log($"Forward: <{tf.Forward.x}, {tf.Forward.y}, {tf.Forward.z}"); move = false; } diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 8c7ffc5e..d3b4e59b 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -67,8 +67,8 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - static constexpr float ABSOLUTE_TOLERANCE = 0.01f; - static constexpr float RELATIVE_TOLERANCE = 0.95f; + static constexpr float ABSOLUTE_TOLERANCE = 0.0005f; + static constexpr float RELATIVE_TOLERANCE = 1.002f; const SHConvexPolyhedron& POLY_A = reinterpret_cast(A); const SHConvexPolyhedron& POLY_B = reinterpret_cast(B); @@ -130,7 +130,7 @@ namespace SHADE const SHConvexPolyhedron* incidentPoly = nullptr; SHCollisionUtils::FaceQuery minFaceQuery; - if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) + if (FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE + ABSOLUTE_TOLERANCE) { minFaceQuery = FACE_QUERY_A; referencePoly = &POLY_A; @@ -157,7 +157,7 @@ namespace SHADE // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on // each edge and use that as the contact point. - if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE) + if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE + ABSOLUTE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE) { const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index cd9668d1..87276c82 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -18,6 +18,14 @@ #include "Physics/SHPhysicsConstants.h" #include "Physics/Collision/Narrowphase/SHCollisionUtils.h" +bool IsInfinity(float value) +{ + const float MAX_VAL = std::numeric_limits::max(); + const float MIN_VAL = -MAX_VAL; + + return !(MIN_VAL <= value && value <= MAX_VAL); +} + namespace SHADE { /*-----------------------------------------------------------------------------------*/ @@ -218,6 +226,10 @@ namespace SHADE if (std::fabs(contact.penetration) > SHPHYSICS_LINEAR_SLOP) errorBias = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP); + // TODO: This should never occur. Move this check into the ConvexVSConvex to find the error. + if (IsInfinity(errorBias)) + errorBias = 0.0f; + // Warm starting // Compute impulses SHVec3 impulse = constraint.normal * contact.normalImpulse; @@ -249,8 +261,6 @@ namespace SHADE void SHContactSolver::solve() noexcept { - - for (auto& [key, constraint] : contactConstraints) { SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second; @@ -275,31 +285,6 @@ namespace SHADE SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB); SHVec3 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 * LINEAR_LOCK_A; - wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A; - - vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; - wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B; - - // Solve normal impulse - // Re-compute relative velocity - velocityA = vA + SHVec3::Cross(wA, contact.rA); - velocityB = vB + SHVec3::Cross(wB, contact.rB); - relativeVelocity = velocityB - velocityA; - // Solve tangent impulse for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) { @@ -325,6 +310,31 @@ namespace SHADE vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B; } + + // 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 * LINEAR_LOCK_A; + wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A; + + vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B; + wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B; } velocityStateA.LinearVelocity = vA;