Implemented a custom physics engine #316
|
|
@ -267,9 +267,9 @@
|
|||
NumberOfChildren: 0
|
||||
Components:
|
||||
Transform Component:
|
||||
Translate: {x: 1, y: 2, z: 3}
|
||||
Rotate: {x: 0, y: 0.785398185, z: 0.785397708}
|
||||
Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456}
|
||||
Translate: {x: 1.81218028, y: 2, z: 3}
|
||||
Rotate: {x: 0.785398006, y: 0.785398483, z: 0.785398304}
|
||||
Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
|
||||
IsActive: true
|
||||
RigidBody Component:
|
||||
Type: Dynamic
|
||||
|
|
|
|||
|
|
@ -124,7 +124,7 @@ namespace SHADE
|
|||
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<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
|
||||
|
|
@ -189,8 +189,78 @@ namespace SHADE
|
|||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -41,4 +41,10 @@ namespace SHADE
|
|||
*/
|
||||
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;
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue