diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index d30c8f07..6d15850b 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -45,9 +45,9 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: -0.5, y: 10, z: -3} + Position: {x: 0, y: 0, z: 7} Pitch: 0 - Yaw: 180 + Yaw: 0 Roll: 0 Width: 1920 Height: 1080 @@ -71,7 +71,7 @@ Auto Mass: false Mass: 10 Drag: 0.00999999978 - Angular Drag: 0.00999999978 + Angular Drag: 0 Use Gravity: true Gravity Scale: 1 Interpolate: true @@ -95,15 +95,6 @@ Density: 1 Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} - - Is Trigger: true - Collision Tag: 1 - Type: Sphere - Radius: 0.5 - Friction: 0.400000006 - Bounciness: 0 - Density: 1 - Position Offset: {x: 0.75, y: 0.5, z: 0} - Rotation Offset: {x: 0, y: 0, z: 0} IsActive: true Scripts: - Type: PhysicsTestObj @@ -280,7 +271,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.524352431, y: 13.5, z: 0.0463808179} + Translate: {x: 1, y: 2, z: 3} Rotate: {x: -0, y: 0.785398066, z: 0.785398185} Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} IsActive: true @@ -321,7 +312,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -0.5, y: 10, z: 0} + Translate: {x: 0, y: 0, z: 3} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true diff --git a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp index dc87d706..7177e517 100644 --- a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp +++ b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp @@ -337,7 +337,7 @@ namespace SHADE { SHASSERT(index >= 0 && index < capacity, "Trying to free an invalid AABB Tree node!") - nodes[index].next = NULL_NODE; + nodes[index].next = freeList; nodes[index].height = NULL_NODE; // Put it back on the free list diff --git a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h index 70e53794..0337eedc 100644 --- a/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h +++ b/SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h @@ -31,22 +31,16 @@ namespace SHADE /* Type Definit */ /*---------------------------------------------------------------------------------*/ - enum class Type : uint8_t - { - VERTEX = 0 - , FACE = 1 - }; - /*---------------------------------------------------------------------------------*/ /* Data Members */ /*---------------------------------------------------------------------------------*/ struct { - uint8_t indexA; - uint8_t indexB; - uint8_t typeA; - uint8_t typeB; + uint8_t inI; + uint8_t outI; + uint8_t inR; + uint8_t outR; }; uint32_t key = std::numeric_limits::max(); diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 980914c1..2a4503ce 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -82,6 +82,7 @@ namespace SHADE { int32_t halfEdgeA = -1; int32_t halfEdgeB = -1; + int32_t axis = -1; float bestDistance = std::numeric_limits::lowest(); }; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index be43af8f..97e9dbd4 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -48,7 +48,8 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - static constexpr float TOLERANCE = 0.1f + SHPHYSICS_LINEAR_SLOP; + static constexpr float ABSOLUTE_TOLERANCE = 0.01f; + static constexpr float RELATIVE_TOLERANCE = 0.95f; const SHConvexPolyhedron& POLY_A = dynamic_cast(A); const SHConvexPolyhedron& POLY_B = dynamic_cast(B); @@ -72,7 +73,7 @@ namespace SHADE const SHConvexPolyhedron* incidentPoly = nullptr; FaceQuery minFaceQuery; - if (FACE_QUERY_A.bestDistance + TOLERANCE > FACE_QUERY_B.bestDistance) + if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) { minFaceQuery = FACE_QUERY_A; referencePoly = &POLY_A; @@ -91,7 +92,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 > minFaceQuery.bestDistance + TOLERANCE) + if (EDGE_QUERY.bestDistance * RELATIVE_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); @@ -111,13 +112,9 @@ namespace SHADE // In this scenario, we only have one contact SHContact contact; - contact.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); - contact.featurePair.indexA = HALF_EDGE_A.tailVertexIndex; - contact.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); - contact.featurePair.indexB = HALF_EDGE_B.tailVertexIndex; - - contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); - contact.penetration = EDGE_QUERY.bestDistance; + contact.featurePair.key = EDGE_QUERY.axis; + contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); + contact.penetration = EDGE_QUERY.bestDistance; manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts; @@ -188,6 +185,7 @@ namespace SHADE const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount(); const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount(); + int32_t axis = -1; for (int32_t i = 0; i < EDGE_COUNT_A; i += 2) { for (int32_t j = 0; j < EDGE_COUNT_B; j += 2) @@ -196,12 +194,15 @@ namespace SHADE if (!IS_MINKOWSKI_FACE) continue; + ++axis; + const float SEPARATION = distanceBetweenEdges(A, B, i, j); if (SEPARATION > edgeQuery.bestDistance) { edgeQuery.bestDistance = SEPARATION; edgeQuery.halfEdgeA = i; edgeQuery.halfEdgeB = j; + edgeQuery.axis = axis; } } } @@ -321,10 +322,10 @@ namespace SHADE * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 ) */ - const float A2_OVER_A1 = VB_DOT_VB / VB_DOT_VA; + const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA; const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB; const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1; - const float R = NUMERATOR / DENOMINATOR; + const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR; // Just take a point from A since it's A -> B return TAIL_A + R * VA; diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index e6a961e0..07466ab1 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -179,7 +179,13 @@ namespace SHADE bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - return SphereVsConvex(manifold, B, A); + if (SphereVsConvex(manifold, B, A)) + { + manifold.normal = -manifold.normal; + return true; + }; + + return false; } /*-----------------------------------------------------------------------------------*/ @@ -326,7 +332,13 @@ namespace SHADE // Face to vertex is in the opposite direction of any tangent. const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1); if (PROJECTION < 0) + { + const float DISTANCE_SQUARED = SHVec3::DistanceSquared(faceVertex, CENTER); + if (DISTANCE_SQUARED > RADIUS * RADIUS) + return 0; + return 3; + } // Belongs in region D by default return 4; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 34e3dabf..ade7b482 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -238,6 +238,8 @@ namespace SHADE void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept { + static const float SQRT_ONE_THIRD = 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,7 +252,7 @@ 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_THIRD) tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f }; else tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y }; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index fa6f1266..b5d7c2cc 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -187,14 +187,7 @@ 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 @@ -208,6 +201,14 @@ 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 = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f; + + contact.bias = ERROR_BIAS + RESTITUTION_BIAS; } velocityStateA.LinearVelocity = vA; diff --git a/SHADE_Engine/src/Physics/SHPhysicsConstants.h b/SHADE_Engine/src/Physics/SHPhysicsConstants.h index 0d8f6fc8..a6cbd608 100644 --- a/SHADE_Engine/src/Physics/SHPhysicsConstants.h +++ b/SHADE_Engine/src/Physics/SHPhysicsConstants.h @@ -26,7 +26,7 @@ namespace SHADE * @brief * Linear Collision & Constraint tolerance. */ - static constexpr float SHPHYSICS_LINEAR_SLOP = 0.005f * SHPHYSICS_LENGTHS_PER_UNIT_METER; + static constexpr float SHPHYSICS_LINEAR_SLOP = 0.05f * SHPHYSICS_LENGTHS_PER_UNIT_METER; /** * @brief