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

Fixed and optimised sphere vs convex polyhedron 

Improved sphere vs convex polyhedron from O(n^2) to O(n). Math is amazing.
This commit is contained in:
Diren D Bharwani 2022-12-31 01:11:03 +08:00
parent 82d46fce99
commit 896b47c1a0
5 changed files with 272 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

231
Assets/Scenes/PhysicsSandbox.shade
View File

@ -4,9 +4,9 @@
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: -1.80977702, y: 3, z: 0} Translate: {x: 2.58071685, y: 0.456140399, z: 0}
Rotate: {x: -0, y: 0, z: -0.506194055} Rotate: {x: -0, y: 0, z: 0.469853699}
Scale: {x: 3.27252102, y: 0.999997199, z: 1} Scale: {x: 4.61071014, y: 0.999995053, z: 1}
IsActive: true IsActive: true
RigidBody Component: RigidBody Component:
Type: Static Type: Static
@ -19,7 +19,7 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: false Freeze Position Y: true
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
Freeze Rotation Y: false Freeze Rotation Y: false
@ -60,54 +60,13 @@
Perspective: true Perspective: true
IsActive: true IsActive: true
Scripts: ~ Scripts: ~
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 0, z: 0}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: false
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: false
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Sphere
Radius: 2.5
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: ~
- EID: 3 - EID: 3
Name: Default Name: Default
IsActive: true IsActive: true
NumberOfChildren: 0 NumberOfChildren: 0
Components: Components:
Transform Component: Transform Component:
Translate: {x: -1.97624588, y: 5, z: 0} Translate: {x: -2.24715948, y: 6.47200441, z: 0}
Rotate: {x: -0, y: 0, z: -0} Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 1, y: 1, z: 1} Scale: {x: 1, y: 1, z: 1}
IsActive: true IsActive: true
@ -141,6 +100,186 @@
Position Offset: {x: 0, y: 0, z: 0} Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true IsActive: true
Scripts:
- Type: PhysicsTestObj
Enabled: true
forceAmount: 50
torqueAmount: 500
- EID: 2
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: 4.09544182, z: 0}
Rotate: {x: -0, y: 0, z: -0.437829614}
Scale: {x: 4.61071014, y: 0.999995887, z: 1}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: true
Freeze Position Y: true
Freeze Position Z: true
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 1}
Friction: 0.400000006
Bounciness: 0
Density: 1
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
- EID: 4
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 0, y: -1.74209237, z: 0}
Rotate: {x: -0, y: 0, z: -0}
Scale: {x: 10, y: 0.5, z: 10}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 1}
Friction: 0.400000006
Bounciness: 0
Density: 1
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
- EID: 5
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: -4.80025721, y: 3, z: 0}
Rotate: {x: -0, y: 0, z: 1.57079601}
Scale: {x: 9.99975109, y: 0.499992192, z: 10}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 1}
Friction: 0.400000006
Bounciness: 0
Density: 1
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
- EID: 6
Name: Default
IsActive: true
NumberOfChildren: 0
Components:
Transform Component:
Translate: {x: 4.80000019, y: 3, z: 0}
Rotate: {x: -0, y: 0, z: 1.57079601}
Scale: {x: 9.99975109, y: 0.499992192, z: 10}
IsActive: true
RigidBody Component:
Type: Static
Auto Mass: false
Mass: .inf
Drag: 0.00999999978
Angular Drag: 0.00999999978
Use Gravity: true
Gravity Scale: 1
Interpolate: true
Sleeping Enabled: true
Freeze Position X: false
Freeze Position Y: true
Freeze Position Z: false
Freeze Rotation X: false
Freeze Rotation Y: false
Freeze Rotation Z: false
IsActive: true
Collider Component:
DrawColliders: false
Colliders:
- Is Trigger: false
Collision Tag: 1
Type: Box
Half Extents: {x: 1, y: 1, z: 1}
Friction: 0.400000006
Bounciness: 0
Density: 1
Position Offset: {x: 0, y: 0, z: 0}
Rotation Offset: {x: 0, y: 0, z: 0}
IsActive: true
Scripts: Scripts:
- Type: PhysicsTestObj - Type: PhysicsTestObj
Enabled: true Enabled: true

35
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp
View File

@ -160,6 +160,41 @@ namespace SHADE
throw std::invalid_argument("Index out-of-range!"); throw std::invalid_argument("Index out-of-range!");
return GetVertices()[index]; return GetVertices()[index];
//// Rotate half extents
//const SHVec3 ROTATED_EXTENTS = SHVec3::Rotate(Extents, Orientation);
//const SHVec3 CENTER { Center };
///*
// * Front: -Z
// *
// * 3 _____ 2
// * | |
// * | |
// * |_____|
// * 0 1
// *
// * Back: +Z
// *
// * 7 _____ 6
// * | |
// * | |
// * |_____|
// * 4 5
// *
// */
//switch (index)
//{
// case 0: return CENTER + SHVec3 { -ROTATED_EXTENTS.x, -ROTATED_EXTENTS.y, -ROTATED_EXTENTS.z }
// case 1: return CENTER + SHVec3 {
// case 2: return CENTER + SHVec3 {
// case 3: return CENTER + SHVec3 {
// case 4: return CENTER + SHVec3 {
// case 5: return CENTER + SHVec3 {
// case 6: return CENTER + SHVec3 {
// case 7: return CENTER + SHVec3 {
//}
} }
SHVec3 SHBoxCollisionShape::GetNormal(int faceIndex) const SHVec3 SHBoxCollisionShape::GetNormal(int faceIndex) const

4
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp
View File

@ -126,9 +126,9 @@ namespace SHADE
* *
* Faces: * Faces:
* *
* Front: 0 (0,1,2,3) Normal: Z * Front: 0 (0,1,2,3) Normal: -Z
* Right: 1 (1,5,6,2) Normal: X * Right: 1 (1,5,6,2) Normal: X
* Back: 2 (5,4,7,6) Normal: -Z * Back: 2 (5,4,7,6) Normal: Z
* Left: 3 (4,0,3,7) Normal: -X * Left: 3 (4,0,3,7) Normal: -X
* Top: 4 (3,2,6,7) Normal: Y * Top: 4 (3,2,6,7) Normal: Y
* Bottom: 5 (4,5,1,0) Normal: -Y * Bottom: 5 (4,5,1,0) Normal: -Y

92
SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp
View File

@ -52,8 +52,8 @@ namespace SHADE
const SHSphereCollisionShape& SPHERE = dynamic_cast<const SHSphereCollisionShape&>(A); const SHSphereCollisionShape& SPHERE = dynamic_cast<const SHSphereCollisionShape&>(A);
const SHConvexPolyhedronCollisionShape& CONVEX = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B); const SHConvexPolyhedronCollisionShape& CONVEX = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
// Ensure a gap between A & B const SHVec3 CENTER = SPHERE.GetCenter();
const float TOTAL_RADIUS = SPHERE.GetWorldRadius() + CONVEX.RADIUS; const float TOTAL_RADIUS = SPHERE.GetWorldRadius() + SHConvexPolyhedronCollisionShape::RADIUS;
// Find closest face of polygon to circle // Find closest face of polygon to circle
@ -64,56 +64,49 @@ namespace SHADE
const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure(); const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure();
/* /*
* Test against each face * Test against each face.
* *
* TODO: * 1. For each face, build a plane in point-normal form.
* This check is now O(n^2) because we find the closest point. * 2. Find the signed distance from plane to center of sphere.
* It can be optimised to O(n) by utilising the following steps: * 3. Save best distance and face.
* 1. Rotate sphere into polyhedron's space
* 2. Build a plane equation from the face in point-normal form. We need the plane's offset from the origin.
* 3. Compute distance to the face.
*/ */
for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i) for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i)
{ {
const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i); const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i);
// TODO: Remove and optimise // Build plane equation
// Find the closest point on the face to the sphere
const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size()); const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[0]); // use the first vertex to build a plane
for (int32_t j = 0; j < NUM_VERTICES; ++j) const SHVec3& NORMAL = CONVEX.GetNormal(i);
const float D = -SHVec3::Dot(NORMAL, POINT);
// Find signed distance of center to plane
const float SIGNED_DIST = SHVec3::Dot(NORMAL, CENTER) + D;
// Early out:
// If face is facing away from center, signed dist is negative.
// Therefore signed distance is only positive when sphere is in front of the face.
if (SIGNED_DIST > TOTAL_RADIUS)
return false;
if (SIGNED_DIST > bestDistance)
{ {
// Get vector from center to a vertex on the face bestDistance = SIGNED_DIST;
const SHVec3 A_TO_B = SPHERE.GetCenter() - CONVEX.GetVertex(FACE.vertexIndices[j]); closestFaceIndex = i;
const float PROJECTION = SHVec3::Dot(A_TO_B, FACE.normal);
// Early out
if (PROJECTION > TOTAL_RADIUS)
return false;
if (PROJECTION > bestDistance)
{
bestDistance = PROJECTION;
closestFaceIndex = i;
closestPointIndex = j;
}
} }
} }
uint32_t numContacts = 0; uint32_t numContacts = 0;
// Rotate the normal into the world space
const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex);
const float PENETRATION = TOTAL_RADIUS - bestDistance; const float PENETRATION = TOTAL_RADIUS - bestDistance;
// Check if center is inside polyhedron (below the face) // Check if center is inside polyhedron (below the face)
if (bestDistance < SHMath::EPSILON) if (bestDistance < SHMath::EPSILON)
{ {
manifold.normal = -BEST_NORMAL; manifold.normal = CONVEX.GetNormal(closestFaceIndex);
SHContact newContact; SHContact newContact;
newContact.penetration = PENETRATION; newContact.penetration = PENETRATION;
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * PENETRATION; newContact.position = SPHERE.GetCenter();
newContact.featurePair.key = 0; newContact.featurePair.key = 0;
manifold.contacts[numContacts++] = newContact; manifold.contacts[numContacts++] = newContact;
@ -135,10 +128,25 @@ namespace SHADE
* / / regionC * / / regionC
*/ */
const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex); const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
const int32_t NUM_VERTICES_ON_FACE = static_cast<int32_t>(CLOSEST_FACE.vertexIndices.size()); const SHVec3& NORMAL = CONVEX.GetNormal(closestFaceIndex);
const SHVec3 C = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]); const int32_t NUM_VERTICES_ON_FACE = static_cast<int32_t>(FACE.vertexIndices.size());
// Find closest point on face
closestPointIndex = 0;
float smallestDist = SHVec3::Dot(CENTER - CONVEX.GetVertex(FACE.vertexIndices[0]), NORMAL);
for (int32_t i = 1; i < NUM_VERTICES_ON_FACE; ++i)
{
const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[i]);
const float PROJECTION = SHVec3::Dot(CENTER - POINT, NORMAL);
if (PROJECTION < smallestDist)
closestPointIndex = i;
}
const SHVec3 C = CONVEX.GetVertex(FACE.vertexIndices[closestPointIndex]);
const SHVec3 C_TO_CENTER = SPHERE.GetCenter() - C; const SHVec3 C_TO_CENTER = SPHERE.GetCenter() - C;
const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE; const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE;
@ -146,8 +154,8 @@ namespace SHADE
const SHVec3 POINTS[2] = const SHVec3 POINTS[2] =
{ {
CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_A]) // A CONVEX.GetVertex(FACE.vertexIndices[INDEX_A]) // A
, CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_B]) // B , CONVEX.GetVertex(FACE.vertexIndices[INDEX_B]) // B
}; };
// To be inside either region A or B, 2 conditions must be satisfied // To be inside either region A or B, 2 conditions must be satisfied
@ -170,7 +178,7 @@ namespace SHADE
projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER); projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER);
if (projection >= 0.0f) if (projection >= 0.0f)
{ {
// Sphere Within region A // Sphere Within region
manifold.normal = CP_TO_CENTER; manifold.normal = CP_TO_CENTER;
SHContact newContact; SHContact newContact;
@ -190,7 +198,7 @@ namespace SHADE
{ {
if (C_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS) if (C_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS)
{ {
manifold.normal = SHVec3::Normalise(C_TO_CENTER); manifold.normal = -SHVec3::Normalise(C_TO_CENTER);
SHContact newContact; SHContact newContact;
newContact.penetration = PENETRATION; newContact.penetration = PENETRATION;
@ -207,11 +215,11 @@ namespace SHADE
// Region D // Region D
if (PENETRATION <= TOTAL_RADIUS) if (PENETRATION <= TOTAL_RADIUS)
{ {
manifold.normal = -BEST_NORMAL; manifold.normal = -NORMAL;
SHContact newContact; SHContact newContact;
newContact.penetration = PENETRATION; newContact.penetration = PENETRATION;
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS; newContact.position = SPHERE.GetCenter() - NORMAL * TOTAL_RADIUS;
newContact.featurePair.key = 0; newContact.featurePair.key = 0;
manifold.contacts[numContacts++] = newContact; manifold.contacts[numContacts++] = newContact;

1
SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp
View File

@ -63,7 +63,6 @@ namespace SHADE
const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept
{ {
static ContactPoints contactPoints; static ContactPoints contactPoints;
contactPoints.clear(); contactPoints.clear();
for (auto& manifold : manifolds | std::views::values) for (auto& manifold : manifolds | std::views::values)