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

Implemented a custom physics engine #316

Merged
direnbharwani merged 95 commits from SHPhysics into main 2023-01-23 15:55:45 +08:00
Conversation 0 Commits 95 Files Changed 144 +351 -44
8 changed files with 351 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a0f6cd3ae7 - Show all commits

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];

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

@ -28,13 +28,14 @@ namespace SHADE
}
inline SHManifold::SHManifold(const SHManifold& rhs) noexcept
: shapeA { rhs.shapeA }
, shapeB { rhs.shapeB }
, bodyA { rhs.bodyA }
, bodyB { rhs.bodyB }
, numContacts { rhs.numContacts }
, state { rhs.state }
, normal { rhs.normal }
: shapeA { rhs.shapeA }
, shapeB { rhs.shapeB }
, bodyA { rhs.bodyA }
, 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);
memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@ -44,13 +45,14 @@ namespace SHADE
}
inline SHManifold::SHManifold(SHManifold&& rhs) noexcept
: shapeA { rhs.shapeA }
, shapeB { rhs.shapeB }
, bodyA { rhs.bodyA }
, bodyB { rhs.bodyB }
, numContacts { rhs.numContacts }
, state { rhs.state }
, normal { rhs.normal }
: shapeA { rhs.shapeA }
, shapeB { rhs.shapeB }
, bodyA { rhs.bodyA }
, 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);
memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@ -68,13 +70,14 @@ namespace SHADE
if (this == &rhs)
return *this;
shapeA = rhs.shapeA;
shapeB = rhs.shapeB;
bodyA = rhs.bodyA;
bodyB = rhs.bodyB;
numContacts = rhs.numContacts;
state = rhs.state;
normal = rhs.normal;
shapeA = rhs.shapeA;
shapeB = rhs.shapeB;
bodyA = rhs.bodyA;
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);
memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@ -87,13 +90,14 @@ namespace SHADE
inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept
{
shapeA = rhs.shapeA;
shapeB = rhs.shapeB;
bodyA = rhs.bodyA;
bodyB = rhs.bodyB;
numContacts = rhs.numContacts;
state = rhs.state;
normal = rhs.normal;
shapeA = rhs.shapeA;
shapeB = rhs.shapeB;
bodyA = rhs.bodyA;
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);
memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_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

149
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,15 +341,17 @@ 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;
const float VB_DOT_VA = SHVec3::Dot(VB, VA); // a1
const float VB_DOT_VB = SHVec3::Dot(VB, VB); // a2
const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
const float C_DOT_VA = SHVec3::Dot(C, VA); // c1
const float VB_DOT_VA = SHVec3::Dot(VB, VA); // a1
const float VB_DOT_VB = SHVec3::Dot(VB, VB); // a2
const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
const float C_DOT_VA = SHVec3::Dot(C, VA); // c1
const float C_DOT_VB = SHVec3::Dot(C, VB); // c2
/*
@ -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;
}
}