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

Fixed clipping of contacts against reference planes

This commit is contained in:
Diren D Bharwani 2023-03-01 21:30:00 +08:00
parent 0e3a84a06b
commit 2d2c8a1b20
8 changed files with 473 additions and 464 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
Assets/Scenes/SS_Playground.shade
View File

@ -23,15 +23,39 @@
Transform Component: Transform Component:
Translate: {x: 0, y: 0, z: 0} Translate: {x: 0, y: 0, z: 0}
Rotate: {x: 0, y: 0, z: 0} Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 2, y: 1, z: 1} Scale: {x: 1, y: 1, z: 1}
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
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: 3
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 2, z: 0}
Rotate: {x: 0, y: 0.785398185, z: 0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
Type: Static Type: Dynamic
Auto Mass: false Auto Mass: false
Mass: .inf Mass: 1
Drag: 0.00999999978 Drag: 0.00999999978
Angular Drag: 0.00999999978 Angular Drag: 0.00999999978
Use Gravity: false Use Gravity: true
Gravity Scale: 1 Gravity Scale: 1
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
@ -55,45 +79,4 @@
Position Offset: {x: 0, y: 0, z: 0} Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true IsActive: true
Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 1.14999998, y: 3, z: 0.550000012}
Rotate: {x: 0, y: 0, z: 0}
Scale: {x: 1, y: 1, 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: 0.25
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:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Sphere
Radius: 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: ~ Scripts: ~

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

@ -39,7 +39,7 @@ namespace SHADE
int32_t findIncidentFace (const SHConvexPolyhedron&, const SHVec3& normal) noexcept; int32_t findIncidentFace (const SHConvexPolyhedron&, const SHVec3& normal) noexcept;
bool findFaceContacts (SHManifold&, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept; bool findFaceContacts (SHManifold&, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept;
SHCollisionUtils::ClipVertices clipPolygonWithPlane (const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept; SHCollisionUtils::ClipVertices clipPolygonWithPlane (const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept;
//std::vector<int32_t> reduceContacts (const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept; std::vector<int32_t> reduceContacts (const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept;
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
/* Public Member Functions Definitions */ /* Public Member Functions Definitions */
@ -67,140 +67,143 @@ 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;
//static constexpr float RELATIVE_TOLERANCE = 0.95f; static constexpr float RELATIVE_TOLERANCE = 0.95f;
//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);
//if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) const SHCollisionUtils::ShapeTransform TF_A { POLY_A.GetWorldCentroid(), POLY_A.GetWorldOrientation() };
// return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); const SHCollisionUtils::ShapeTransform TF_B { POLY_B.GetWorldCentroid(), POLY_B.GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
//SHSATInfo cachedInfo; if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID)
return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B);
//const SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); SHSATInfo cachedInfo;
//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; const SHCollisionUtils::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;
// return false; manifold.cachedSATInfo = cachedInfo;
//}
//
//const SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); return false;
//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; const SHCollisionUtils::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;
// return false; manifold.cachedSATInfo = cachedInfo;
//}
//const SHCollisionUtils::EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); return false;
//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; const SHCollisionUtils::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;
// return false; manifold.cachedSATInfo = cachedInfo;
//}
//// Apply weight to improve frame coherence of normal directions. return false;
//// We want a normal in the direction from A -> B, so we flip the normal if needed. }
//bool flipNormal = false;
//const SHConvexPolyhedron* referencePoly = nullptr;
//const SHConvexPolyhedron* incidentPoly = nullptr;
//SHCollisionUtils::FaceQuery minFaceQuery; // Apply weight to improve frame coherence of normal directions.
//if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) // We want a normal in the direction from A -> B, so we flip the normal if needed.
//{ bool flipNormal = false;
// minFaceQuery = FACE_QUERY_A; const SHConvexPolyhedron* referencePoly = nullptr;
// referencePoly = &POLY_A; const SHConvexPolyhedron* incidentPoly = nullptr;
// incidentPoly = &POLY_B;
// flipNormal = false;
// cachedInfo.type = SHSATInfo::Type::FACE; SHCollisionUtils::FaceQuery minFaceQuery;
// cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
// cachedInfo.info.refFace = minFaceQuery.closestFace; {
//} minFaceQuery = FACE_QUERY_A;
//else referencePoly = &POLY_A;
//{ incidentPoly = &POLY_B;
// minFaceQuery = FACE_QUERY_B; flipNormal = false;
// referencePoly = &POLY_B;
// incidentPoly = &POLY_A;
// flipNormal = true;
// cachedInfo.type = SHSATInfo::Type::FACE; cachedInfo.type = SHSATInfo::Type::FACE;
// cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
// cachedInfo.info.refFace = minFaceQuery.closestFace; cachedInfo.info.refFace = minFaceQuery.closestFace;
//} }
else
{
minFaceQuery = FACE_QUERY_B;
referencePoly = &POLY_B;
incidentPoly = &POLY_A;
flipNormal = true;
// cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
cachedInfo.info.refFace = minFaceQuery.closestFace;
}
//// 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
//// each edge and use that as the contact point. // 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 > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
//{ {
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
// const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex); const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex);
// const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex); const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex);
// const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex); const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex);
// const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex); const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex);
// const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
// const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
// manifold.normal = SHVec3::Cross(VB, VA); manifold.normal = SHVec3::Cross(VB, VA);
// // Flip normal if need to ( A -> B) // Flip normal if need to ( A -> B)
// if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) if (SHVec3::Dot(manifold.normal, TAIL_A - A.GetWorldCentroid()) < 0.0f)
// 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; uint32_t numContacts = 0;
// const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
// SHContact contact; SHContact contact;
// contact.featurePair.key = EDGE_QUERY.axis; contact.featurePair.key = EDGE_QUERY.axis;
// contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f); contact.penetration = -EDGE_QUERY.bestDistance;
// contact.penetration = -EDGE_QUERY.bestDistance;
// manifold.contacts[numContacts++] = contact; contact.localPointA = INV_TF_A * CONTACTS.closestPointB;
// manifold.numContacts = numContacts; contact.localPointB = INV_TF_B * CONTACTS.closestPointA;
// // Cache the info manifold.contacts[numContacts++] = contact;
// cachedInfo.type = SHSATInfo::Type::EDGE; manifold.numContacts = numContacts;
// cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
// cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
// manifold.cachedSATInfo = cachedInfo; // Cache the info
cachedInfo.type = SHSATInfo::Type::EDGE;
cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
// return true; manifold.cachedSATInfo = cachedInfo;
//}
//const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); return true;
//const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); }
//manifold.cachedSATInfo = cachedInfo; const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
//return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
return false; manifold.cachedSATInfo = cachedInfo;
return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
} }
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -295,6 +298,7 @@ namespace SHADE
const float ADC = SHVec3::Dot(a, DXC); const float ADC = SHVec3::Dot(a, DXC);
const float BDC = SHVec3::Dot(b, DXC); const float BDC = SHVec3::Dot(b, DXC);
// Short circuit may be faster without storing the booleans
return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f; return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
} }
@ -311,17 +315,17 @@ namespace SHADE
const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A); const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B); const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B);
// Check if the edges are parallel (abs dot product is 1) // Check if the edges are parallel (cross product length close to 0)
const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B)); if (SHMath::CompareFloat(SHVec3::Cross(DIR_A, DIR_B).LengthSquared(), 0.0f))
if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f))
return std::numeric_limits<float>::lowest(); return std::numeric_limits<float>::lowest();
SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B); SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) // Flip normal if need to (A -> B)
if (SHVec3::Dot(normal, TAIL_A - A.GetWorldCentroid()) < 0.0f)
normal = -normal; normal = -normal;
return SHVec3::Dot(normal, HEAD_B - HEAD_A); return SHVec3::Dot(normal, TAIL_B - TAIL_A);
} }
SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges(const SHConvexPolyhedron& polyA, const SHConvexPolyhedron& polyB, int32_t edgeA, int32_t edgeB) noexcept SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges(const SHConvexPolyhedron& polyA, const SHConvexPolyhedron& polyB, int32_t edgeA, int32_t edgeB) noexcept
@ -347,7 +351,7 @@ namespace SHADE
const float A = D1.LengthSquared(); // a const float A = D1.LengthSquared(); // a
const float E = D2.LengthSquared(); // e const float E = D2.LengthSquared(); // e
const float B = SHVec3::Dot(D1, D2); // b const float B = SHVec3::Dot(D1, D2); // b
const float C = SHVec3::Dot(D1, R); // c const float C = SHVec3::Dot(D1, R); // c
const float F = SHVec3::Dot(D2, R); // f const float F = SHVec3::Dot(D2, R); // f
@ -425,144 +429,159 @@ namespace SHADE
bool findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept bool findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept
{ {
//const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); const SHCollisionUtils::ShapeTransform TF_INC { incPoly.GetWorldCentroid(), incPoly.GetWorldOrientation() };
//const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); const SHCollisionUtils::ShapeTransform TF_REF { refPoly.GetWorldCentroid(), refPoly.GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform INV_TF_INC = TF_INC.GetInverse();
const SHCollisionUtils::ShapeTransform INV_TF_REF = TF_REF.GetInverse();
//const int32_t NUM_INCIDENT_VERTICES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
//const int32_t NUM_REFERENCE_VERTICES = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
//// Build incoming vertices to clip const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace);
//SHCollisionUtils::ClipVertices clipIn; const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace);
//clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{});
//int32_t numClipIn = 0; const int32_t NUM_INCIDENT_VERTICES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
//for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i) const int32_t NUM_REFERENCE_VERTICES = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
//{
// const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
// const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index; const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace);
const SHVec3 LOCAL_REF_NORMAL = SHVec3::Rotate(REF_NORMAL, INV_TF_REF.orientation);
// // The incoming id is the previous edge // Build incoming vertices to clip
// // The outgoing id is the current edge (where this vertex is the tail of) // But it in the reference local-space
SHCollisionUtils::ClipVertices clipIn;
clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{});
// SHCollisionUtils::ClipVertex v; int32_t numClipIn = 0;
// v.position = incPoly.GetVertex(V_INDEX); for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
// v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex; {
// v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex; const int32_t PREV_I = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
// v.featurePair.inR = 0; const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
// v.featurePair.outR = 0;
// clipIn[numClipIn++] = v; // The incoming id is the previous edge
//} // The outgoing id is the current edge (where this vertex is the tail of)
//// Clip the vertices against the reference face side planes. SHCollisionUtils::ClipVertex v;
//// Number of side planes == number of edges == number of vertices v.position = INV_TF_REF * incPoly.GetVertex(V_INDEX);
//const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); v.featurePair.inI = INCIDENT_FACE.vertexIndices[PREV_I].edgeIndex;
//for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i) v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex;
//{ v.featurePair.inR = 0;
// // Side plane can be built with the vertex on the edge and the plane's normal v.featurePair.outR = 0;
// // Plane normal = faceNormal X tangent (v2 - v1)
// const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index; clipIn[numClipIn++] = v;
// const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index; }
// const SHVec3 V1 = refPoly.GetVertex(V1_INDEX); // Clip the vertices against the reference face side planes.
// const SHVec3 V2 = refPoly.GetVertex(V2_INDEX); // Number of side planes == number of edges == number of vertices
for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i)
{
// Side plane can be built with the vertex on the edge and the plane's normal
// Plane normal = faceNormal X tangent (v2 - v1)
// const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
// const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) }; const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
// SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); // Get reference vertices in reference local space
// if (clipOut.empty()) const SHVec3 V1 = INV_TF_REF * refPoly.GetVertex(V1_INDEX);
// return false; const SHVec3 V2 = INV_TF_REF * refPoly.GetVertex(V2_INDEX);
// // Replace the clip container's contents with the clipped points for the next clipping pass const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
// const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size()); const SHVec3 PLANE_NORMAL = SHVec3::Cross(LOCAL_REF_NORMAL, TANGENT);
// for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex) const SHPlane CLIP_PLANE { V1, PLANE_NORMAL };
// {
// clipIn[clippedIndex].position = clipOut[clippedIndex].position;
// clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key;
// }
// numClipIn = NUM_CLIPPED;
//}
//// From the final set of clipped points, only keep the points that are below the reference plane. SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
//const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL }; if (clipOut.empty())
return false;
//uint32_t numContacts = 0; // Replace the clip container's contents with the clipped points for the next clipping pass
const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size());
for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex)
{
clipIn[clippedIndex].position = clipOut[clippedIndex].position;
clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key;
}
numClipIn = NUM_CLIPPED;
}
//std::vector<SHContact> contacts; // From the final set of clipped points, only keep the points that are below the reference plane.
//for (int32_t i = 0; i < numClipIn; ++i) const SHVec3 LOCAL_REF_POINT = INV_TF_REF * refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index);
//{ const SHPlane REFERENCE_PLANE{ LOCAL_REF_POINT, LOCAL_REF_NORMAL };
// const SHVec3 POS = clipIn[i].position;
// const float DIST = REFERENCE_PLANE.SignedDistance(POS);
// if (DIST <= 0.0f)
// {
// SHContact contact;
// contact.position = POS;
// contact.penetration = -DIST;
// if (flip) uint32_t numContacts = 0;
// {
// contact.featurePair.inI = clipIn[i].featurePair.inR;
// contact.featurePair.inR = clipIn[i].featurePair.inI;
// contact.featurePair.outI = clipIn[i].featurePair.outR;
// contact.featurePair.outR = clipIn[i].featurePair.outI;
// }
// else
// {
// contact.featurePair.key = clipIn[i].featurePair.key;
// }
// contacts.emplace_back(contact); std::vector<SHContact> contacts;
// ++numContacts; for (int32_t i = 0; i < numClipIn; ++i)
// } {
//} const SHVec3 POS = clipIn[i].position;
const float DIST = REFERENCE_PLANE.SignedDistance(POS);
if (DIST <= 0.0f)
{
SHContact contact;
contact.penetration = -DIST;
//// Reduce contact manifold if more than 4 points // A: Incident, B: Reference
//if (numContacts > 4) // Project the clipped point onto the reference place for localPointB
//{ contact.localPointA = INV_TF_INC * (TF_REF * POS);
// const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL); contact.localPointB = POS + LOCAL_REF_NORMAL * SHVec3::Dot(LOCAL_REF_NORMAL, POS - LOCAL_REF_POINT);
// std::vector<SHContact> reducedContacts;
// const int32_t NUM_REDUCED = static_cast<int32_t>(INDICES_TO_KEEP.size()); if (flip)
// for (int32_t i = 0; i < NUM_REDUCED; ++i) {
// reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]); contact.featurePair.inI = clipIn[i].featurePair.inR;
contact.featurePair.inR = clipIn[i].featurePair.inI;
contact.featurePair.outI = clipIn[i].featurePair.outR;
contact.featurePair.outR = clipIn[i].featurePair.outI;
// contacts.clear(); std::swap(contact.localPointA, contact.localPointB);
// // Copy contacts to main container }
// for (auto& contact : reducedContacts) else
// contacts.emplace_back(contact); {
//} contact.featurePair.key = clipIn[i].featurePair.key;
}
//// Remove potential duplicate contact points contacts.emplace_back(contact);
//// No way about this being an n^2 loop ++numContacts;
//static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE; }
//for (auto i = contacts.begin(); i != contacts.end(); ++i) }
//{
// for (auto j = i + 1; j != contacts.end();)
// {
// const float D2 = SHVec3::DistanceSquared(i->position, j->position);
// if (D2 < THRESHOLD)
// j = contacts.erase(j);
// else
// ++j;
// }
//}
//// Copy final contacts into the manifold // Reduce contact manifold if more than 4 points
//numContacts = static_cast<int32_t>(contacts.size()); if (numContacts > 4)
//for (int32_t i = 0; i < numContacts; ++i) {
// manifold.contacts[i] = contacts[i]; const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL);
std::vector<SHContact> reducedContacts;
//manifold.numContacts = numContacts; const int32_t NUM_REDUCED = static_cast<int32_t>(INDICES_TO_KEEP.size());
//manifold.normal = REF_NORMAL; for (int32_t i = 0; i < NUM_REDUCED; ++i)
//if (flip) reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]);
// manifold.normal = -manifold.normal;
//return true; contacts.clear();
// Copy contacts to main container
for (auto& contact : reducedContacts)
contacts.emplace_back(contact);
}
return false; // Remove potential duplicate contact points
// No way about this being an n^2 loop
static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE;
for (auto i = contacts.begin(); i != contacts.end(); ++i)
{
for (auto j = i + 1; j != contacts.end();)
{
const float D2 = SHVec3::DistanceSquared(i->localPointA, j->localPointA);
if (D2 < THRESHOLD)
j = contacts.erase(j);
else
++j;
}
}
// Copy final contacts into the manifold
numContacts = static_cast<int32_t>(contacts.size());
for (int32_t i = 0; i < numContacts; ++i)
manifold.contacts[i] = contacts[i];
manifold.numContacts = numContacts;
manifold.normal = REF_NORMAL;
if (flip)
manifold.normal = -manifold.normal;
return true;
} }
@ -591,9 +610,12 @@ namespace SHADE
{ {
SHCollisionUtils::ClipVertex intersection; SHCollisionUtils::ClipVertex intersection;
const SHVec3 PLANE_NORMAL = -plane.GetNormal();
// In case the edge is parallel, the intersection is just the start point // In case the edge is parallel, the intersection is just the start point
const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f); const float DOT = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL);
const float ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos); const bool IS_PARALLEL = SHMath::CompareFloat(DOT, 0.0f);
const float ALPHA = IS_PARALLEL ? 0.0f : (plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT;
intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
intersection.featurePair.inI = in[v1Index].featurePair.outI; intersection.featurePair.inI = in[v1Index].featurePair.outI;
@ -610,9 +632,12 @@ namespace SHADE
{ {
SHCollisionUtils::ClipVertex intersection; SHCollisionUtils::ClipVertex intersection;
// In case the edge is parallel, the intersection is just the start point const SHVec3 PLANE_NORMAL = plane.GetNormal();
const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f);
const float ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos); // In case the edge is parallel, the intersection is just the end point
const float DOT = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL);
const bool IS_PARALLEL = SHMath::CompareFloat(DOT, 0.0f);
const float ALPHA = IS_PARALLEL ? 1.0f : (-plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT;
intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
intersection.featurePair.inI = 0; intersection.featurePair.inI = 0;
@ -638,220 +663,225 @@ namespace SHADE
return out; return out;
} }
//std::vector<int32_t> reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept std::vector<int32_t> reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept
//{ {
// std::vector<int32_t> indicesToKeep; std::vector<int32_t> indicesToKeep;
// // Use a map to temporarily store and track the contacts we want // Use a map to temporarily store and track the contacts we want
// std::unordered_map<int, const SHContact*> contactMap; std::unordered_map<int, const SHContact*> contactMap;
// const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size()); const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size());
// for (int32_t i = 0; i < NUM_CONTACTS; ++i) for (int32_t i = 0; i < NUM_CONTACTS; ++i)
// contactMap.emplace(i, &in[i]); contactMap.emplace(i, &in[i]);
// // Find the furthest point in a given direction // Find the furthest point in a given direction
// int32_t indexToKeep = -1; int32_t indexToKeep = -1;
// float bestDistance = std::numeric_limits<float>::lowest(); float bestDistance = std::numeric_limits<float>::lowest();
// for (const auto& [index, contact] : contactMap) for (const auto& [index, contact] : contactMap)
// { {
// const float DIST = SHVec3::Dot(contact->position, SHVec3::One); const float DIST = SHVec3::Dot(contact->localPointA, SHVec3::One);
// if (DIST > bestDistance) if (DIST > bestDistance)
// { {
// bestDistance = DIST; bestDistance = DIST;
// indexToKeep = index; indexToKeep = index;
// } }
// } }
// indicesToKeep.emplace_back(indexToKeep); indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep); contactMap.erase(indexToKeep);
// indexToKeep = -1; indexToKeep = -1;
// bestDistance = std::numeric_limits<float>::lowest(); bestDistance = std::numeric_limits<float>::lowest();
// // Find point furthest away from the first index // Find point furthest away from the first index
// const SHVec3& FIRST_POS = in[indicesToKeep.back()].position; const SHVec3& FIRST_POS = in[indicesToKeep.back()].localPointA;
// for (const auto& [index, contact] : contactMap) for (const auto& [index, contact] : contactMap)
// { {
// const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->position); const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->localPointA);
// if (DIST_SQUARED > bestDistance) if (DIST_SQUARED > bestDistance)
// { {
// bestDistance = DIST_SQUARED; bestDistance = DIST_SQUARED;
// indexToKeep = index; indexToKeep = index;
// } }
// } }
// indicesToKeep.emplace_back(indexToKeep); indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep); contactMap.erase(indexToKeep);
// indexToKeep = -1; indexToKeep = -1;
// // We compute the triangle with the largest area. // We compute the triangle with the largest area.
// // The area can be positive or negative depending on the winding order. // The area can be positive or negative depending on the winding order.
// float maxArea = std::numeric_limits<float>::lowest(); float maxArea = std::numeric_limits<float>::lowest();
// float minArea = std::numeric_limits<float>::max(); float minArea = std::numeric_limits<float>::max();
// int32_t maxAreaIndex = -1; int32_t maxAreaIndex = -1;
// int32_t minAreaIndex = -1; int32_t minAreaIndex = -1;
// const SHVec3& SECOND_POS = in[indicesToKeep.back()].position; const SHVec3& SECOND_POS = in[indicesToKeep.back()].localPointA;
// for (const auto& [index, contact] : contactMap) for (const auto& [index, contact] : contactMap)
// { {
// const SHVec3& POS = contact->position; const SHVec3& POS = contact->localPointA;
// const SHVec3 TO_P1 = FIRST_POS - POS; const SHVec3 TO_P1 = FIRST_POS - POS;
// const SHVec3 TO_P2 = SECOND_POS - POS; const SHVec3 TO_P2 = SECOND_POS - POS;
// const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
// if (AREA > maxArea) if (AREA > maxArea)
// { {
// maxArea = AREA; maxArea = AREA;
// maxAreaIndex = index; maxAreaIndex = index;
// } }
// if (AREA < minArea) if (AREA < minArea)
// { {
// minArea = AREA; minArea = AREA;
// minAreaIndex = index; minAreaIndex = index;
// } }
// } }
// // Compare which triangle creates the largest area // Compare which triangle creates the largest area
// bool isAreaPositive = false; bool isAreaPositive = false;
// if (maxArea > (-minArea)) if (maxArea > (-minArea))
// { {
// isAreaPositive = true; isAreaPositive = true;
// indexToKeep = maxAreaIndex; indexToKeep = maxAreaIndex;
// } }
// else else
// { {
// isAreaPositive = false; isAreaPositive = false;
// indexToKeep = minAreaIndex; indexToKeep = minAreaIndex;
// } }
// indicesToKeep.emplace_back(indexToKeep); indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep); contactMap.erase(indexToKeep);
// indexToKeep = -1; indexToKeep = -1;
// // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle
// // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa. // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa.
// float bestArea = 0.0f; float bestArea = 0.0f;
// const SHVec3& THIRD_POS = in[indicesToKeep.back()].position; const SHVec3& THIRD_POS = in[indicesToKeep.back()].localPointA;
// const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS }; const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
// for (const auto& [index, contact] : contactMap) for (const auto& [index, contact] : contactMap)
// { {
// const SHVec3& Q = contact->position; const SHVec3& Q = contact->localPointA;
// for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
// { {
// const int P1 = i; const int P1 = i;
// const int P2 = (i + 1) % 3; const int P2 = (i + 1) % 3;
// const SHVec3 TO_P1 = ABC[P1] - Q; const SHVec3 TO_P1 = ABC[P1] - Q;
// const SHVec3 TO_P2 = ABC[P2] - Q; const SHVec3 TO_P2 = ABC[P2] - Q;
// const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
// if (isAreaPositive && AREA < bestArea) if (isAreaPositive && AREA < bestArea)
// { {
// bestArea = AREA; bestArea = AREA;
// indexToKeep = index; indexToKeep = index;
// } }
// if (!isAreaPositive && AREA > bestArea) if (!isAreaPositive && AREA > bestArea)
// { {
// bestArea = AREA; bestArea = AREA;
// indexToKeep = index; indexToKeep = index;
// } }
// } }
// } }
// indicesToKeep.emplace_back(indexToKeep); indicesToKeep.emplace_back(indexToKeep);
// return indicesToKeep; return indicesToKeep;
//} }
bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
{ {
//if (cachedInfo.type == SHSATInfo::Type::FACE) if (cachedInfo.type == SHSATInfo::Type::FACE)
//{ {
// SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!") SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!")
// // Assume the reference poly was A // Assume the reference poly was A
// const SHConvexPolyhedron* incPoly = &B; const SHConvexPolyhedron* incPoly = &B;
// const SHConvexPolyhedron* refPoly = &A; const SHConvexPolyhedron* refPoly = &A;
// bool flip = false; bool flip = false;
// // Swap if it was B // Swap if it was B
// if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B) if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B)
// { {
// refPoly = &B; refPoly = &B;
// incPoly = &A; incPoly = &A;
// flip = true; flip = true;
// } }
// const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace); const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace);
// const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace); const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace);
// const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL); const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL);
// const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), 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); const float DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT);
// if (DISTANCE > 0.0f) if (DISTANCE > 0.0f)
// return false; return false;
// // Find the incident face // Find the incident face
// const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL); const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL);
// return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip); return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip);
//} }
//if (cachedInfo.type == SHSATInfo::Type::EDGE) if (cachedInfo.type == SHSATInfo::Type::EDGE)
//{ {
// const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
// if (DISTANCE > 0.0f) if (DISTANCE > 0.0f)
// return false; return false;
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA); const SHCollisionUtils::ShapeTransform TF_A { A.GetWorldCentroid(), A.GetWorldOrientation() };
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB); const SHCollisionUtils::ShapeTransform TF_B { B.GetWorldCentroid(), B.GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
// const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA);
// const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB);
// 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 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex);
// const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); 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);
// manifold.normal = SHVec3::Cross(VB, VA); const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
// // Flip normal if need to ( A -> B) const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
// if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
// manifold.normal = -manifold.normal;
// // In this scenario, we only have one contact manifold.normal = SHVec3::Cross(VB, VA);
// uint32_t numContacts = 0; // Flip normal if need to ( A -> B)
if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
manifold.normal = -manifold.normal;
// const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); // In this scenario, we only have one contact
uint32_t numContacts = 0;
// SHContact contact; const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
// // Take feature pair key from previous manifold
// contact.featurePair.key = manifold.contacts[0].featurePair.key;
// contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f);
// contact.penetration = -DISTANCE;
// manifold.contacts[numContacts++] = contact; SHContact contact;
// manifold.numContacts = numContacts; // Take feature pair key from previous manifold
contact.featurePair.key = manifold.contacts[0].featurePair.key;
contact.penetration = -DISTANCE;
// return true; contact.localPointA = INV_TF_A * CONTACTS.closestPointB;
//} contact.localPointB = INV_TF_B * CONTACTS.closestPointA;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
// Should never reach this. // Should never reach this.
return false; return false;
} }
} // namespace SHADE } // namespace SHADE

2
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp
View File

@ -238,7 +238,7 @@ namespace SHADE
// Build plane equation // Build plane equation
// Use first vertex to build the plane // Use first vertex to build the plane
const SHPlane FACE_PLANE { polyhedron.GetVertex(FACE.vertexIndices[0].index), polyhedron.GetNormal(i) }; const SHPlane FACE_PLANE { polyhedron.GetVertex(FACE.vertexIndices[0].index), -polyhedron.GetNormal(i) };
// Find signed distance of center to plane // Find signed distance of center to plane
const float SIGNED_DIST = FACE_PLANE.SignedDistance(CENTER); const float SIGNED_DIST = FACE_PLANE.SignedDistance(CENTER);

12
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp
View File

@ -68,8 +68,8 @@ namespace SHADE
const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() }; const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform TF_B = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() }; const SHCollisionUtils::ShapeTransform TF_B = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse();
const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse();
// Only populate the manifold if there is a collision // Only populate the manifold if there is a collision
@ -83,16 +83,16 @@ namespace SHADE
if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f)) if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
{ {
manifold.normal = SHVec3::UnitY; manifold.normal = SHVec3::UnitY;
contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, TF_A.GetInverse().orientation); contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, INV_TF_A.orientation);
contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, TF_B.GetInverse().orientation); contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, INV_TF_B.orientation);
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;
} }
else else
{ {
manifold.normal = SHVec3::Normalise(A_TO_B); manifold.normal = SHVec3::Normalise(A_TO_B);
contact.localPointA = RADIUS_A * SHVec3::Normalise(TF_A.GetInverse() * CENTER_B); contact.localPointA = RADIUS_A * SHVec3::Normalise(INV_TF_A * CENTER_B);
contact.localPointB = RADIUS_B * SHVec3::Normalise(TF_B.GetInverse() * CENTER_A); contact.localPointB = RADIUS_B * SHVec3::Normalise(INV_TF_B * CENTER_A);
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;

14
SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp
View File

@ -130,8 +130,8 @@ namespace SHADE
* Right: 1 (1,5,6,2) Normal: X * Right: 1 (1,5,6,2) Normal: X
* Back: 2 (5,4,7,6) Normal: Z * Back: 2 (5,4,7,6) Normal: Z
* Left: 3 (4,0,3,7) Normal: -X * Left: 3 (4,0,3,7) Normal: -X
* Top: 4 (3,2,6,7) Normal: Y * Bottom: 4 (0,4,5,1) Normal: -Y
* Bottom: 5 (4,5,1,0) Normal: -Y * Top: 5 (2,6,7,3) Normal: Y
* *
*/ */
@ -143,18 +143,18 @@ namespace SHADE
, SHVec3::UnitX , SHVec3::UnitX
, SHVec3::UnitZ , SHVec3::UnitZ
, -SHVec3::UnitX , -SHVec3::UnitX
, SHVec3::UnitY
, -SHVec3::UnitY , -SHVec3::UnitY
, SHVec3::UnitY
}; };
const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE] const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
{ {
{ 0, 1, 2, 3 } { 0, 1, 2, 3 }
, { 5, 6, 2, 1 } , { 1, 5, 6, 2 }
, { 5, 4, 7, 6 } , { 4, 7, 6, 5 }
, { 0, 3, 7, 4 } , { 4, 0, 3, 7 }
, { 0, 4, 5, 1 }
, { 2, 6, 7, 3 } , { 2, 6, 7, 3 }
, { 5, 1, 0, 4 }
}; };
for (int i = 0; i < NUM_FACES; ++i) for (int i = 0; i < NUM_FACES; ++i)

11
SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp
View File

@ -52,18 +52,15 @@ namespace SHADE
, .normal = manifold.normal , .normal = manifold.normal
}; };
const auto* SHAPE_A = manifold.shapeA;
const auto* SHAPE_B = manifold.shapeB; const auto* SHAPE_B = manifold.shapeB;
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
for (uint32_t i = 0; i < manifold.numContacts; ++i) for (uint32_t i = 0; i < manifold.numContacts; ++i)
{ {
const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA; const SHVec3 WORLD_POINT_A = TF_B * manifold.contacts[i].localPointA;
const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB; const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB;
collisionEvent.contactPoints[i] = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f); collisionEvent.contactPoints[i] = WORLD_POINT_A;
} }
collisionEvents.emplace_back(collisionEvent); collisionEvents.emplace_back(collisionEvent);
@ -92,12 +89,12 @@ namespace SHADE
for (uint32_t i = 0; i < manifold.numContacts; ++i) for (uint32_t i = 0; i < manifold.numContacts; ++i)
{ {
// Contact position will be the middle of worldPointA & worldPointB // Contact position will be the middle of worldPointA & worldPointB
const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA; const SHVec3 WORLD_POINT_A = TF_B * manifold.contacts[i].localPointA;
const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB; const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB;
const ContactInfo INFO const ContactInfo INFO
{ {
.position = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f) .position = WORLD_POINT_A
, .normal = manifold.normal , .normal = manifold.normal
}; };

13
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
View File

@ -100,8 +100,7 @@ namespace SHADE
// Collision data // Collision data
newConstraint.numContacts = manifold.numContacts; newConstraint.numContacts = manifold.numContacts;
newConstraint.normal = manifold.normal;
newConstraint.normal = manifold.normal;
static constexpr size_t TANGENTS_SIZE = sizeof(SHVec3) * SHContact::NUM_TANGENTS; static constexpr size_t TANGENTS_SIZE = sizeof(SHVec3) * SHContact::NUM_TANGENTS;
static constexpr size_t CONTACTS_SIZE = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS; static constexpr size_t CONTACTS_SIZE = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS;
@ -109,14 +108,14 @@ namespace SHADE
memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE); memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE);
memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE); memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE);
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; // Compute rA & rB for contacts with the local centroids
const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; const SHVec3 LOCAL_COM_A = BODY_A ? BODY_A->localCentroid : SHVec3::Zero;
const SHVec3 LOCAL_COM_B = BODY_B ? BODY_B->localCentroid : SHVec3::Zero;
// Compute rA & rB for contacts
for (uint32_t i = 0; i < newConstraint.numContacts; ++i) for (uint32_t i = 0; i < newConstraint.numContacts; ++i)
{ {
newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA; newConstraint.contacts[i].rA = newConstraint.contacts[i].localPointA - LOCAL_COM_A;
newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB; newConstraint.contacts[i].rB = newConstraint.contacts[i].localPointB - LOCAL_COM_B;
} }
} }

2
SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp
View File

@ -80,7 +80,7 @@ namespace SHADE
for (auto& contactPoint : CONTACT_POINTS) for (auto& contactPoint : CONTACT_POINTS)
{ {
const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f }); const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f });
debugDrawSystem->DrawSphere(TRS, CONTACT_COLOUR); debugDrawSystem->DrawWireSphere(TRS, CONTACT_COLOUR);
debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.5f, CONTACT_COLOUR, true); debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.5f, CONTACT_COLOUR, true);
} }
} }