This commit is contained in:
Diren D Bharwani 2023-01-01 03:24:34 +08:00
parent 67907b1ca9
commit f3c0bdbcfd
5 changed files with 144 additions and 133 deletions

View File

@ -66,7 +66,7 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: -0.903104782, y: 7, z: -0.782080948}
Translate: {x: -1.96328545, y: 7, z: 0.743723333}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true

View File

@ -97,4 +97,28 @@ namespace SHADE
{
return halfEdgeStructure;
}
int32_t SHConvexPolyhedronCollisionShape::GetFaceCount() const noexcept
{
return halfEdgeStructure->GetFaceCount();
}
int32_t SHConvexPolyhedronCollisionShape::GetHalfEdgeCount() const noexcept
{
return halfEdgeStructure->GetHalfEdgeCount();
}
const SHHalfEdgeStructure::Face& SHConvexPolyhedronCollisionShape::GetFace(int index) const
{
// Assume it has already been initialised
return halfEdgeStructure->GetFace(index);
}
const SHHalfEdgeStructure::HalfEdge& SHConvexPolyhedronCollisionShape::GetHalfEdge(int index) const
{
// Assume it has already been initialised
return halfEdgeStructure->GetHalfEdge(index);
}
} // namespace SHADE

View File

@ -57,9 +57,14 @@ namespace SHADE
/* Getter Functions */
/*---------------------------------------------------------------------------------*/
[[nodiscard]] const SHHalfEdgeStructure* GetHalfEdgeStructure () const noexcept;
[[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0;
[[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0;
[[nodiscard]] const SHHalfEdgeStructure* GetHalfEdgeStructure () const noexcept;
[[nodiscard]] int32_t GetFaceCount () const noexcept;
[[nodiscard]] const SHHalfEdgeStructure::Face& GetFace (int index) const;
[[nodiscard]] int32_t GetHalfEdgeCount () const noexcept;
[[nodiscard]] const SHHalfEdgeStructure::HalfEdge& GetHalfEdge (int index) const;
[[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0;
[[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0;
protected:
/*---------------------------------------------------------------------------------*/

View File

@ -86,6 +86,7 @@ namespace SHADE
static FaceQuery findClosestFace (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron) noexcept;
static int32_t findClosestPoint (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron, int32_t faceIndex) noexcept;
static int32_t findVoronoiRegion (const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept;
static bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;

View File

@ -33,8 +33,6 @@ namespace SHADE
const SHVec3 CENTER = SPHERE.GetCenter();
const float RADIUS = SPHERE.GetWorldRadius();
const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure();
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding)
return false;
@ -60,8 +58,6 @@ namespace SHADE
const SHVec3 CENTER = SPHERE.GetCenter();
const float RADIUS = SPHERE.GetWorldRadius();
const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure();
const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
if (!FACE_QUERY.colliding)
return false;
@ -83,147 +79,75 @@ namespace SHADE
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
}
// Find closest face of polygon to circle
const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetHalfEdgeStructure()->GetFace(FACE_QUERY.closestFace);
const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
const SHVec3& FACE_NORMAL = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
// Check against voronoi regions of the face to determine the type of the intersection test
// We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
// If none of these are true, the sphere is above the face but not separating
/*
* | 2
* _ _ _ _ _ _ | _ _ _
* / /
* | / regionD | / regionA
* |/ _ _ _ _ _|/ _ _ _
* 3/ regionB /1
* / / regionC
*
*/
// Get points and build tangents
const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
const SHVec3 P1_TO_CENTER = CENTER - P1;
// To be inside either region A or B, 2 conditions must be satisfied
// 1. Same side as tangent
// 2. Same side as adjacent normal
// 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)
{
// Find closest point
const SHVec3 CP = P1 + projection * tangent;
// Check 2nd condition
// Get adjacent normal from the cross product (tangent x normal)
//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;
const SHVec3 ADJ_NORMAL = SHVec3::Cross(tangent, FACE_NORMAL);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
if (projection >= 0.0f)
{
// Must be smaller than radius
if (projection >= RADIUS)
return false;
const SHVec3 CP_TO_CENTER = CENTER - CP;
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
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
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);
const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
projection = SHVec3::Dot(P1_TO_CENTER, tangent);
// Get the voronoi region it belongs in
const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
if (REGION == 0)
return false;
if (projection >= 0.0f)
// Create contact information based on region
const SHVec3 P1_TO_CENTER = CENTER - P1;
switch (REGION)
{
// Find closest point
const SHVec3 CP = P1 + projection * tangent;
// Check 2nd condition
// Get adjacent normal from the cross product (normal x tangent)
//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;
//const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE);
const SHVec3 ADJ_NORMAL = SHVec3::Cross(FACE_NORMAL, tangent);
projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
if (projection >= 0.0f)
case 1: // Region A
case 2: // Region B
{
// Must be smaller than radius
if (projection >= RADIUS)
return false;
// Find closest point
const SHVec3& TANGENT = REGION == 1 ? TANGENT_1 : TANGENT_2;
const SHVec3 CP = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
const SHVec3 CP_TO_CENTER = CENTER - CP;
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
contact.position = CP;
contact.position = CP;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
return true;
break;
}
case 3: // Region C
{
manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
contact.penetration = RADIUS - P1_TO_CENTER.Length();
contact.position = P1;
break;
}
case 4: // Region D
{
manifold.normal = -FACE_NORMAL;
contact.penetration = PENETRATION;
contact.position = CENTER - FACE_NORMAL * RADIUS;
break;
}
default: return false; // Should never happen
}
// Region C has a negative dot product with any of the tangents.
projection = SHVec3::Dot(P1_TO_CENTER, tangent);
if (projection < 0)
{
manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
contact.penetration = RADIUS - P1_TO_CENTER.Length();
contact.position = P1;
manifold.contacts[numContacts++] = contact;
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.numContacts = numContacts;
@ -246,8 +170,6 @@ namespace SHADE
const SHVec3 CENTER = sphere.GetCenter();
const float RADIUS = sphere.GetWorldRadius();
const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure();
/*
* Test against each face.
*
@ -255,9 +177,9 @@ namespace SHADE
* 2. Find the signed distance from plane to center of sphere.
* 3. Save best distance and face.
*/
for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i)
for (int32_t i = 0; i < polyhedron.GetFaceCount(); ++i)
{
const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i);
const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(i);
// Build plane equation
@ -291,12 +213,8 @@ namespace SHADE
int32_t closestPointIndex = -1;
const SHVec3 CENTER = sphere.GetCenter();
const float RADIUS = sphere.GetWorldRadius();
const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure();
const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(faceIndex);
const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(faceIndex);
const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size());
float smallestDist = std::numeric_limits<float>::max();
@ -315,4 +233,67 @@ namespace SHADE
return closestPointIndex;
}
int32_t SHCollision::findVoronoiRegion(const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept
{
static constexpr int NUM_TANGENTS = 2;
// Check against voronoi regions of the face to determine the type of the intersection test
// We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
// If none of these are true, the sphere is above the face but not separating
/*
* | 2
* _ _ _ _ _ _ | _ _ _
* / /
* | / regionD | / regionA
* |/ _ _ _ _ _|/ _ _ _
* 3/ regionB /1
* / / regionC
*
*/
const SHVec3& CENTER = sphere.GetCenter();
const float RADIUS = sphere.GetWorldRadius();
const SHVec3 TANGENTS [NUM_TANGENTS] { tangent1, tangent2 };
const SHVec3 ADJACENT_NORMALS [NUM_TANGENTS] { SHVec3::Cross(tangent1, faceNormal), SHVec3::Cross(faceNormal, tangent2) };
const SHVec3 FACE_TO_CENTER = CENTER - faceVertex;
// To be inside either region A or B, 2 conditions must be satisfied
// 1. Same side as tangent
// 2. Same side as adjacent normal
// Check Region A & B
for (int i = 0; i < NUM_TANGENTS; ++i)
{
float projection = SHVec3::Dot(FACE_TO_CENTER, TANGENTS[i]);
if (projection >= 0.0f)
{
// Find closest point
const SHVec3 CLOSEST_POINT = faceVertex + projection * TANGENTS[i];
projection = SHVec3::Dot(FACE_TO_CENTER, ADJACENT_NORMALS[i]);
if (projection >= 0.0f)
{
if (projection > RADIUS)
return 0;
// Region 1 or 2 ( A or B)
return i + 1;
}
}
}
// Check Region C
// Face to vertex is in the opposite direction of any tangent.
const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1);
if (PROJECTION < 0)
return 3;
// Belongs in region D by default
return 4;
}
} // namespace SHADE