Another attempt to fix invalid error bias
This commit is contained in:
Diren D Bharwani
parent
ea97dc3b50
commit
e293094b6d
|
|
@ -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: ~
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue