xiaoqi
/
SHADE_Y3
1
0
Fork 0
Code Issues 12 Pull Requests Packages Projects Releases 9 Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Assets/Scenes/PhysicsSandbox.shade
View File

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

4
SHADE_Engine/src/Physics/Collision/Contacts/SHCollisionEvents.h
View File

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

3
SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h
View File

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

4
SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp
View File

@ -34,6 +34,7 @@ namespace SHADE
, bodyB { rhs.bodyB }
, numContacts { rhs.numContacts }
, state { rhs.state }
, cachedSATInfo { rhs.cachedSATInfo }
, normal { rhs.normal }
{
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 }
, numContacts { rhs.numContacts }
, state { rhs.state }
, cachedSATInfo { rhs.cachedSATInfo }
, normal { rhs.normal }
{
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;
numContacts = rhs.numContacts;
state = rhs.state;
cachedSATInfo = rhs.cachedSATInfo;
normal = rhs.normal;
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;
numContacts = rhs.numContacts;
state = rhs.state;
cachedSATInfo = rhs.cachedSATInfo;
normal = rhs.normal;
static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);

11
SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h
View File

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

139
SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp
View File

@ -48,25 +48,56 @@ 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.01f; // 0.0005
static constexpr float RELATIVE_TOLERANCE = 0.95f; // 1.0002
const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
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);
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;
}
const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
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;
}
const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
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;
}
// 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.
@ -81,6 +112,10 @@ namespace SHADE
referencePoly = &POLY_A;
incidentPoly = &POLY_B;
flipNormal = false;
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
cachedInfo.info.refFace = minFaceQuery.closestFace;
}
else
{
@ -88,9 +123,13 @@ namespace SHADE
referencePoly = &POLY_B;
incidentPoly = &POLY_A;
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
@ -114,6 +153,8 @@ namespace SHADE
manifold.normal = -manifold.normal;
// In this scenario, we only have one contact
uint32_t numContacts = 0;
SHContact contact;
contact.featurePair.key = EDGE_QUERY.axis;
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
@ -122,12 +163,20 @@ namespace SHADE
manifold.contacts[numContacts++] = contact;
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;
}
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
manifold.cachedSATInfo = cachedInfo;
return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
}
@ -268,6 +317,7 @@ namespace SHADE
* (LB(s) - LA(r)) /dot VB = 0
*
* Where LB(s) - LA(r) is the same vector as VB X VA.
*
*/
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
@ -291,6 +341,8 @@ namespace SHADE
* c = C' /dot U
*
* U is either VA or VB
*
* TODO: Check if segments degenerate into a point
*/
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 NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
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);
// Just take a point from A since it's A -> B
@ -687,5 +741,82 @@ namespace SHADE
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

87
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.h Normal file
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"

71
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp Normal file
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;
}
}