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

Fixed contact point derivation and manifold reduction. 

System tends to be a bit unstable now, will look into it. Islands and sleeping needs to be implemented to reduce unnecessary drift
This commit is contained in:
Diren D Bharwani 2023-01-07 00:53:05 +08:00
parent 57498bb8b8
commit 3e91f99d78
7 changed files with 177 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Assets/Scenes/PhysicsSandbox.shade
View File

@ -45,9 +45,9 @@
NumberOfChildren: 0
Components:
Camera Component:
Position: {x: -0.5, y: 10, z: -3}
Position: {x: -0.5, y: 10, z: 3}
Pitch: 0
Yaw: 180
Yaw: 0
Roll: 0
Width: 1920
Height: 1080
@ -280,8 +280,8 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0.524352431, y: 13.5, z: 0.0463808179}
Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
Translate: {x: -0.5, y: 13.5, z: 0}
Rotate: {x: -0, y: 0, z: 0}
Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
IsActive: true
RigidBody Component:
@ -321,7 +321,7 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: -0.5, y: 10, z: 0}
Translate: {x: 0, y: 10, z: 0}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true

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

@ -116,7 +116,8 @@ namespace SHADE
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;
static std::vector<SHContact> clipPolygonAgainstPlane (const std::vector<SHContact>& in, const SHPlane& plane) noexcept;
static std::vector<SHContact> reduceContacts (const std::vector<SHContact>& in, const SHVec3& normal) noexcept;
// TODO: Reduce Manifold
};

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

@ -135,14 +135,17 @@ namespace SHADE
std::vector<SHContact> incidentPolygon;
for (const int32_t i : std::views::iota(0, MAX_NUM_CONTACTS))
{
SHContact contact;
const int32_t VERTEX_INDEX = INCIDENT_FACE.vertexIndices[i].index;
incidentPolygon[i].position = incidentPoly->GetVertex(VERTEX_INDEX);
contact.position = incidentPoly->GetVertex(VERTEX_INDEX);
contact.featurePair.indexA = minFaceQuery.closestFace;
contact.featurePair.indexB = VERTEX_INDEX;
contact.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::FACE);
contact.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
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);
incidentPolygon.emplace_back(contact);
}
// Number of vertices == number of edges on face
@ -164,7 +167,7 @@ namespace SHADE
const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
const SHPlane PLANE{ V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) };
auto clipped = clipsPolygonAgainstPlane(incidentPolygon, PLANE);
auto clipped = clipPolygonAgainstPlane(incidentPolygon, PLANE);
memcpy_s(incidentPolygon.data(), COPY_SIZE, clipped.data(), COPY_SIZE);
}
@ -198,13 +201,18 @@ namespace SHADE
if (numContacts > 4)
{
// TODO: Reduce the manifold
auto reducedSet = reduceContacts(finalContacts, REFERENCE_NORMAL);
numContacts = 4;
memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, reducedSet.data(), sizeof(SHContact) * numContacts);
}
else
{
memcpy_s(manifold.contacts, sizeof(SHContact) * numContacts, finalContacts.data(), sizeof(SHContact) * numContacts);
}
manifold.normal = flipNormal ? -REFERENCE_NORMAL : REFERENCE_NORMAL;
manifold.numContacts = numContacts;
@ -415,8 +423,10 @@ namespace SHADE
return bestFace;
}
std::vector<SHContact> SHCollision::clipsPolygonAgainstPlane(const std::vector<SHContact>& in, const SHPlane& plane) noexcept
std::vector<SHContact> SHCollision::clipPolygonAgainstPlane(const std::vector<SHContact>& in, const SHPlane& plane) noexcept
{
static constexpr float THRESHOLD = -SHMath::EPSILON;
std::vector<SHContact> out;
//
@ -429,26 +439,30 @@ namespace SHADE
* 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
* 2. Both in-front: Keep tail
* 3. Head behind, tail in-front: Keep interpolated position
* 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 SHContact& TAIL = in[i];
const SHContact& HEAD = in[(i + 1) % NUM_POINTS];
const float HEAD_DISTANCE = plane.SignedDistance(HEAD.position);
const float TAIL_DISTANCE = plane.SignedDistance(TAIL.position);
const float HEAD_DISTANCE = plane.SignedDistance(HEAD.position);
// Scenario 1: Both Behind
if (HEAD_DISTANCE < 0.0f && TAIL_DISTANCE < 0.0f)
if (HEAD_DISTANCE < THRESHOLD && TAIL_DISTANCE < THRESHOLD)
continue;
// Scenario 2: Tail In-Front, Head Behind
if (HEAD_DISTANCE >= 0.0f && TAIL_DISTANCE < 0.0f)
// Scenario 2: Both In-Front
if (HEAD_DISTANCE >= THRESHOLD && TAIL_DISTANCE >= THRESHOLD)
out.emplace_back(TAIL);
// Scenario 3: Head In-Front, Tail Behind
if (HEAD_DISTANCE >= THRESHOLD && TAIL_DISTANCE < THRESHOLD)
{
SHContact interpolated;
@ -463,16 +477,10 @@ namespace SHADE
out.emplace_back(interpolated);
}
// Scenario 3: Both In-Front
if (HEAD_DISTANCE >= 0.0f && TAIL_DISTANCE >= 0.0f)
// Scenario 4: Head Behind, Tail In-Front
if (HEAD_DISTANCE < THRESHOLD && TAIL_DISTANCE >= THRESHOLD)
{
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;
@ -491,4 +499,111 @@ namespace SHADE
return out;
}
std::vector<SHContact> SHCollision::reduceContacts(const std::vector<SHContact>& in, const SHVec3& normal) noexcept
{
std::vector<SHContact> out;
std::unordered_map<int, SHVec3> contactMap;
const int NUM_IN = static_cast<int>(in.size());
for (const int i : std::views::iota(0, NUM_IN))
contactMap.emplace(i, in[i].position);
// We'll be reusing these in all 4 phases
float bestMetric = std::numeric_limits<float>::lowest();
int bestVertex = -1;
// Find the furthest point alone the -Z-axis
for (const auto& [index, pos] : contactMap)
{
const float PROJECTION = SHVec3::Dot(pos, -SHVec3::UnitZ);
if (PROJECTION > bestMetric)
{
bestMetric = PROJECTION;
bestVertex = index;
}
}
const SHVec3& A = out.emplace_back(in[bestVertex]).position;
contactMap.erase(bestVertex);
// Reset
bestMetric = std::numeric_limits<float>::lowest();
bestVertex = -1;
// Find farthest point
for (const auto& [index, pos] : contactMap)
{
const float DISTANCE = SHVec3::DistanceSquared(A, pos);
if (DISTANCE > bestMetric)
{
bestMetric = DISTANCE;
bestVertex = index;
}
}
const SHVec3& B = out.emplace_back(in[bestVertex]).position;
contactMap.erase(bestVertex);
// Reset
bestMetric = std::numeric_limits<float>::lowest();
bestVertex = -1;
// Find next point with largest area
// Largest area can be found as 0.5 * crossProduct /dot normal
for (const auto& [index, pos] : contactMap)
{
const SHVec3 CA = A - pos;
const SHVec3 CB = B - pos;
const float AREA = SHVec3::Dot(0.5f * SHVec3::Cross(CA, CB), normal);
// We only want triangles with negative areas!
if (AREA > bestMetric)
{
bestMetric = AREA;
bestVertex = index;
}
}
const SHVec3& C = out.emplace_back(in[bestVertex]).position;
contactMap.erase(bestVertex);
const float CURRENT_AREA = bestMetric;
// Reset
bestMetric = std::numeric_limits<float>::lowest();
bestVertex = -1;
// Find the contact point which forms the largest polygon
// We need to construct 3 triangles and compare them
for (const auto& [index, pos] : contactMap)
{
const SHVec3 DA = A - pos;
const SHVec3 DB = B - pos;
const SHVec3 DC = C - pos;
float areas[3] =
{
SHVec3::Dot(0.5f * SHVec3::Cross(DA, DB), normal) // DAB
, SHVec3::Dot(0.5f * SHVec3::Cross(DB, DC), normal) // DBC
, SHVec3::Dot(0.5f * SHVec3::Cross(DC, DA), normal) // DCA
};
for (int i = 0; i < 3; ++i)
{
if (areas[i] < 0.0f && areas[i] + CURRENT_AREA > bestMetric)
{
bestMetric = areas[i] + CURRENT_AREA;
bestVertex = index;
}
}
}
out.emplace_back(in[bestVertex]);
return out;
}
} // namespace SHADE

31
SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp
View File

@ -113,6 +113,11 @@ namespace SHADE
return relativeExtents;
}
int32_t SHBox::GetNumVertices() const noexcept
{
return NUM_VERTICES;
}
SHVec3 SHBox::GetVertex(int index) const
{
if (index < 0 || index >= NUM_VERTICES)
@ -290,30 +295,4 @@ namespace SHADE
return SHAABB{ CENTROID, HALF_EXTENTS };
}
SHVec3 SHBox::FindSupportPoint(const SHVec3& direction) const noexcept
{
float bestDistance = std::numeric_limits<float>::lowest();
SHVec3 vertices[NUM_VERTICES];
GetCorners(vertices);
// No reason to put the center really..
SHVec3 bestPoint = Center;
for (auto& vertex : vertices)
{
const float PROJECTION = SHVec3::Dot(vertex, direction);
if (PROJECTION > bestDistance)
{
bestDistance = PROJECTION;
bestPoint = vertex;
}
}
return bestPoint;
}
} // namespace SHADE

2
SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h
View File

@ -85,6 +85,7 @@ namespace SHADE
// Overriden Methods
[[nodiscard]] int32_t GetNumVertices () const noexcept override;
[[nodiscard]] SHVec3 GetVertex (int index) const override;
[[nodiscard]] SHVec3 GetNormal (int faceIndex) const override;
@ -114,7 +115,6 @@ namespace SHADE
[[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override;
[[nodiscard]] SHMatrix GetTRS () const noexcept override;
[[nodiscard]] SHAABB ComputeAABB () const noexcept override;
[[nodiscard]] SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept override;
private:
/*---------------------------------------------------------------------------------*/

21
SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.cpp
View File

@ -92,5 +92,26 @@ namespace SHADE
return halfEdgeStructure->GetHalfEdge(index);
}
SHVec3 SHConvexPolyhedron::FindSupportPoint(const SHVec3& direction) const noexcept
{
float bestDistance = std::numeric_limits<float>::lowest();
SHVec3 bestPoint = SHVec3::Zero;
for (const int32_t i : std::views::iota(0, GetNumVertices()))
{
const SHVec3 VERTEX = GetVertex(i);
const float PROJECTION = SHVec3::Dot(VERTEX, direction);
if (PROJECTION > bestDistance)
{
bestDistance = PROJECTION;
bestPoint = VERTEX;
}
}
return bestPoint;
}
} // namespace SHADE

7
SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h
View File

@ -65,8 +65,9 @@ namespace SHADE
// Virtual Methods
[[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0;
[[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0;
[[nodiscard]] virtual int32_t GetNumVertices () const noexcept = 0;
[[nodiscard]] virtual SHVec3 GetVertex (int index) const = 0;
[[nodiscard]] virtual SHVec3 GetNormal (int faceIndex) const = 0;
/*---------------------------------------------------------------------------------*/
/* Member Functions */
@ -80,7 +81,7 @@ namespace SHADE
* @return
* The most extreme vertex in the given direction.
*/
[[nodiscard]] virtual SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept = 0;
[[nodiscard]] SHVec3 FindSupportPoint (const SHVec3& direction) const noexcept;
protected:
/*---------------------------------------------------------------------------------*/