Added mostly working sphere vs convex polyhedron collision detection
This commit is contained in:
Diren D Bharwani
parent
d98d6a9e06
commit
3586c7ffdc
|
|
@ -4,7 +4,7 @@
|
|||
NumberOfChildren: 0
|
||||
Components:
|
||||
Transform Component:
|
||||
Translate: {x: 0.0700113177, y: 2.5, z: 0}
|
||||
Translate: {x: 0, y: 2.5, z: 0}
|
||||
Rotate: {x: -0, y: 0, z: -0}
|
||||
Scale: {x: 1, y: 1, z: 1}
|
||||
IsActive: true
|
||||
|
|
@ -30,8 +30,8 @@
|
|||
Colliders:
|
||||
- Is Trigger: false
|
||||
Collision Tag: 1
|
||||
Type: Sphere
|
||||
Radius: 1
|
||||
Type: Box
|
||||
Half Extents: {x: 1, y: 1, z: 1}
|
||||
Friction: 0.400000006
|
||||
Bounciness: 0
|
||||
Density: 1
|
||||
|
|
@ -49,9 +49,9 @@
|
|||
NumberOfChildren: 0
|
||||
Components:
|
||||
Camera Component:
|
||||
Position: {x: 0, y: 0.5, z: 5}
|
||||
Position: {x: 3, y: 4, z: 0}
|
||||
Pitch: 0
|
||||
Yaw: 0
|
||||
Yaw: 90
|
||||
Roll: 0
|
||||
Width: 1920
|
||||
Height: 1080
|
||||
|
|
@ -107,8 +107,8 @@
|
|||
NumberOfChildren: 0
|
||||
Components:
|
||||
Transform Component:
|
||||
Translate: {x: 0, y: 5, z: 0}
|
||||
Rotate: {x: -0, y: 0, z: 0}
|
||||
Translate: {x: 0, y: 5, z: 0.834425449}
|
||||
Rotate: {x: -0, y: 0, z: -0}
|
||||
Scale: {x: 1, y: 1, z: 1}
|
||||
IsActive: true
|
||||
RigidBody Component:
|
||||
|
|
@ -122,7 +122,7 @@
|
|||
Interpolate: true
|
||||
Sleeping Enabled: true
|
||||
Freeze Position X: false
|
||||
Freeze Position Y: false
|
||||
Freeze Position Y: true
|
||||
Freeze Position Z: false
|
||||
Freeze Rotation X: false
|
||||
Freeze Rotation Y: false
|
||||
|
|
@ -133,8 +133,8 @@
|
|||
Colliders:
|
||||
- Is Trigger: false
|
||||
Collision Tag: 1
|
||||
Type: Box
|
||||
Half Extents: {x: 1, y: 1, z: 1}
|
||||
Type: Sphere
|
||||
Radius: 1
|
||||
Friction: 0.400000006
|
||||
Bounciness: 0
|
||||
Density: 1
|
||||
|
|
|
|||
|
|
@ -47,6 +47,176 @@ namespace SHADE
|
|||
|
||||
bool SHCollision::SphereVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||
{
|
||||
// Convert to underlying types
|
||||
// For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual.
|
||||
const SHSphereCollisionShape& SPHERE = dynamic_cast<const SHSphereCollisionShape&>(A);
|
||||
const SHConvexPolyhedronCollisionShape& CONVEX = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
|
||||
|
||||
// Ensure a gap between A & B
|
||||
const float TOTAL_RADIUS = SPHERE.GetWorldRadius() + CONVEX.RADIUS;
|
||||
|
||||
// Find closest face of polygon to circle
|
||||
|
||||
int32_t closestFaceIndex = -1;
|
||||
int32_t closestPointIndex = -1;
|
||||
float bestDistance = std::numeric_limits<float>::lowest();
|
||||
|
||||
const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure();
|
||||
|
||||
/*
|
||||
* Test against each face
|
||||
*
|
||||
* TODO:
|
||||
* This check is now O(n^2) because we find the closest point.
|
||||
* It can be optimised to O(n) by utilising the following steps:
|
||||
* 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)
|
||||
{
|
||||
const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i);
|
||||
|
||||
// TODO: Remove and optimise
|
||||
// Find the closest point on the face to the sphere
|
||||
const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
|
||||
for (int32_t j = 0; j < NUM_VERTICES; ++j)
|
||||
{
|
||||
// Get vector from center to a vertex on the face
|
||||
const SHVec3 A_TO_B = SPHERE.GetCenter() - CONVEX.GetVertex(FACE.vertexIndices[j]);
|
||||
|
||||
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;
|
||||
const float penetration = TOTAL_RADIUS - bestDistance;
|
||||
|
||||
// Rotate the normal into the world space
|
||||
const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex);
|
||||
|
||||
// Check if center is inside polyhedron (below the face)
|
||||
if (bestDistance < SHMath::EPSILON)
|
||||
{
|
||||
SHContact newContact;
|
||||
newContact.penetration = penetration;
|
||||
newContact.position = SPHERE.GetCenter();
|
||||
newContact.featurePair.key = 0;
|
||||
|
||||
manifold.contacts[numContacts++] = newContact;
|
||||
manifold.normal = BEST_NORMAL;
|
||||
|
||||
manifold.numContacts = numContacts;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check against voronoi regions of the face to determine the type of the intersection test
|
||||
// We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
|
||||
// If none of these are true, the sphere is above the face but not separating
|
||||
|
||||
const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
|
||||
const int32_t NUM_VERTICES_ON_FACE = static_cast<int32_t>(CLOSEST_FACE.vertexIndices.size());
|
||||
|
||||
const SHVec3& CLOSEST_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]);
|
||||
const SHVec3 CP_TO_CENTER = SPHERE.GetCenter() - CLOSEST_POINT;
|
||||
|
||||
// Check closest point -> prev point
|
||||
{
|
||||
const int32_t PREV_POINT_INDEX = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1;
|
||||
const SHVec3& PREV_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[PREV_POINT_INDEX]);
|
||||
|
||||
const SHVec3 CP_TO_PREV = SHVec3::Normalise(PREV_POINT - CLOSEST_POINT);
|
||||
|
||||
float projection = SHVec3::Dot(CP_TO_CENTER, CP_TO_PREV);
|
||||
if (projection >= 0.0f)
|
||||
{
|
||||
// Sphere is inside this region, check if distance from center is lesser than radius
|
||||
if (penetration >= TOTAL_RADIUS)
|
||||
return false;
|
||||
|
||||
SHContact newContact;
|
||||
newContact.penetration = penetration;
|
||||
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
|
||||
newContact.featurePair.key = 0;
|
||||
|
||||
manifold.contacts[numContacts++] = newContact;
|
||||
manifold.normal = BEST_NORMAL;
|
||||
|
||||
manifold.numContacts = numContacts;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check closest point -> next point
|
||||
{
|
||||
const int32_t NEXT_POINT_INDEX = closestPointIndex + 1 % NUM_VERTICES_ON_FACE;
|
||||
const SHVec3& NEXT_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[NEXT_POINT_INDEX]);
|
||||
|
||||
const SHVec3 CP_TO_NEXT = SHVec3::Normalise(NEXT_POINT - CLOSEST_POINT);
|
||||
|
||||
float projection = SHVec3::Dot(CP_TO_CENTER, CP_TO_NEXT);
|
||||
if (projection >= 0.0f)
|
||||
{
|
||||
// Sphere is inside this region, check if distance from center is lesser than radius
|
||||
if (penetration >= TOTAL_RADIUS)
|
||||
return false;
|
||||
|
||||
SHContact newContact;
|
||||
newContact.penetration = penetration;
|
||||
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
|
||||
newContact.featurePair.key = 0;
|
||||
|
||||
manifold.contacts[numContacts++] = newContact;
|
||||
manifold.normal = BEST_NORMAL;
|
||||
|
||||
manifold.numContacts = numContacts;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if it hit the closest point
|
||||
{
|
||||
if (CP_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS)
|
||||
{
|
||||
SHContact newContact;
|
||||
newContact.penetration = penetration;
|
||||
newContact.position = CLOSEST_POINT;
|
||||
newContact.featurePair.key = 0;
|
||||
|
||||
manifold.contacts[numContacts++] = newContact;
|
||||
manifold.normal = SHVec3::Normalise(CP_TO_CENTER);
|
||||
|
||||
manifold.numContacts = numContacts;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// It is above the closest face
|
||||
if (penetration <= TOTAL_RADIUS)
|
||||
{
|
||||
SHContact newContact;
|
||||
newContact.penetration = penetration;
|
||||
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
|
||||
newContact.featurePair.key = 0;
|
||||
|
||||
manifold.contacts[numContacts++] = newContact;
|
||||
manifold.normal = BEST_NORMAL;
|
||||
|
||||
manifold.numContacts = numContacts;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,17 +25,17 @@ namespace SHADE
|
|||
|
||||
bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||
{
|
||||
const SHSphereCollisionShape& SPHERE_A = reinterpret_cast<const SHSphereCollisionShape&>(A);
|
||||
const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
|
||||
const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
|
||||
const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
|
||||
|
||||
return SHSphere::Intersect(SPHERE_A, SPHERE_B);
|
||||
}
|
||||
|
||||
bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||
{
|
||||
// Convert to spheres
|
||||
const SHSphereCollisionShape& SPHERE_A = reinterpret_cast<const SHSphereCollisionShape&>(A);
|
||||
const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
|
||||
// Convert to underlying types
|
||||
const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
|
||||
const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
|
||||
|
||||
const SHVec3 A_TO_B = SPHERE_B.GetCenter() - SPHERE_A.GetCenter();
|
||||
const float DISTANCE_BETWEEN_CENTERS_SQUARED = A_TO_B.LengthSquared();
|
||||
|
|
|
|||
Loading…
Reference in New Issue