Solved edge case for sphere vs convex polyhedron

This commit is contained in:
Diren D Bharwani 2023-01-01 02:42:44 +08:00
parent 3a7336fe15
commit 00f8726e46
2 changed files with 72 additions and 72 deletions

View File

@ -5,7 +5,7 @@
Components: Components:
Transform Component: Transform Component:
Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883} Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883}
Rotate: {x: -1.48352849, y: -4.78713309e-06, z: 0.469859391} Rotate: {x: 0, y: 0, z: 0.436332315}
Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722} Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
@ -66,7 +66,7 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: -2.49145222, y: 6.17996597, z: 0.811235607} Translate: {x: -2.01111817, y: 7, z: 0.695722342}
Rotate: {x: -0, y: 0, z: -0} Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 1, y: 1, z: 1}
IsActive: true IsActive: true
@ -112,7 +112,7 @@
Components: Components:
Transform Component: Transform Component:
Translate: {x: 0, y: 4.09544182, z: 0} Translate: {x: 0, y: 4.09544182, z: 0}
Rotate: {x: -0, y: 0, z: 0} Rotate: {x: -0, y: 0, z: -0.436332315}
Scale: {x: 4.61071014, y: 0.999995887, z: 1} Scale: {x: 4.61071014, y: 0.999995887, z: 1}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:

View File

@ -73,6 +73,7 @@ namespace SHADE
contact.featurePair.key = 0; contact.featurePair.key = 0;
// Check if center is inside polyhedron (below the face) // Check if center is inside polyhedron (below the face)
// TODO: Change to a point in polygon test.
if (FACE_QUERY.bestDistance < SHMath::EPSILON) if (FACE_QUERY.bestDistance < SHMath::EPSILON)
{ {
manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
@ -116,112 +117,111 @@ namespace SHADE
// 2. Same side as adjacent normal // 2. Same side as adjacent normal
// Check in regions A // Check in regions A
const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
SHVec3 tangent = SHVec3::Normalise(P2 - P1);
float projection = SHVec3::Dot(P1_TO_CENTER, tangent);
if (projection >= 0.0f)
{ {
const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; // Find closest point
const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); const SHVec3 CP = P1 + projection * tangent;
const SHVec3 TANGENT = SHVec3::Normalise(P2 - P1); // Check 2nd condition
// Get adjacent normal from twin half edge
const int32_t EDGE_INDEX = FACE.vertexIndices[CLOSEST_POINT].edgeIndex;
const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex;
const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
if (projection >= 0.0f) if (projection >= 0.0f)
{ {
// Find closest point // Must be smaller than radius
const SHVec3 CP = P1 + projection * TANGENT; if (projection >= RADIUS)
return false;
// Check 2nd condition const SHVec3 CP_TO_CENTER = CENTER - CP;
// Get adjacent normal from twin half edge
const int32_t EDGE_INDEX = FACE.vertexIndices[CLOSEST_POINT].edgeIndex;
const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
if (projection >= 0.0f) contact.position = CP;
{
// Must be smaller than radius
if (projection >= RADIUS)
return false;
const SHVec3 CP_TO_CENTER = CENTER - CP; manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); return true;
contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
contact.position = CP;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
} }
} }
// Check in region B // Check in region B
const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
tangent = SHVec3::Normalise(P3 - P1);
projection = SHVec3::Dot(P1_TO_CENTER, tangent);
if (projection >= 0.0f)
{ {
const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; // Find closest point
const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); const SHVec3 CP = P1 + projection * tangent;
const SHVec3 TANGENT = SHVec3::Normalise(P3 - P1); // Check 2nd condition
// Get adjacent normal from twin half edge
const int32_t EDGE_INDEX = FACE.vertexIndices[P3_INDEX].edgeIndex;
const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex;
const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
if (projection >= 0.0f) if (projection >= 0.0f)
{ {
// Find closest point // Must be smaller than radius
const SHVec3 CP = P1 + projection * TANGENT; if (projection >= RADIUS)
return false;
// Check 2nd condition const SHVec3 CP_TO_CENTER = CENTER - CP;
// Get adjacent normal from twin half edge
const int32_t EDGE_INDEX = FACE.vertexIndices[P3_INDEX].edgeIndex;
const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
if (projection >= 0.0f) contact.position = CP;
{
// Must be smaller than radius
if (projection >= RADIUS)
return false;
const SHVec3 CP_TO_CENTER = CENTER - CP; manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); return true;
contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
contact.position = CP;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
} }
} }
// Either region C or D. Take whichever depth is smaller // Region C has a negative dot product with any of the tangents.
const float C_PENETRATION = RADIUS - P1_TO_CENTER.Length(); projection = SHVec3::Dot(P1_TO_CENTER, tangent);
if (projection < 0)
if (std::fabs(C_PENETRATION) < RADIUS && std::fabs(C_PENETRATION) < PENETRATION)
{ {
manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
contact.penetration = C_PENETRATION; contact.penetration = RADIUS - P1_TO_CENTER.Length();
contact.position = P1; contact.position = P1;
}
else
{
manifold.normal = -FACE_NORMAL;
contact.penetration = PENETRATION; manifold.contacts[numContacts++] = contact;
contact.position = CENTER - FACE_NORMAL * RADIUS; manifold.numContacts = numContacts;
return true;
} }
// Region D
manifold.normal = -FACE_NORMAL;
contact.penetration = PENETRATION;
contact.position = CENTER - FACE_NORMAL * RADIUS;
manifold.contacts[numContacts++] = contact; manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts; manifold.numContacts = numContacts;