Added mostly working sphere vs convex polyhedron collision detection
This commit is contained in:
parent
d98d6a9e06
commit
3586c7ffdc
|
@ -4,7 +4,7 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Transform Component:
|
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}
|
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
|
||||||
|
@ -30,8 +30,8 @@
|
||||||
Colliders:
|
Colliders:
|
||||||
- Is Trigger: false
|
- Is Trigger: false
|
||||||
Collision Tag: 1
|
Collision Tag: 1
|
||||||
Type: Sphere
|
Type: Box
|
||||||
Radius: 1
|
Half Extents: {x: 1, y: 1, z: 1}
|
||||||
Friction: 0.400000006
|
Friction: 0.400000006
|
||||||
Bounciness: 0
|
Bounciness: 0
|
||||||
Density: 1
|
Density: 1
|
||||||
|
@ -49,9 +49,9 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Camera Component:
|
Camera Component:
|
||||||
Position: {x: 0, y: 0.5, z: 5}
|
Position: {x: 3, y: 4, z: 0}
|
||||||
Pitch: 0
|
Pitch: 0
|
||||||
Yaw: 0
|
Yaw: 90
|
||||||
Roll: 0
|
Roll: 0
|
||||||
Width: 1920
|
Width: 1920
|
||||||
Height: 1080
|
Height: 1080
|
||||||
|
@ -107,8 +107,8 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Transform Component:
|
Transform Component:
|
||||||
Translate: {x: 0, y: 5, z: 0}
|
Translate: {x: 0, y: 5, z: 0.834425449}
|
||||||
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
|
||||||
RigidBody Component:
|
RigidBody Component:
|
||||||
|
@ -122,7 +122,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
|
||||||
|
@ -133,8 +133,8 @@
|
||||||
Colliders:
|
Colliders:
|
||||||
- Is Trigger: false
|
- Is Trigger: false
|
||||||
Collision Tag: 1
|
Collision Tag: 1
|
||||||
Type: Box
|
Type: Sphere
|
||||||
Half Extents: {x: 1, y: 1, z: 1}
|
Radius: 1
|
||||||
Friction: 0.400000006
|
Friction: 0.400000006
|
||||||
Bounciness: 0
|
Bounciness: 0
|
||||||
Density: 1
|
Density: 1
|
||||||
|
|
|
@ -47,6 +47,176 @@ namespace SHADE
|
||||||
|
|
||||||
bool SHCollision::SphereVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -25,17 +25,17 @@ namespace SHADE
|
||||||
|
|
||||||
bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
bool SHCollision::SphereVsSphere(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||||
{
|
{
|
||||||
const SHSphereCollisionShape& SPHERE_A = reinterpret_cast<const SHSphereCollisionShape&>(A);
|
const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
|
||||||
const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
|
const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
|
||||||
|
|
||||||
return SHSphere::Intersect(SPHERE_A, SPHERE_B);
|
return SHSphere::Intersect(SPHERE_A, SPHERE_B);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
bool SHCollision::SphereVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
|
||||||
{
|
{
|
||||||
// Convert to spheres
|
// Convert to underlying types
|
||||||
const SHSphereCollisionShape& SPHERE_A = reinterpret_cast<const SHSphereCollisionShape&>(A);
|
const SHSphereCollisionShape& SPHERE_A = dynamic_cast<const SHSphereCollisionShape&>(A);
|
||||||
const SHSphereCollisionShape& SPHERE_B = reinterpret_cast<const SHSphereCollisionShape&>(B);
|
const SHSphereCollisionShape& SPHERE_B = dynamic_cast<const SHSphereCollisionShape&>(B);
|
||||||
|
|
||||||
const SHVec3 A_TO_B = SPHERE_B.GetCenter() - SPHERE_A.GetCenter();
|
const SHVec3 A_TO_B = SPHERE_B.GetCenter() - SPHERE_A.GetCenter();
|
||||||
const float DISTANCE_BETWEEN_CENTERS_SQUARED = A_TO_B.LengthSquared();
|
const float DISTANCE_BETWEEN_CENTERS_SQUARED = A_TO_B.LengthSquared();
|
||||||
|
|
Loading…
Reference in New Issue