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:
Translate: {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
RigidBody Component:
Type: Static
Type: Dynamic
Auto Mass: false
Mass: .inf
Mass: 1
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: false
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
@ -55,45 +79,4 @@
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
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: ~

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;
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;
//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 */
@ -67,140 +67,143 @@ 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;
static constexpr float RELATIVE_TOLERANCE = 0.95f;
//const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
//const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
//if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID)
// return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B);
const SHCollisionUtils::ShapeTransform TF_A { POLY_A.GetWorldCentroid(), POLY_A.GetWorldOrientation() };
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);
//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;
SHSATInfo cachedInfo;
// 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);
//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::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);
//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;
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.
//// 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;
return false;
}
//SHCollisionUtils::FaceQuery minFaceQuery;
//if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
//{
// minFaceQuery = FACE_QUERY_A;
// referencePoly = &POLY_A;
// incidentPoly = &POLY_B;
// flipNormal = 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.
bool flipNormal = false;
const SHConvexPolyhedron* referencePoly = nullptr;
const SHConvexPolyhedron* incidentPoly = nullptr;
// cachedInfo.type = SHSATInfo::Type::FACE;
// cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
// cachedInfo.info.refFace = minFaceQuery.closestFace;
//}
//else
//{
// minFaceQuery = FACE_QUERY_B;
// referencePoly = &POLY_B;
// incidentPoly = &POLY_A;
// flipNormal = true;
SHCollisionUtils::FaceQuery minFaceQuery;
if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
{
minFaceQuery = FACE_QUERY_A;
referencePoly = &POLY_A;
incidentPoly = &POLY_B;
flipNormal = false;
// cachedInfo.type = SHSATInfo::Type::FACE;
// cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
// cachedInfo.info.refFace = minFaceQuery.closestFace;
//}
cachedInfo.type = SHSATInfo::Type::FACE;
cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
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
//// each edge and use that as the contact point.
//if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
//{
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
// const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
// If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
// each edge and use that as the contact point.
if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
{
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
// const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex);
// const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex);
// const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex);
// const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex);
const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex);
const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex);
const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex);
const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex);
// const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
// const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
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;
manifold.normal = SHVec3::Cross(VB, VA);
// Flip normal if need to ( A -> B)
if (SHVec3::Dot(manifold.normal, TAIL_A - A.GetWorldCentroid()) < 0.0f)
manifold.normal = -manifold.normal;
// // In this scenario, we only have one contact
// uint32_t numContacts = 0;
// In this scenario, we only have one contact
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;
// contact.featurePair.key = EDGE_QUERY.axis;
// contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f);
// contact.penetration = -EDGE_QUERY.bestDistance;
SHContact contact;
contact.featurePair.key = EDGE_QUERY.axis;
contact.penetration = -EDGE_QUERY.bestDistance;
// manifold.contacts[numContacts++] = contact;
// manifold.numContacts = numContacts;
contact.localPointA = INV_TF_A * CONTACTS.closestPointB;
contact.localPointB = INV_TF_B * CONTACTS.closestPointA;
// // Cache the info
// cachedInfo.type = SHSATInfo::Type::EDGE;
// cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
// cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
// 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);
//const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
return true;
}
//manifold.cachedSATInfo = cachedInfo;
//return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
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 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;
}
@ -311,17 +315,17 @@ namespace SHADE
const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B);
// Check if the edges are parallel (abs dot product is 1)
const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B));
if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f))
// Check if the edges are parallel (cross product length close to 0)
if (SHMath::CompareFloat(SHVec3::Cross(DIR_A, DIR_B).LengthSquared(), 0.0f))
return std::numeric_limits<float>::lowest();
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;
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
@ -347,7 +351,7 @@ namespace SHADE
const float A = D1.LengthSquared(); // a
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 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
{
//const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace);
//const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace);
const SHCollisionUtils::ShapeTransform TF_INC { incPoly.GetWorldCentroid(), incPoly.GetWorldOrientation() };
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
//SHCollisionUtils::ClipVertices clipIn;
//clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{});
const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace);
const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace);
//int32_t numClipIn = 0;
//for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
//{
// const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
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());
// 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
// // The outgoing id is the current edge (where this vertex is the tail of)
// Build incoming vertices to clip
// But it in the reference local-space
SHCollisionUtils::ClipVertices clipIn;
clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{});
// SHCollisionUtils::ClipVertex v;
// v.position = incPoly.GetVertex(V_INDEX);
// v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex;
// v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex;
// v.featurePair.inR = 0;
// v.featurePair.outR = 0;
int32_t numClipIn = 0;
for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
{
const int32_t PREV_I = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
// 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.
//// Number of side planes == number of edges == number of vertices
//const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace);
//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)
SHCollisionUtils::ClipVertex v;
v.position = INV_TF_REF * incPoly.GetVertex(V_INDEX);
v.featurePair.inI = INCIDENT_FACE.vertexIndices[PREV_I].edgeIndex;
v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex;
v.featurePair.inR = 0;
v.featurePair.outR = 0;
// const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
// const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
clipIn[numClipIn++] = v;
}
// const SHVec3 V1 = refPoly.GetVertex(V1_INDEX);
// const SHVec3 V2 = refPoly.GetVertex(V2_INDEX);
// Clip the vertices against the reference face side planes.
// 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 SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) };
const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
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);
// if (clipOut.empty())
// return false;
// Get reference vertices in reference local space
const SHVec3 V1 = INV_TF_REF * refPoly.GetVertex(V1_INDEX);
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 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;
//}
const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
const SHVec3 PLANE_NORMAL = SHVec3::Cross(LOCAL_REF_NORMAL, TANGENT);
const SHPlane CLIP_PLANE { V1, PLANE_NORMAL };
//// From the final set of clipped points, only keep the points that are below the reference plane.
//const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL };
SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
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;
//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.position = POS;
// contact.penetration = -DIST;
// From the final set of clipped points, only keep the points that are below the reference plane.
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 };
// if (flip)
// {
// 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;
// }
uint32_t numContacts = 0;
// contacts.emplace_back(contact);
// ++numContacts;
// }
//}
std::vector<SHContact> contacts;
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
//if (numContacts > 4)
//{
// const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL);
// std::vector<SHContact> reducedContacts;
// A: Incident, B: Reference
// Project the clipped point onto the reference place for localPointB
contact.localPointA = INV_TF_INC * (TF_REF * POS);
contact.localPointB = POS + LOCAL_REF_NORMAL * SHVec3::Dot(LOCAL_REF_NORMAL, POS - LOCAL_REF_POINT);
// const int32_t NUM_REDUCED = static_cast<int32_t>(INDICES_TO_KEEP.size());
// for (int32_t i = 0; i < NUM_REDUCED; ++i)
// reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]);
if (flip)
{
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();
// // Copy contacts to main container
// for (auto& contact : reducedContacts)
// contacts.emplace_back(contact);
//}
std::swap(contact.localPointA, contact.localPointB);
}
else
{
contact.featurePair.key = clipIn[i].featurePair.key;
}
//// 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->position, j->position);
// if (D2 < THRESHOLD)
// j = contacts.erase(j);
// else
// ++j;
// }
//}
contacts.emplace_back(contact);
++numContacts;
}
}
//// 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];
// Reduce contact manifold if more than 4 points
if (numContacts > 4)
{
const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL);
std::vector<SHContact> reducedContacts;
//manifold.numContacts = numContacts;
//manifold.normal = REF_NORMAL;
//if (flip)
// manifold.normal = -manifold.normal;
const int32_t NUM_REDUCED = static_cast<int32_t>(INDICES_TO_KEEP.size());
for (int32_t i = 0; i < NUM_REDUCED; ++i)
reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]);
//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;
const SHVec3 PLANE_NORMAL = -plane.GetNormal();
// 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 ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos);
const float DOT = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL);
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.featurePair.inI = in[v1Index].featurePair.outI;
@ -610,9 +632,12 @@ namespace SHADE
{
SHCollisionUtils::ClipVertex intersection;
// 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 ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos);
const SHVec3 PLANE_NORMAL = plane.GetNormal();
// 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.featurePair.inI = 0;
@ -638,220 +663,225 @@ namespace SHADE
return out;
}
//std::vector<int32_t> reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept
//{
// std::vector<int32_t> indicesToKeep;
std::vector<int32_t> reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept
{
std::vector<int32_t> indicesToKeep;
// // Use a map to temporarily store and track the contacts we want
// std::unordered_map<int, const SHContact*> contactMap;
// const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size());
// for (int32_t i = 0; i < NUM_CONTACTS; ++i)
// contactMap.emplace(i, &in[i]);
// Use a map to temporarily store and track the contacts we want
std::unordered_map<int, const SHContact*> contactMap;
const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size());
for (int32_t i = 0; i < NUM_CONTACTS; ++i)
contactMap.emplace(i, &in[i]);
// // Find the furthest point in a given direction
// int32_t indexToKeep = -1;
// float bestDistance = std::numeric_limits<float>::lowest();
// Find the furthest point in a given direction
int32_t indexToKeep = -1;
float bestDistance = std::numeric_limits<float>::lowest();
// for (const auto& [index, contact] : contactMap)
// {
// const float DIST = SHVec3::Dot(contact->position, SHVec3::One);
// if (DIST > bestDistance)
// {
// bestDistance = DIST;
// indexToKeep = index;
// }
// }
for (const auto& [index, contact] : contactMap)
{
const float DIST = SHVec3::Dot(contact->localPointA, SHVec3::One);
if (DIST > bestDistance)
{
bestDistance = DIST;
indexToKeep = index;
}
}
// indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep);
indicesToKeep.emplace_back(indexToKeep);
contactMap.erase(indexToKeep);
// indexToKeep = -1;
// bestDistance = std::numeric_limits<float>::lowest();
indexToKeep = -1;
bestDistance = std::numeric_limits<float>::lowest();
// // Find point furthest away from the first index
// const SHVec3& FIRST_POS = in[indicesToKeep.back()].position;
// for (const auto& [index, contact] : contactMap)
// {
// const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->position);
// if (DIST_SQUARED > bestDistance)
// {
// bestDistance = DIST_SQUARED;
// indexToKeep = index;
// }
// }
// Find point furthest away from the first index
const SHVec3& FIRST_POS = in[indicesToKeep.back()].localPointA;
for (const auto& [index, contact] : contactMap)
{
const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->localPointA);
if (DIST_SQUARED > bestDistance)
{
bestDistance = DIST_SQUARED;
indexToKeep = index;
}
}
// indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep);
indicesToKeep.emplace_back(indexToKeep);
contactMap.erase(indexToKeep);
// indexToKeep = -1;
indexToKeep = -1;
// // We compute the triangle with the largest area.
// // The area can be positive or negative depending on the winding order.
// We compute the triangle with the largest area.
// The area can be positive or negative depending on the winding order.
// float maxArea = std::numeric_limits<float>::lowest();
// float minArea = std::numeric_limits<float>::max();
float maxArea = std::numeric_limits<float>::lowest();
float minArea = std::numeric_limits<float>::max();
// int32_t maxAreaIndex = -1;
// int32_t minAreaIndex = -1;
int32_t maxAreaIndex = -1;
int32_t minAreaIndex = -1;
// const SHVec3& SECOND_POS = in[indicesToKeep.back()].position;
// for (const auto& [index, contact] : contactMap)
// {
// const SHVec3& POS = contact->position;
// const SHVec3 TO_P1 = FIRST_POS - POS;
// const SHVec3 TO_P2 = SECOND_POS - POS;
const SHVec3& SECOND_POS = in[indicesToKeep.back()].localPointA;
for (const auto& [index, contact] : contactMap)
{
const SHVec3& POS = contact->localPointA;
const SHVec3 TO_P1 = FIRST_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)
// {
// maxArea = AREA;
// maxAreaIndex = index;
// }
if (AREA > maxArea)
{
maxArea = AREA;
maxAreaIndex = index;
}
// if (AREA < minArea)
// {
// minArea = AREA;
// minAreaIndex = index;
// }
// }
if (AREA < minArea)
{
minArea = AREA;
minAreaIndex = index;
}
}
// // Compare which triangle creates the largest area
// bool isAreaPositive = false;
// if (maxArea > (-minArea))
// {
// isAreaPositive = true;
// indexToKeep = maxAreaIndex;
// }
// else
// {
// isAreaPositive = false;
// indexToKeep = minAreaIndex;
// }
// Compare which triangle creates the largest area
bool isAreaPositive = false;
if (maxArea > (-minArea))
{
isAreaPositive = true;
indexToKeep = maxAreaIndex;
}
else
{
isAreaPositive = false;
indexToKeep = minAreaIndex;
}
// indicesToKeep.emplace_back(indexToKeep);
// contactMap.erase(indexToKeep);
indicesToKeep.emplace_back(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
// // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa.
// float bestArea = 0.0f;
// 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.
float bestArea = 0.0f;
// const SHVec3& THIRD_POS = in[indicesToKeep.back()].position;
// const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
const SHVec3& THIRD_POS = in[indicesToKeep.back()].localPointA;
const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
// for (const auto& [index, contact] : contactMap)
// {
// const SHVec3& Q = contact->position;
for (const auto& [index, contact] : contactMap)
{
const SHVec3& Q = contact->localPointA;
// for (int i = 0; i < 3; ++i)
// {
// const int P1 = i;
// const int P2 = (i + 1) % 3;
for (int i = 0; i < 3; ++i)
{
const int P1 = i;
const int P2 = (i + 1) % 3;
// const SHVec3 TO_P1 = ABC[P1] - Q;
// const SHVec3 TO_P2 = ABC[P2] - Q;
const SHVec3 TO_P1 = ABC[P1] - 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)
// {
// bestArea = AREA;
// indexToKeep = index;
// }
if (isAreaPositive && AREA < bestArea)
{
bestArea = AREA;
indexToKeep = index;
}
// if (!isAreaPositive && AREA > bestArea)
// {
// bestArea = AREA;
// indexToKeep = index;
// }
// }
// }
if (!isAreaPositive && AREA > bestArea)
{
bestArea = AREA;
indexToKeep = index;
}
}
}
// indicesToKeep.emplace_back(indexToKeep);
// return indicesToKeep;
//}
indicesToKeep.emplace_back(indexToKeep);
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!")
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;
// 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;
// }
// 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 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 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);
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;
if (DISTANCE > 0.0f)
return false;
// // Find the incident face
// const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL);
// 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);
//}
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;
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 SHCollisionUtils::ShapeTransform TF_A { A.GetWorldCentroid(), A.GetWorldOrientation() };
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 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 SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA);
const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB);
// const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
// const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
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);
// 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;
const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
// // In this scenario, we only have one contact
// uint32_t numContacts = 0;
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;
// 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;
// // 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;
const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
// manifold.contacts[numContacts++] = contact;
// manifold.numContacts = numContacts;
SHContact contact;
// 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.
return false;
}
} // namespace SHADE

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

@ -238,7 +238,7 @@ namespace SHADE
// Build plane equation
// 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
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_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
@ -83,16 +83,16 @@ namespace SHADE
if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f))
{
manifold.normal = SHVec3::UnitY;
contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, TF_A.GetInverse().orientation);
contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, TF_B.GetInverse().orientation);
contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, INV_TF_A.orientation);
contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, INV_TF_B.orientation);
manifold.contacts[numContacts++] = contact;
}
else
{
manifold.normal = SHVec3::Normalise(A_TO_B);
contact.localPointA = RADIUS_A * SHVec3::Normalise(TF_A.GetInverse() * CENTER_B);
contact.localPointB = RADIUS_B * SHVec3::Normalise(TF_B.GetInverse() * CENTER_A);
contact.localPointA = RADIUS_A * SHVec3::Normalise(INV_TF_A * CENTER_B);
contact.localPointB = RADIUS_B * SHVec3::Normalise(INV_TF_B * CENTER_A);
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
* Back: 2 (5,4,7,6) Normal: Z
* Left: 3 (4,0,3,7) Normal: -X
* Top: 4 (3,2,6,7) Normal: Y
* Bottom: 5 (4,5,1,0) Normal: -Y
* Bottom: 4 (0,4,5,1) Normal: -Y
* Top: 5 (2,6,7,3) Normal: Y
*
*/
@ -143,18 +143,18 @@ namespace SHADE
, SHVec3::UnitX
, SHVec3::UnitZ
, -SHVec3::UnitX
, SHVec3::UnitY
, -SHVec3::UnitY
, SHVec3::UnitY
};
const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
{
{ 0, 1, 2, 3 }
, { 5, 6, 2, 1 }
, { 5, 4, 7, 6 }
, { 0, 3, 7, 4 }
, { 1, 5, 6, 2 }
, { 4, 7, 6, 5 }
, { 4, 0, 3, 7 }
, { 0, 4, 5, 1 }
, { 2, 6, 7, 3 }
, { 5, 1, 0, 4 }
};
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
};
const auto* SHAPE_A = manifold.shapeA;
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() };
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;
collisionEvent.contactPoints[i] = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f);
collisionEvent.contactPoints[i] = WORLD_POINT_A;
}
collisionEvents.emplace_back(collisionEvent);
@ -92,12 +89,12 @@ namespace SHADE
for (uint32_t i = 0; i < manifold.numContacts; ++i)
{
// 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 ContactInfo INFO
{
.position = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f)
.position = WORLD_POINT_A
, .normal = manifold.normal
};

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

@ -100,8 +100,7 @@ namespace SHADE
// Collision data
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 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.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE);
const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() };
const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() };
// Compute rA & rB for contacts with the local centroids
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)
{
newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA;
newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB;
newConstraint.contacts[i].rA = newConstraint.contacts[i].localPointA - LOCAL_COM_A;
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)
{
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);
}
}