Cached SAT for improved stability

The effects of baumgarte stabilisation can be rather obvious especially when polyhedrons are thrown around at angles. Regardless, the system is relatively stable bar the added energy from the solving method, which may make for a more "bombastic" physics playground
This commit is contained in:
Diren D Bharwani 2023-01-23 00:37:22 +08:00
parent 0c3106f15b
commit a0f6cd3ae7
8 changed files with 351 additions and 44 deletions

View File

@ -45,7 +45,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Camera Component: Camera Component:
Position: {x: 0, y: 2, z: 7} Position: {x: 0, y: 2, z: 10}
Pitch: 0 Pitch: 0
Yaw: 0 Yaw: 0
Roll: 0 Roll: 0
@ -62,7 +62,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: -1, y: 7.5, z: 0} Translate: {x: -2, y: 7.5, z: 0}
Rotate: {x: -0, y: 0, z: 0.785398185} Rotate: {x: -0, y: 0, z: 0.785398185}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 1, y: 1, z: 1}
IsActive: true IsActive: true
@ -77,7 +77,7 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: false Freeze Position Y: true
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
Freeze Rotation Y: false Freeze Rotation Y: false
@ -267,8 +267,8 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: 2, y: 2, z: 3} Translate: {x: 0, y: 7, z: 0}
Rotate: {x: 0.785398185, y: 0.785398185, z: 0.785398185} Rotate: {x: 0, y: 0.785398185, z: 0}
Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:

View File

@ -38,7 +38,7 @@ namespace SHADE
{ {
public: public:
SHCollisionKey info; SHCollisionKey info;
SHCollisionState state; SHCollisionState state = SHCollisionState::INVALID;
}; };
/** /**
@ -52,7 +52,7 @@ namespace SHADE
static constexpr int MAX_NUM_CONTACTS = 4; static constexpr int MAX_NUM_CONTACTS = 4;
SHCollisionKey info; SHCollisionKey info;
SHCollisionState state; SHCollisionState state = SHCollisionState::INVALID;
SHVec3 normal; SHVec3 normal;
SHVec3 contactPoints[MAX_NUM_CONTACTS]; SHVec3 contactPoints[MAX_NUM_CONTACTS];
}; };

View File

@ -12,6 +12,7 @@
// Primary Header // Primary Header
#include "Physics/Dynamics/SHRigidBody.h" #include "Physics/Dynamics/SHRigidBody.h"
#include "Physics/Collision/Narrowphase/SHSATInfo.h"
#include "Physics/Collision/Shapes/SHCollisionShape.h" #include "Physics/Collision/Shapes/SHCollisionShape.h"
#include "SHContact.h" #include "SHContact.h"
#include "SHCollisionEvents.h" #include "SHCollisionEvents.h"
@ -58,6 +59,8 @@ namespace SHADE
uint32_t numContacts = 0; uint32_t numContacts = 0;
SHCollisionState state = SHCollisionState::INVALID; SHCollisionState state = SHCollisionState::INVALID;
SHSATInfo cachedSATInfo;
SHVec3 normal; SHVec3 normal;
SHVec3 tangents[SHContact::NUM_TANGENTS]; SHVec3 tangents[SHContact::NUM_TANGENTS];
SHContact contacts[MAX_NUM_CONTACTS]; SHContact contacts[MAX_NUM_CONTACTS];

View File

@ -34,6 +34,7 @@ namespace SHADE
, bodyB { rhs.bodyB } , bodyB { rhs.bodyB }
, numContacts { rhs.numContacts } , numContacts { rhs.numContacts }
, state { rhs.state } , state { rhs.state }
, cachedSATInfo { rhs.cachedSATInfo }
, normal { rhs.normal } , normal { rhs.normal }
{ {
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS); static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@ -50,6 +51,7 @@ namespace SHADE
, bodyB { rhs.bodyB } , bodyB { rhs.bodyB }
, numContacts { rhs.numContacts } , numContacts { rhs.numContacts }
, state { rhs.state } , state { rhs.state }
, cachedSATInfo { rhs.cachedSATInfo }
, normal { rhs.normal } , normal { rhs.normal }
{ {
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS); static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@ -74,6 +76,7 @@ namespace SHADE
bodyB = rhs.bodyB; bodyB = rhs.bodyB;
numContacts = rhs.numContacts; numContacts = rhs.numContacts;
state = rhs.state; state = rhs.state;
cachedSATInfo = rhs.cachedSATInfo;
normal = rhs.normal; normal = rhs.normal;
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS); static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@ -93,6 +96,7 @@ namespace SHADE
bodyB = rhs.bodyB; bodyB = rhs.bodyB;
numContacts = rhs.numContacts; numContacts = rhs.numContacts;
state = rhs.state; state = rhs.state;
cachedSATInfo = rhs.cachedSATInfo;
normal = rhs.normal; normal = rhs.normal;
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS); static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);

View File

@ -139,6 +139,8 @@ namespace SHADE
, const SHConvexPolyhedron& B , const SHConvexPolyhedron& B
) noexcept; ) noexcept;
static EdgeQuery queryEdgeDirections static EdgeQuery queryEdgeDirections
( (
const SHConvexPolyhedron& A const SHConvexPolyhedron& A
@ -207,5 +209,14 @@ namespace SHADE
, const SHVec3& faceNormal , const SHVec3& faceNormal
) noexcept; ) noexcept;
// Cached Convex VS Convex
static bool cachedConvexVSConvex
(
SHManifold& manifold
, const SHSATInfo& cachedInfo
, const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
) noexcept;
}; };
} // namespace SHADE } // namespace SHADE

View File

@ -48,25 +48,56 @@ namespace SHADE
bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
{ {
static constexpr float ABSOLUTE_TOLERANCE = 0.01f; static constexpr float ABSOLUTE_TOLERANCE = 0.01f; // 0.0005
static constexpr float RELATIVE_TOLERANCE = 0.95f; static constexpr float RELATIVE_TOLERANCE = 0.95f; // 1.0002
const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A); const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B); const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
// TODO: Check against cached separating axis. if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID)
return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B);
SHSATInfo cachedInfo;
const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
if (FACE_QUERY_A.bestDistance > 0.0f) if (FACE_QUERY_A.bestDistance > 0.0f)
{
// cache the info
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
cachedInfo.info.refFace = FACE_QUERY_A.closestFace;
manifold.cachedSATInfo = cachedInfo;
return false; return false;
}
const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
if (FACE_QUERY_B.bestDistance > 0.0f) if (FACE_QUERY_B.bestDistance > 0.0f)
{
// cache the info
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
cachedInfo.info.refFace = FACE_QUERY_B.closestFace;
manifold.cachedSATInfo = cachedInfo;
return false; return false;
}
const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
if (EDGE_QUERY.bestDistance > 0.0f) if (EDGE_QUERY.bestDistance > 0.0f)
{
// cache the info
cachedInfo.type = SHSATInfo::Type::EDGE;
cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
manifold.cachedSATInfo = cachedInfo;
return false; return false;
}
// Apply weight to improve frame coherence of normal directions. // Apply weight to improve frame coherence of normal directions.
// We want a normal in the direction from A -> B, so we flip the normal if needed. // We want a normal in the direction from A -> B, so we flip the normal if needed.
@ -81,6 +112,10 @@ namespace SHADE
referencePoly = &POLY_A; referencePoly = &POLY_A;
incidentPoly = &POLY_B; incidentPoly = &POLY_B;
flipNormal = false; flipNormal = false;
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
cachedInfo.info.refFace = minFaceQuery.closestFace;
} }
else else
{ {
@ -88,9 +123,13 @@ namespace SHADE
referencePoly = &POLY_B; referencePoly = &POLY_B;
incidentPoly = &POLY_A; incidentPoly = &POLY_A;
flipNormal = true; flipNormal = true;
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
cachedInfo.info.refFace = minFaceQuery.closestFace;
} }
uint32_t numContacts = 0;
// If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
@ -114,6 +153,8 @@ namespace SHADE
manifold.normal = -manifold.normal; manifold.normal = -manifold.normal;
// In this scenario, we only have one contact // In this scenario, we only have one contact
uint32_t numContacts = 0;
SHContact contact; SHContact contact;
contact.featurePair.key = EDGE_QUERY.axis; contact.featurePair.key = EDGE_QUERY.axis;
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
@ -122,12 +163,20 @@ namespace SHADE
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts; manifold.numContacts = numContacts;
// Cache the info
cachedInfo.type = SHSATInfo::Type::EDGE;
cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
manifold.cachedSATInfo = cachedInfo;
return true; return true;
} }
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
manifold.cachedSATInfo = cachedInfo;
return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
} }
@ -268,6 +317,7 @@ namespace SHADE
* (LB(s) - LA(r)) /dot VB = 0 * (LB(s) - LA(r)) /dot VB = 0
* *
* Where LB(s) - LA(r) is the same vector as VB X VA. * Where LB(s) - LA(r) is the same vector as VB X VA.
*
*/ */
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
@ -291,6 +341,8 @@ namespace SHADE
* c = C' /dot U * c = C' /dot U
* *
* U is either VA or VB * U is either VA or VB
*
* TODO: Check if segments degenerate into a point
*/ */
const SHVec3 C = TAIL_B - TAIL_A; const SHVec3 C = TAIL_B - TAIL_A;
@ -310,6 +362,8 @@ namespace SHADE
const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : 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 NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1; const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
// We clamp it because the factor cannot be beyond [0,1] as it would be beyond the segment if it was so.
const float R = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f); const float R = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f);
// Just take a point from A since it's A -> B // Just take a point from A since it's A -> B
@ -687,5 +741,82 @@ namespace SHADE
return indicesToKeep; return indicesToKeep;
} }
bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
{
if (cachedInfo.type == SHSATInfo::Type::FACE)
{
SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!")
// Assume the reference poly was A
const SHConvexPolyhedron* incPoly = &B;
const SHConvexPolyhedron* refPoly = &A;
bool flip = false;
// Swap if it was B
if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B)
{
refPoly = &B;
incPoly = &A;
flip = true;
}
const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace);
const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace);
const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL);
const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL };
const float DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT);
if (DISTANCE > 0.0f)
return false;
// Find the incident face
const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL);
return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip);
}
if (cachedInfo.type == SHSATInfo::Type::EDGE)
{
const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
if (DISTANCE > 0.0f)
return false;
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB);
const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex);
const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex);
const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex);
const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex);
const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
manifold.normal = SHVec3::Cross(VB, VA);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
manifold.normal = -manifold.normal;
// In this scenario, we only have one contact
uint32_t numContacts = 0;
SHContact contact;
// Take feature pair key from previous manifold
contact.featurePair.key = manifold.contacts[0].featurePair.key;
contact.position = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
contact.penetration = -DISTANCE;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
// Should never reach this.
return false;
}
} // namespace SHADE } // namespace SHADE

View File

@ -0,0 +1,87 @@
/****************************************************************************************
* \file SHSATInfo.h
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Interface for storing information of collision detection between two
* convex shapes using SAT.
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#pragma once
#include <numeric> // int32_t
namespace SHADE
{
/**
* @brief
* Primarily used to cached collision information between two convex polyhedrons for
* temporal coherence.
*/
struct SHSATInfo
{
public:
/*---------------------------------------------------------------------------------*/
/* Type Definitions */
/*---------------------------------------------------------------------------------*/
enum class ShapeID : int32_t
{
A = 0
, B = 1
, INVALID = -1
};
enum class Type
{
EDGE
, FACE
, INVALID = -1
};
union ContactInfo
{
struct // FaceContact
{
ShapeID refPoly;
int32_t refFace;
};
struct // Edge Contact
{
int32_t edgeA;
int32_t edgeB;
};
};
/*---------------------------------------------------------------------------------*/
/* Data Members */
/*---------------------------------------------------------------------------------*/
Type type;
ContactInfo info;
/*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */
/*---------------------------------------------------------------------------------*/
SHSATInfo () noexcept;
SHSATInfo (const SHSATInfo& rhs) noexcept;
SHSATInfo (SHSATInfo&& rhs) noexcept;
~SHSATInfo () noexcept = default;
/*---------------------------------------------------------------------------------*/
/* Operator Overloads */
/*---------------------------------------------------------------------------------*/
SHSATInfo& operator= (const SHSATInfo& rhs) noexcept;
SHSATInfo& operator= (SHSATInfo&& rhs) noexcept;
};
} // namespace SHADE
#include "SHSATInfo.hpp"

View File

@ -0,0 +1,71 @@
/****************************************************************************************
* \file SHSATInfo.hpp
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Implementation of inlined methods for cached SAT Info.
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#pragma once
// Primary Header
#include "SHSATInfo.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Constructors & Destructor */
/*-----------------------------------------------------------------------------------*/
inline SHSATInfo::SHSATInfo() noexcept
: type { Type::INVALID }
, info {}
{
info.refPoly = ShapeID::INVALID;
info.refFace = -1;
}
inline SHSATInfo::SHSATInfo(const SHSATInfo& rhs) noexcept
: type { rhs.type }
, info {}
{
info.refPoly = rhs.info.refPoly;
info.refFace = rhs.info.refFace;
}
inline SHSATInfo::SHSATInfo(SHSATInfo&& rhs) noexcept
: type { rhs.type }
, info {}
{
info.refPoly = rhs.info.refPoly;
info.refFace = rhs.info.refFace;
}
/*---------------------------------------------------------------------------------- */
/* Operator Overloads */
/*---------------------------------------------------------------------------------- */
inline SHSATInfo& SHSATInfo::operator=(const SHSATInfo& rhs) noexcept
{
if (this == &rhs)
return *this;
type = rhs.type;
info.refPoly = rhs.info.refPoly;
info.refFace = rhs.info.refFace;
return *this;
}
inline SHSATInfo& SHSATInfo::operator=(SHSATInfo&& rhs) noexcept
{
type = rhs.type;
info.refPoly = rhs.info.refPoly;
info.refFace = rhs.info.refFace;
return *this;
}
}