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

Added untested contact point derivation 

Left with reducing manifold with more than 4 contact points
This commit is contained in:
Diren D Bharwani 2023-01-06 21:07:30 +08:00
parent 8ca4045d55
commit 57498bb8b8
2 changed files with 165 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -11,6 +11,7 @@
#pragma once
// Project Headers
#include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Contacts/SHManifold.h"
#include "Physics/Collision/Contacts/SHCollisionKey.h"
#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
@ -101,29 +102,22 @@ namespace SHADE
// Convex VS Convex
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
static float distanceBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static int32_t findIncidentFace (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
/*
* TODO:
*
*
*
*
*
* static uint32_t clip [sutherland-hodgemann clipping]
*
* ! References
* References
* https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
* https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
*/
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
static float distanceBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
static int32_t findIncidentFace (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
static std::vector<SHContact> clipsPolygonAgainstPlane (const std::vector<SHContact>& in, const SHPlane& plane) noexcept;
// TODO: Reduce Manifold
};
} // namespace SHADE

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

@ -128,25 +128,87 @@ namespace SHADE
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
// Create initial set of contacts as the entire incident face
const SHHalfEdgeStructure::Face& INCIDENT_FACE = incidentPoly->GetFace(INCIDENT_FACE_IDX);
const int32_t MAX_NUM_CONTACTS = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
/*
* TODO:
*
* !!
* 4. From above, save the axis of minimum penetration (reference face)
* 5. Find the most anti-parallel face on other shape (incident face)
* 6. Clip incident face against side planes of reference face. (Sutherland-Hodgeman Clipping).
* Keep all vertices below reference face.
* 7. Reduce manifold to 4 contact points. We only need 4 contact points for a stable manifold.
*
* Remember to save IDs in queries.
* During generation of incident face, store IDs of face.
*
*/
std::vector<SHContact> incidentPolygon;
for (const int32_t i : std::views::iota(0, MAX_NUM_CONTACTS))
{
const int32_t VERTEX_INDEX = INCIDENT_FACE.vertexIndices[i].index;
incidentPolygon[i].position = incidentPoly->GetVertex(VERTEX_INDEX);
incidentPolygon[i].featurePair.indexA = minFaceQuery.closestFace;
incidentPolygon[i].featurePair.indexB = VERTEX_INDEX;
incidentPolygon[i].featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE);
incidentPolygon[i].featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
}
return false;
// Number of vertices == number of edges on face
// Clip against each edge of face
const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace);
const int32_t NUM_EDGES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
const size_t COPY_SIZE = sizeof(SHContact) * MAX_NUM_CONTACTS;
for (const int32_t i : std::views::iota(0, NUM_EDGES))
{
// Build a plane on the reference poly normal X tangent and the tail of the tangent
const int32_t V1_IDX = REFERENCE_FACE.vertexIndices[i].index;
const int32_t V2_IDX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_EDGES].index;
const SHVec3 V1 = referencePoly->GetVertex(V1_IDX);
const SHVec3 V2 = referencePoly->GetVertex(V2_IDX);
const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
const SHPlane PLANE{ V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) };
auto clipped = clipsPolygonAgainstPlane(incidentPolygon, PLANE);
memcpy_s(incidentPolygon.data(), COPY_SIZE, clipped.data(), COPY_SIZE);
}
// From clipped polygon, only keep points that are below the reference face
// The penetration is the signed distance to the plane formed by the reference face.
const SHPlane REFERENCE_PLANE{ referencePoly->GetVertex(REFERENCE_FACE.vertexIndices[0].index), REFERENCE_NORMAL };
std::vector<SHContact> finalContacts;
for (const auto& cp : incidentPolygon)
{
const float DEPTH = REFERENCE_PLANE.SignedDistance(cp.position);
if (DEPTH <= 0.0f)
{
SHContact contact;
contact.position = cp.position;
contact.penetration = -DEPTH;
if (flipNormal)
{
contact.featurePair.indexA = cp.featurePair.indexB;
contact.featurePair.indexB = cp.featurePair.indexA;
contact.featurePair.typeA = cp.featurePair.typeB;
contact.featurePair.typeB = cp.featurePair.typeA;
}
finalContacts.emplace_back(contact);
++numContacts;
}
}
if (numContacts > 4)
{
// TODO: Reduce the manifold
}
else
{
memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, finalContacts.data(), sizeof(SHContact) * numContacts);
}
manifold.normal = flipNormal ? -REFERENCE_NORMAL : REFERENCE_NORMAL;
manifold.numContacts = numContacts;
return true;
}
/*-----------------------------------------------------------------------------------*/
@ -324,6 +386,7 @@ namespace SHADE
const float A2_OVER_A1 = VB_DOT_VB / VB_DOT_VA;
const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
const float R = NUMERATOR / DENOMINATOR;
// Just take a point from A since it's A -> B
@ -352,5 +415,80 @@ namespace SHADE
return bestFace;
}
std::vector<SHContact> SHCollision::clipsPolygonAgainstPlane(const std::vector<SHContact>& in, const SHPlane& plane) noexcept
{
std::vector<SHContact> out;
//
// If both are in front of the plane, keep the head.
// If both are behind the plane, ignore both.
// If they are on different sides, find the position in between that lies on the plane. Keep the head and interpolation position.
/*
* For sutherland-hodgman clipping, we check where any two sets of points lie with respect to the plane
* This variation is has 4 scenarios:
*
* 1. Both behind: ignore
* 2. Head behind, tail in-front: Keep interpolated position
* 3. Both in-front: Keep tail
* 4. Head in-front, tail behind: Keep head & interpolated position
*/
const size_t NUM_POINTS = in.size();
for (size_t i = 0; i < NUM_POINTS; ++i)
{
const SHContact& HEAD = in[i];
const SHContact& TAIL = in[(i + 1) % NUM_POINTS];
const float HEAD_DISTANCE = plane.SignedDistance(HEAD.position);
const float TAIL_DISTANCE = plane.SignedDistance(TAIL.position);
// Scenario 1: Both Behind
if (HEAD_DISTANCE < 0.0f && TAIL_DISTANCE < 0.0f)
continue;
// Scenario 2: Tail In-Front, Head Behind
if (HEAD_DISTANCE >= 0.0f && TAIL_DISTANCE < 0.0f)
{
SHContact interpolated;
interpolated.featurePair.indexA = TAIL.featurePair.indexA;
interpolated.featurePair.indexB = TAIL.featurePair.indexB;
interpolated.featurePair.typeA = TAIL.featurePair.typeB;
interpolated.featurePair.typeB = TAIL.featurePair.typeA;
const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position);
interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE);
out.emplace_back(interpolated);
}
// Scenario 3: Both In-Front
if (HEAD_DISTANCE >= 0.0f && TAIL_DISTANCE >= 0.0f)
{
out.emplace_back(TAIL);
}
// Scenario 4: Head Behind, Tail In-Front
if (HEAD_DISTANCE < 0.0f && TAIL_DISTANCE >= 0.0f)
{
out.emplace_back(HEAD);
SHContact interpolated;
interpolated.featurePair.indexA = TAIL.featurePair.indexA;
interpolated.featurePair.indexB = TAIL.featurePair.indexB;
interpolated.featurePair.typeA = TAIL.featurePair.typeB;
interpolated.featurePair.typeB = TAIL.featurePair.typeA;
const SHVec3 DIR = SHVec3::Normalise(HEAD.position - TAIL.position);
interpolated.position = TAIL.position + DIR * std::fabs(TAIL_DISTANCE);
out.emplace_back(interpolated);
}
}
return out;
}
} // namespace SHADE