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

First half of re-implementing face-face contact derivation

This commit is contained in:
Diren D Bharwani 2023-01-16 02:44:27 +08:00
parent dab109bc77
commit 19bffc9124
7 changed files with 202 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
Assets/Scenes/PhysicsSandbox.shade
View File

@ -45,7 +45,7 @@
NumberOfChildren: 0
Components:
Camera Component:
Position: {x: 0, y: 0, z: 7}
Position: {x: 0, y: 2, z: 7}
Pitch: 0
Yaw: 0
Roll: 0
@ -62,14 +62,14 @@
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335}
Rotate: {x: -0, y: 0, z: -0}
Translate: {x: -1.5, y: 7.5, z: 0}
Rotate: {x: -0, y: 0, z: 0.785398185}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Dynamic
Auto Mass: false
Mass: 10
Mass: 0.52359879
Drag: 0.00999999978
Angular Drag: 0
Use Gravity: true
@ -84,7 +84,7 @@
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: true
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
@ -96,18 +96,14 @@
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts:
- Type: PhysicsTestObj
Enabled: true
forceAmount: 50
torqueAmount: 500
Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 4.09544182, z: 0}
Translate: {x: 0, y: 4, z: 0}
Rotate: {x: -0, y: 0, z: -0.436332315}
Scale: {x: 4.61071014, y: 0.999995887, z: 1}
IsActive: true
@ -272,17 +268,17 @@
Components:
Transform Component:
Translate: {x: 1, y: 2, z: 3}
Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
Rotate: {x: 0, y: 0.785398185, z: 0.785397708}
Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456}
IsActive: true
RigidBody Component:
Type: Dynamic
Auto Mass: false
Mass: 1
Drag: 0.00999999978
Angular Drag: 0.00999999978
Angular Drag: 0
Use Gravity: true
Gravity Scale: 0.5
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
@ -305,7 +301,11 @@
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~
Scripts:
- Type: PhysicsTestObj
Enabled: true
forceAmount: 50
torqueAmount: 5
- EID: 8
Name: Default
IsActive: true

2
SHADE_Engine/src/Math/SHMathHelpers.h
View File

@ -46,7 +46,7 @@ namespace SHADE
/*---------------------------------------------------------------------------------*/
/** Standard Epsilon value for comparing Single-Precision Floating-Point values. */
static constexpr float EPSILON = 0.001f;
static constexpr float EPSILON = 0.0001f;
/** Single-Precision Floating-Point value of infinity */
static constexpr float INF = std::numeric_limits<float>::infinity();

12
SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h
View File

@ -27,20 +27,16 @@ namespace SHADE
union SHContactFeatures
{
public:
/*---------------------------------------------------------------------------------*/
/* Type Definit */
/*---------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------*/
/* Data Members */
/*---------------------------------------------------------------------------------*/
struct
{
uint8_t inI;
uint8_t outI;
uint8_t inR;
uint8_t outR;
uint8_t inI; // Incoming Incident Edge
uint8_t outI; // Outgoing Incident Edge
uint8_t inR; // Incoming Reference Edge
uint8_t outR; // Outgoing Reference Edge
};
uint32_t key = std::numeric_limits<uint32_t>::max();

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

@ -11,6 +11,7 @@
#pragma once
// Project Headers
#include "Math/Geometry/SHPlane.h"
#include "Physics/Collision/Contacts/SHManifold.h"
#include "Physics/Collision/Contacts/SHCollisionKey.h"
#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
@ -86,15 +87,21 @@ namespace SHADE
float bestDistance = std::numeric_limits<float>::lowest();
};
struct ClipVertex
{
SHVec3 position;
SHContactFeatures featurePair;
};
/*---------------------------------------------------------------------------------*/
/* Member Functions */
/*---------------------------------------------------------------------------------*/
// Sphere VS Convex
static FaceQuery findClosestFace (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;
static FaceQuery findClosestFace (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
@ -102,29 +109,22 @@ namespace SHADE
// Convex VS Convex
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) 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;
/*
* TODO:
*
*
*
*
*
* static uint32_t clip [sutherland-hodgemann clipping]
*
* ! References
* https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
* https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
*/
static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) 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 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;
};
} // namespace SHADE

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

@ -54,6 +54,8 @@ namespace SHADE
const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
// TODO: Check against cached separating axis.
const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
if (FACE_QUERY_A.bestDistance > 0.0f)
return false;
@ -78,6 +80,7 @@ namespace SHADE
minFaceQuery = FACE_QUERY_A;
referencePoly = &POLY_A;
incidentPoly = &POLY_B;
flipNormal = false;
}
else
{
@ -114,7 +117,7 @@ namespace SHADE
SHContact contact;
contact.featurePair.key = EDGE_QUERY.axis;
contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
contact.penetration = EDGE_QUERY.bestDistance;
contact.penetration = -EDGE_QUERY.bestDistance;
manifold.contacts[numContacts++] = contact;
manifold.numContacts = numContacts;
@ -125,23 +128,67 @@ namespace SHADE
const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
const SHHalfEdgeStructure::Face& INCIDENT_FACE = incidentPoly->GetFace(INCIDENT_FACE_IDX);
const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace);
/*
* TODO:
*
* !!
* 4. From above, save the axis of minimum penetration (reference face)
* 5. Find the most anti-parallel face on other shape (incident face)
* 6. Clip incident face against side planes of reference face. (Sutherland-Hodgeman Clipping).
* Keep all vertices below reference face.
* 7. Reduce manifold to 4 contact points. We only need 4 contact points for a stable manifold.
*
* Remember to save IDs in queries.
* During generation of incident face, store IDs of face.
*
*/
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.
return false;
}
@ -353,5 +400,77 @@ namespace SHADE
return bestFace;
}
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;
int32_t v1Index = numIn - 1;
int32_t v2Index = 0;
float v1Distance = plane.SignedDistance(in[v1Index].position);
float v2Distance = 0.0f;
for (int32_t i = 0; i < numIn; ++i)
{
v2Index = i;
const SHVec3 v1Pos = in[v1Index].position;
const SHVec3 v2Pos = in[v2Index].position;
v2Distance = plane.SignedDistance(v2Pos);
// v1 in front, v2 behind
// keep the intersection point
if (v1Distance >= 0.0f && v2Distance < 0.0f)
{
ClipVertex intersection;
// In case the edge is parallel, the intersection is just the start point
const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f);
const float ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos);
intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
intersection.featurePair.inI = in[v1Index].featurePair.outI;
intersection.featurePair.outI = 0;
intersection.featurePair.inR = 0;
intersection.featurePair.outR = planeIdx;
out.emplace_back(intersection);
}
// v1 behind, v2 in front
// keep intersection point & v2
if(v1Distance < 0.0f && v2Distance >= 0.0f)
{
ClipVertex intersection;
// In case the edge is parallel, the intersection is just the start point
const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f);
const float ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos);
intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
intersection.featurePair.inI = 0;
intersection.featurePair.outI = in[v2Index].featurePair.inI;
intersection.featurePair.inR = planeIdx;
intersection.featurePair.outR = 0;
out.emplace_back(intersection);
out.emplace_back(in[v2Index]);
}
// both in front, keep v2
if (v1Distance >= 0.0f && v2Distance >= 0.0f)
{
out.emplace_back(in[v2Index]);
}
// v2 is the next v1
v1Index = v2Index;
v1Distance = v2Distance;
}
return out;
}
} // namespace SHADE

52
SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
View File

@ -187,7 +187,7 @@ namespace SHADE
* restituion bias = restitution * (relative velocity /dot normal)
*/
const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
const float ERROR_BIAS = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP);
// Warm starting
// Compute impulses
@ -244,6 +244,31 @@ namespace SHADE
SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB);
SHVec3 relativeVelocity = velocityB - velocityA;
// Get scalar of relative velocity along the normal
const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
// Compute true normal impulse
const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
// Apply impulses
vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
// Solve normal impulse
// Re-compute relative velocity
velocityA = vA + SHVec3::Cross(wA, contact.rA);
velocityB = vB + SHVec3::Cross(wB, contact.rB);
relativeVelocity = velocityB - velocityA;
// Solve tangent impulse
for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
{
@ -269,31 +294,6 @@ namespace SHADE
vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
}
// Solve normal impulse
// Re-compute relative velocity
velocityA = vA + SHVec3::Cross(wA, contact.rA);
velocityB = vB + SHVec3::Cross(wB, contact.rB);
relativeVelocity = velocityB - velocityA;
// Get scalar of relative velocity along the normal
const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
// Compute true normal impulse
const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
// Apply impulses
vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
}
velocityStateA.LinearVelocity = vA;

2
SHADE_Engine/src/Physics/SHPhysicsConstants.h
View File

@ -26,7 +26,7 @@ namespace SHADE
* @brief
* Linear Collision & Constraint tolerance.
*/
static constexpr float SHPHYSICS_LINEAR_SLOP = 0.05f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
static constexpr float SHPHYSICS_LINEAR_SLOP = 0.01f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
/**
* @brief