Fixed voronoi region tests for sphere vs convex polyhedron
This commit is contained in:
parent
3586c7ffdc
commit
82d46fce99
|
@ -4,22 +4,22 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Transform Component:
|
Transform Component:
|
||||||
Translate: {x: 0, y: 2.5, z: 0}
|
Translate: {x: -1.80977702, y: 3, z: 0}
|
||||||
Rotate: {x: -0, y: 0, z: -0}
|
Rotate: {x: -0, y: 0, z: -0.506194055}
|
||||||
Scale: {x: 1, y: 1, z: 1}
|
Scale: {x: 3.27252102, y: 0.999997199, z: 1}
|
||||||
IsActive: true
|
IsActive: true
|
||||||
RigidBody Component:
|
RigidBody Component:
|
||||||
Type: Dynamic
|
Type: Static
|
||||||
Auto Mass: false
|
Auto Mass: false
|
||||||
Mass: 10
|
Mass: .inf
|
||||||
Drag: 1
|
Drag: 0.00999999978
|
||||||
Angular Drag: 1
|
Angular Drag: 0.00999999978
|
||||||
Use Gravity: true
|
Use Gravity: true
|
||||||
Gravity Scale: 1
|
Gravity Scale: 1
|
||||||
Interpolate: true
|
Interpolate: true
|
||||||
Sleeping Enabled: true
|
Sleeping Enabled: true
|
||||||
Freeze Position X: false
|
Freeze Position X: false
|
||||||
Freeze Position Y: true
|
Freeze Position Y: false
|
||||||
Freeze Position Z: false
|
Freeze Position Z: false
|
||||||
Freeze Rotation X: false
|
Freeze Rotation X: false
|
||||||
Freeze Rotation Y: false
|
Freeze Rotation Y: false
|
||||||
|
@ -49,9 +49,9 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Camera Component:
|
Camera Component:
|
||||||
Position: {x: 3, y: 4, z: 0}
|
Position: {x: 0, y: 4, z: 5}
|
||||||
Pitch: 0
|
Pitch: 0
|
||||||
Yaw: 90
|
Yaw: 0
|
||||||
Roll: 0
|
Roll: 0
|
||||||
Width: 1920
|
Width: 1920
|
||||||
Height: 1080
|
Height: 1080
|
||||||
|
@ -107,7 +107,7 @@
|
||||||
NumberOfChildren: 0
|
NumberOfChildren: 0
|
||||||
Components:
|
Components:
|
||||||
Transform Component:
|
Transform Component:
|
||||||
Translate: {x: 0, y: 5, z: 0.834425449}
|
Translate: {x: -1.97624588, y: 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
|
||||||
|
@ -115,14 +115,14 @@
|
||||||
Type: Dynamic
|
Type: Dynamic
|
||||||
Auto Mass: false
|
Auto Mass: false
|
||||||
Mass: 1
|
Mass: 1
|
||||||
Drag: 1
|
Drag: 0.00999999978
|
||||||
Angular Drag: 1
|
Angular Drag: 0.00999999978
|
||||||
Use Gravity: true
|
Use Gravity: true
|
||||||
Gravity Scale: 1
|
Gravity Scale: 1
|
||||||
Interpolate: true
|
Interpolate: true
|
||||||
Sleeping Enabled: true
|
Sleeping Enabled: true
|
||||||
Freeze Position X: false
|
Freeze Position X: false
|
||||||
Freeze Position Y: true
|
Freeze Position Y: false
|
||||||
Freeze Position Z: false
|
Freeze Position Z: false
|
||||||
Freeze Rotation X: false
|
Freeze Rotation X: false
|
||||||
Freeze Rotation Y: false
|
Freeze Rotation Y: false
|
||||||
|
|
|
@ -159,8 +159,6 @@ namespace SHADE
|
||||||
if (index < 0 || index >= NUM_VERTICES)
|
if (index < 0 || index >= NUM_VERTICES)
|
||||||
throw std::invalid_argument("Index out-of-range!");
|
throw std::invalid_argument("Index out-of-range!");
|
||||||
|
|
||||||
// DirectX already puts vertex 0 - 4 on the front face for our case.
|
|
||||||
// Otherwise, it would need to be wrapped around for the correct vertex.
|
|
||||||
return GetVertices()[index];
|
return GetVertices()[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -101,22 +101,22 @@ namespace SHADE
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t numContacts = 0;
|
uint32_t numContacts = 0;
|
||||||
const float penetration = TOTAL_RADIUS - bestDistance;
|
|
||||||
|
|
||||||
// Rotate the normal into the world space
|
// Rotate the normal into the world space
|
||||||
const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex);
|
const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex);
|
||||||
|
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;
|
||||||
|
|
||||||
SHContact newContact;
|
SHContact newContact;
|
||||||
newContact.penetration = penetration;
|
newContact.penetration = PENETRATION;
|
||||||
newContact.position = SPHERE.GetCenter();
|
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * PENETRATION;
|
||||||
newContact.featurePair.key = 0;
|
newContact.featurePair.key = 0;
|
||||||
|
|
||||||
manifold.contacts[numContacts++] = newContact;
|
manifold.contacts[numContacts++] = newContact;
|
||||||
manifold.normal = BEST_NORMAL;
|
|
||||||
|
|
||||||
manifold.numContacts = numContacts;
|
manifold.numContacts = numContacts;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -125,95 +125,98 @@ namespace SHADE
|
||||||
// We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
|
// 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
|
// If none of these are true, the sphere is above the face but not separating
|
||||||
|
|
||||||
|
/*
|
||||||
|
* | A
|
||||||
|
* _ _ _ _ _ _ | _ _ _
|
||||||
|
* / /
|
||||||
|
* | / | / regionA
|
||||||
|
* |/ _ _ _ _ _|/ _ _ _
|
||||||
|
* B/ regionB /C
|
||||||
|
* / / regionC
|
||||||
|
*/
|
||||||
|
|
||||||
const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
|
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 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 C = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]);
|
||||||
const SHVec3 CP_TO_CENTER = SPHERE.GetCenter() - CLOSEST_POINT;
|
const SHVec3 C_TO_CENTER = SPHERE.GetCenter() - C;
|
||||||
|
|
||||||
// Check closest point -> prev point
|
const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE;
|
||||||
|
const int32_t INDEX_B = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1;
|
||||||
|
|
||||||
|
const SHVec3 POINTS[2] =
|
||||||
{
|
{
|
||||||
const int32_t PREV_POINT_INDEX = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1;
|
CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_A]) // A
|
||||||
const SHVec3& PREV_POINT = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[PREV_POINT_INDEX]);
|
, CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_B]) // B
|
||||||
|
};
|
||||||
|
|
||||||
const SHVec3 CP_TO_PREV = SHVec3::Normalise(PREV_POINT - CLOSEST_POINT);
|
// To be inside either region A or B, 2 conditions must be satisfied
|
||||||
|
// 1. Same side as tangent
|
||||||
|
// 2. Same side as normal from edge to sphere
|
||||||
|
|
||||||
float projection = SHVec3::Dot(CP_TO_CENTER, CP_TO_PREV);
|
// Check in regions A & B
|
||||||
|
for (int i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
const SHVec3 TANGENT = SHVec3::Normalise(POINTS[i] - C);
|
||||||
|
|
||||||
|
float projection = SHVec3::Dot(C_TO_CENTER, TANGENT);
|
||||||
if (projection >= 0.0f)
|
if (projection >= 0.0f)
|
||||||
{
|
{
|
||||||
// Sphere is inside this region, check if distance from center is lesser than radius
|
// Check 2nd condition
|
||||||
if (penetration >= TOTAL_RADIUS)
|
// Find closest point
|
||||||
return false;
|
const SHVec3 CP = C + projection * TANGENT;
|
||||||
|
const SHVec3 CP_TO_CENTER = SHVec3::Normalise(C - CP);
|
||||||
|
|
||||||
SHContact newContact;
|
projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER);
|
||||||
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)
|
if (projection >= 0.0f)
|
||||||
{
|
{
|
||||||
// Sphere is inside this region, check if distance from center is lesser than radius
|
// Sphere Within region A
|
||||||
if (penetration >= TOTAL_RADIUS)
|
manifold.normal = CP_TO_CENTER;
|
||||||
return false;
|
|
||||||
|
|
||||||
SHContact newContact;
|
SHContact newContact;
|
||||||
newContact.penetration = penetration;
|
newContact.penetration = TOTAL_RADIUS - projection;
|
||||||
newContact.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
|
newContact.position = CP;
|
||||||
newContact.featurePair.key = 0;
|
newContact.featurePair.key = 0;
|
||||||
|
|
||||||
manifold.contacts[numContacts++] = newContact;
|
manifold.contacts[numContacts++] = newContact;
|
||||||
manifold.normal = BEST_NORMAL;
|
|
||||||
|
|
||||||
manifold.numContacts = numContacts;
|
manifold.numContacts = numContacts;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check region C (closest point)
|
||||||
|
{
|
||||||
|
if (C_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS)
|
||||||
|
{
|
||||||
|
manifold.normal = SHVec3::Normalise(C_TO_CENTER);
|
||||||
|
|
||||||
|
SHContact newContact;
|
||||||
|
newContact.penetration = PENETRATION;
|
||||||
|
newContact.position = C;
|
||||||
|
newContact.featurePair.key = 0;
|
||||||
|
|
||||||
|
manifold.contacts[numContacts++] = newContact;
|
||||||
|
manifold.numContacts = numContacts;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if it hit the closest point
|
// Region D
|
||||||
{
|
if (PENETRATION <= TOTAL_RADIUS)
|
||||||
if (CP_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS)
|
|
||||||
{
|
{
|
||||||
|
manifold.normal = -BEST_NORMAL;
|
||||||
|
|
||||||
SHContact newContact;
|
SHContact newContact;
|
||||||
newContact.penetration = penetration;
|
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.position = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
|
||||||
newContact.featurePair.key = 0;
|
newContact.featurePair.key = 0;
|
||||||
|
|
||||||
manifold.contacts[numContacts++] = newContact;
|
manifold.contacts[numContacts++] = newContact;
|
||||||
manifold.normal = BEST_NORMAL;
|
|
||||||
|
|
||||||
manifold.numContacts = numContacts;
|
manifold.numContacts = numContacts;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -81,7 +81,7 @@ namespace SHADE
|
||||||
{
|
{
|
||||||
const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f });
|
const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f });
|
||||||
debugDrawSystem->DrawCube(TRS, CONTACT_COLOUR);
|
debugDrawSystem->DrawCube(TRS, CONTACT_COLOUR);
|
||||||
debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.3f, CONTACT_COLOUR, true);
|
debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.5f, CONTACT_COLOUR, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue