Fixed convex-convex face detection

Minor bugs with contact point detection. Will test more before pushing into main
This commit is contained in:
Diren D Bharwani 2023-01-16 15:01:14 +08:00
parent 85f0902c2d
commit c077575a73
4 changed files with 220 additions and 8 deletions

View File

@ -267,9 +267,9 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: 1, y: 2, z: 3} Translate: {x: 1.81218028, y: 2, z: 3}
Rotate: {x: 0, y: 0.785398185, z: 0.785397708} Rotate: {x: 0.785398006, y: 0.785398483, z: 0.785398304}
Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456} Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
Type: Dynamic Type: Dynamic

View File

@ -124,7 +124,7 @@ namespace SHADE
static SHVec3 findClosestPointBetweenEdges (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 int32_t findIncidentFace (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
static std::vector<ClipVertex> clipPolygonWithPlane (const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept; static std::vector<ClipVertex> clipPolygonWithPlane (const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept;
static std::vector<ClipVertex> reduceContacts (const std::vector<ClipVertex>& in) noexcept; static std::vector<int32_t> reduceContacts (const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept;
}; };
} // namespace SHADE } // namespace SHADE

View File

@ -189,8 +189,78 @@ namespace SHADE
} }
// From the final set of clipped points, only keep the points that are below the reference plane. // From the final set of clipped points, only keep the points that are below the reference plane.
const SHPlane REFERENCE_PLANE{ referencePoly->GetVertex(REFERENCE_FACE.vertexIndices.front().index), REFERENCE_NORMAL };
return false; 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;
if (flipNormal)
{
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);
++numContacts;
}
}
// Reduce contact manifold if more than 4 points
if (numContacts > 4)
{
const auto INDICES_TO_KEEP = reduceContacts(contacts, REFERENCE_NORMAL);
std::vector<SHContact> reducedContacts;
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]]);
contacts.clear();
// Copy contacts to main container
for (auto& contact : reducedContacts)
contacts.emplace_back(contact);
}
// 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;
}
}
// 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 = REFERENCE_NORMAL;
if (flipNormal)
manifold.normal = -manifold.normal;
return true;
} }
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -472,5 +542,141 @@ namespace SHADE
return out; return out;
} }
std::vector<int32_t> SHCollision::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]);
// 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;
}
}
indicesToKeep.emplace_back(indexToKeep);
contactMap.erase(indexToKeep);
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;
}
}
indicesToKeep.emplace_back(indexToKeep);
contactMap.erase(indexToKeep);
indexToKeep = -1;
// 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();
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 float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
if (AREA > maxArea)
{
maxArea = AREA;
maxAreaIndex = 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;
}
indicesToKeep.emplace_back(indexToKeep);
contactMap.erase(indexToKeep);
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;
const SHVec3& THIRD_POS = in[indicesToKeep.back()].position;
const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
for (const auto& [index, contact] : contactMap)
{
const SHVec3& Q = contact->position;
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 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;
}
}
}
indicesToKeep.emplace_back(indexToKeep);
return indicesToKeep;
}
} // namespace SHADE } // namespace SHADE

View File

@ -34,11 +34,17 @@ namespace SHADE
*/ */
static constexpr float SHPHYSICS_RESTITUTION_THRESHOLD = 1.0f; static constexpr float SHPHYSICS_RESTITUTION_THRESHOLD = 1.0f;
/** /**
* @brief * @brief
* Scaling factor to control how fast overlaps are resolved.<br/> * Scaling factor to control how fast overlaps are resolved.<br/>
* 1 is ideal for instant correction, but values close to 1 can lead to overshoot. * 1 is ideal for instant correction, but values close to 1 can lead to overshoot.
*/ */
static constexpr float SHPHYSICS_BAUMGARTE = 0.2f; static constexpr float SHPHYSICS_BAUMGARTE = 0.2f;
/**
* @brief
* Distance threshold to consider two contacts as the same.
*/
static constexpr float SHPHYSICS_SAME_CONTACT_DISTANCE = 0.01;
} }