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

Abstracted contact derivation as setup for cached SAT

This commit is contained in:
Diren D Bharwani 2023-01-22 19:20:03 +08:00
parent 826a1bc617
commit 0c3106f15b
4 changed files with 243 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Assets/Scenes/PhysicsSandbox.shade
View File

@ -79,9 +79,9 @@
Freeze Position X: false Freeze Position X: false
Freeze Position Y: false Freeze Position Y: false
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: true Freeze Rotation X: false
Freeze Rotation Y: true Freeze Rotation Y: false
Freeze Rotation Z: true Freeze Rotation Z: false
IsActive: true IsActive: true
Collider Component: Collider Component:
DrawColliders: false DrawColliders: false
@ -90,7 +90,7 @@
Collision Tag: 1 Collision Tag: 1
Type: Sphere Type: Sphere
Radius: 1 Radius: 1
Friction: 1 Friction: 0.400000006
Bounciness: 0 Bounciness: 0
Density: 1 Density: 1
Position Offset: {x: 0, y: 0, z: 0} Position Offset: {x: 0, y: 0, z: 0}
@ -131,7 +131,7 @@
Collision Tag: 2 Collision Tag: 2
Type: Box Type: Box
Half Extents: {x: 1, y: 1, z: 1} Half Extents: {x: 1, y: 1, z: 1}
Friction: 1 Friction: 0.400000006
Bounciness: 0 Bounciness: 0
Density: 1 Density: 1
Position Offset: {x: 0, y: 0, z: 0} Position Offset: {x: 0, y: 0, z: 0}
@ -268,7 +268,7 @@
Components: Components:
Transform Component: Transform Component:
Translate: {x: 2, y: 2, z: 3} Translate: {x: 2, y: 2, z: 3}
Rotate: {x: 0, y: 0, z: 0} Rotate: {x: 0.785398185, y: 0.785398185, z: 0.785398185}
Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337} Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
@ -278,7 +278,7 @@
Drag: 0.00999999978 Drag: 0.00999999978
Angular Drag: 0 Angular Drag: 0
Use Gravity: true Use Gravity: true
Gravity Scale: 5 Gravity Scale: 1
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false

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

@ -99,9 +99,27 @@ namespace SHADE
// Sphere VS Convex // Sphere VS Convex
static FaceQuery findClosestFace (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron) noexcept; static FaceQuery findClosestFace
static int32_t findClosestPoint (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron, int32_t faceIndex) noexcept; (
static int32_t findVoronoiRegion (const SHSphere& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept; const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
) noexcept;
static int32_t findClosestPoint
(
const SHSphere& sphere
, const SHConvexPolyhedron& polyhedron
, int32_t faceIndex
) noexcept;
static int32_t findVoronoiRegion
(
const SHSphere& sphere
, const SHVec3& faceVertex
, const SHVec3& faceNormal
, const SHVec3& tangent1
, const SHVec3& tangent2
) noexcept;
// Capsule VS Convex // Capsule VS Convex
@ -115,16 +133,79 @@ namespace SHADE
* https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp * https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
*/ */
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; static FaceQuery queryFaceDirections
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; (
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
) noexcept;
static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; static EdgeQuery queryEdgeDirections
static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; (
static float distanceBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; const SHConvexPolyhedron& A
static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; , const SHConvexPolyhedron& B
static int32_t findIncidentFace (const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept; ) noexcept;
static std::vector<ClipVertex> clipPolygonWithPlane (const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept;
static std::vector<int32_t> reduceContacts (const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept; static bool buildMinkowskiFace
(
const SHConvexPolyhedron& A
, const SHConvexPolyhedron& B
, int32_t edgeA
, int32_t edgeB
) noexcept;
static bool isMinkowskiFace
(
const SHVec3& a
, const SHVec3& b
, const SHVec3& c
, const SHVec3& d
) noexcept;
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 bool findFaceContacts
(
SHManifold& manifold
, const SHConvexPolyhedron& incPoly
, int32_t incFace
, const SHConvexPolyhedron& refPoly
, int32_t refFace
, bool flip
) noexcept;
static std::vector<ClipVertex> clipPolygonWithPlane
(
const std::vector<ClipVertex>& in
, int32_t numIn
, const SHPlane& plane
, int32_t planeIdx
) noexcept;
static std::vector<int32_t> reduceContacts
(
const std::vector<SHContact>& in
, const SHVec3& faceNormal
) noexcept;
}; };
} // namespace SHADE } // namespace SHADE

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

@ -128,139 +128,7 @@ namespace SHADE
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
const SHHalfEdgeStructure::Face& INCIDENT_FACE = incidentPoly->GetFace(INCIDENT_FACE_IDX); return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace);
const int32_t NUM_INCIDENT_VERTICES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
const int32_t NUM_REFERENCE_VERTICES = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
// Build incoming vertices to clip
std::vector<ClipVertex> clipIn;
clipIn.resize(NUM_INCIDENT_VERTICES * 2, ClipVertex{});
int32_t numClipIn = 0;
for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
{
const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
// The incoming id is the previous edge
// The outgoing id is the current edge (where this vertex is the tail of)
ClipVertex v;
v.position = incidentPoly->GetVertex(V_INDEX);
v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex;
v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex;
v.featurePair.inR = 0;
v.featurePair.outR = 0;
clipIn[numClipIn++] = v;
}
// Clip the vertices against the reference face side planes.
// Number of side planes == number of edges == number of vertices
for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i)
{
// Side plane can be built with the vertex on the edge and the plane's normal
// Plane normal = faceNormal X tangent (v2 - v1)
const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
const SHVec3 V1 = referencePoly->GetVertex(V1_INDEX);
const SHVec3 V2 = referencePoly->GetVertex(V2_INDEX);
const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) };
std::vector<ClipVertex> clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
if (clipOut.empty())
return false;
// Replace the clip container's contents with the clipped points for the next clipping pass
const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size());
for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex)
{
clipIn[clippedIndex].position = clipOut[clippedIndex].position;
clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key;
}
numClipIn = NUM_CLIPPED;
}
// 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 };
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;
} }
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
@ -442,7 +310,7 @@ namespace SHADE
const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA; const float A2_OVER_A1 = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA;
const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB; const float NUMERATOR = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1; const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR; const float R = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f);
// Just take a point from A since it's A -> B // Just take a point from A since it's A -> B
return TAIL_A + R * VA; return TAIL_A + R * VA;
@ -470,6 +338,147 @@ namespace SHADE
return bestFace; return bestFace;
} }
bool SHCollision::findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept
{
const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace);
const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace);
const int32_t NUM_INCIDENT_VERTICES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
const int32_t NUM_REFERENCE_VERTICES = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
// Build incoming vertices to clip
std::vector<ClipVertex> clipIn;
clipIn.resize(NUM_INCIDENT_VERTICES * 2, ClipVertex{});
int32_t numClipIn = 0;
for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
{
const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
// The incoming id is the previous edge
// The outgoing id is the current edge (where this vertex is the tail of)
ClipVertex v;
v.position = incPoly.GetVertex(V_INDEX);
v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex;
v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex;
v.featurePair.inR = 0;
v.featurePair.outR = 0;
clipIn[numClipIn++] = v;
}
// Clip the vertices against the reference face side planes.
// Number of side planes == number of edges == number of vertices
const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace);
for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i)
{
// Side plane can be built with the vertex on the edge and the plane's normal
// Plane normal = faceNormal X tangent (v2 - v1)
const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
const SHVec3 V1 = refPoly.GetVertex(V1_INDEX);
const SHVec3 V2 = refPoly.GetVertex(V2_INDEX);
const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1);
const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) };
std::vector<ClipVertex> clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
if (clipOut.empty())
return false;
// Replace the clip container's contents with the clipped points for the next clipping pass
const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size());
for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex)
{
clipIn[clippedIndex].position = clipOut[clippedIndex].position;
clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key;
}
numClipIn = NUM_CLIPPED;
}
// From the final set of clipped points, only keep the points that are below the reference plane.
const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL };
uint32_t numContacts = 0;
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 (flip)
{
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, REF_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 = REF_NORMAL;
if (flip)
manifold.normal = -manifold.normal;
return true;
}
std::vector<SHCollision::ClipVertex> SHCollision::clipPolygonWithPlane(const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept std::vector<SHCollision::ClipVertex> SHCollision::clipPolygonWithPlane(const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept
{ {
std::vector<ClipVertex> out; std::vector<ClipVertex> out;

4
SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.h
View File

@ -139,10 +139,6 @@ namespace SHADE
// TODO: Move to island when islands are set up // TODO: Move to island when islands are set up
void integrateForces (SHRigidBody& rigidBody, float dt) const noexcept; void integrateForces (SHRigidBody& rigidBody, float dt) const noexcept;
void integrateVelocities (SHRigidBody& rigidBody, float dt) const noexcept; void integrateVelocities (SHRigidBody& rigidBody, float dt) const noexcept;
}; };