Another attempt to fix invalid error bias

This commit is contained in:
Diren D Bharwani 2023-03-04 17:31:50 +08:00
parent ea97dc3b50
commit e293094b6d
4 changed files with 307 additions and 43 deletions

View File

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

View File

@ -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<RaycastHit> rayList1 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535);
List<RaycastHit> rayList2 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535);
List<RaycastHit> rayList3 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, dirNor), rayDistance, false, (ushort)65535);
Vector3 offset = new Vector3(0.0f, rayYOffset, 0.0f);
List<RaycastHit> rayList1 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535);
List<RaycastHit> rayList2 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535);
List<RaycastHit> 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;
}

View File

@ -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<const SHConvexPolyhedron&>(A);
const SHConvexPolyhedron& POLY_B = reinterpret_cast<const SHConvexPolyhedron&>(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);

View File

@ -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<float>::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;