diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 4230dcb8..632dd485 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -5,7 +5,7 @@ Components: Transform Component: Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883} - Rotate: {x: -1.48352849, y: -4.78713309e-06, z: 0.469859391} + Rotate: {x: 0, y: 0, z: 0.436332315} Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722} IsActive: true RigidBody Component: @@ -66,7 +66,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: -2.49145222, y: 6.17996597, z: 0.811235607} + Translate: {x: -2.01111817, y: 7, z: 0.695722342} Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true @@ -112,7 +112,7 @@ Components: Transform Component: Translate: {x: 0, y: 4.09544182, z: 0} - Rotate: {x: -0, y: 0, z: 0} + Rotate: {x: -0, y: 0, z: -0.436332315} Scale: {x: 4.61071014, y: 0.999995887, z: 1} IsActive: true RigidBody Component: diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index e84bd16b..568d33a3 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -73,6 +73,7 @@ namespace SHADE contact.featurePair.key = 0; // Check if center is inside polyhedron (below the face) + // TODO: Change to a point in polygon test. if (FACE_QUERY.bestDistance < SHMath::EPSILON) { manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace); @@ -116,112 +117,111 @@ namespace SHADE // 2. Same side as adjacent normal // Check in regions A + const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; + const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); + + SHVec3 tangent = SHVec3::Normalise(P2 - P1); + + float projection = SHVec3::Dot(P1_TO_CENTER, tangent); + + if (projection >= 0.0f) { - const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES; - const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index); + // Find closest point + const SHVec3 CP = P1 + projection * tangent; - const SHVec3 TANGENT = SHVec3::Normalise(P2 - P1); + // Check 2nd condition + // Get adjacent normal from twin half edge + const int32_t EDGE_INDEX = FACE.vertexIndices[CLOSEST_POINT].edgeIndex; + const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; - float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); + const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; + const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); if (projection >= 0.0f) { - // Find closest point - const SHVec3 CP = P1 + projection * TANGENT; + // Must be smaller than radius + if (projection >= RADIUS) + return false; - // Check 2nd condition - // Get adjacent normal from twin half edge - const int32_t EDGE_INDEX = FACE.vertexIndices[CLOSEST_POINT].edgeIndex; - const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; + const SHVec3 CP_TO_CENTER = CENTER - CP; - const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; - const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); - projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); - if (projection >= 0.0f) - { - // Must be smaller than radius - if (projection >= RADIUS) - return false; + contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); + contact.position = CP; - const SHVec3 CP_TO_CENTER = CENTER - CP; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; - manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); - - contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - contact.position = CP; - - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; - - return true; - } + return true; } } // Check in region B + const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; + const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + + tangent = SHVec3::Normalise(P3 - P1); + + projection = SHVec3::Dot(P1_TO_CENTER, tangent); + + if (projection >= 0.0f) { - const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1; - const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index); + // Find closest point + const SHVec3 CP = P1 + projection * tangent; - const SHVec3 TANGENT = SHVec3::Normalise(P3 - P1); + // Check 2nd condition + // Get adjacent normal from twin half edge + const int32_t EDGE_INDEX = FACE.vertexIndices[P3_INDEX].edgeIndex; + const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; - float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT); + const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; + const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); if (projection >= 0.0f) { - // Find closest point - const SHVec3 CP = P1 + projection * TANGENT; + // Must be smaller than radius + if (projection >= RADIUS) + return false; - // Check 2nd condition - // Get adjacent normal from twin half edge - const int32_t EDGE_INDEX = FACE.vertexIndices[P3_INDEX].edgeIndex; - const int32_t TWIN_EDGE = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex; + const SHVec3 CP_TO_CENTER = CENTER - CP; - const int32_t ADJ_FACE = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex; - const SHVec3& ADJ_NORMAL = POLYHEDRON.GetNormal(ADJ_FACE); + manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); - projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL); - if (projection >= 0.0f) - { - // Must be smaller than radius - if (projection >= RADIUS) - return false; + contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); + contact.position = CP; - const SHVec3 CP_TO_CENTER = CENTER - CP; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; - manifold.normal = -SHVec3::Normalise(CP_TO_CENTER); - - contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal); - contact.position = CP; - - manifold.contacts[numContacts++] = contact; - manifold.numContacts = numContacts; - - return true; - } + return true; } } - // Either region C or D. Take whichever depth is smaller + // Region C has a negative dot product with any of the tangents. - const float C_PENETRATION = RADIUS - P1_TO_CENTER.Length(); - - if (std::fabs(C_PENETRATION) < RADIUS && std::fabs(C_PENETRATION) < PENETRATION) + projection = SHVec3::Dot(P1_TO_CENTER, tangent); + if (projection < 0) { manifold.normal = -SHVec3::Normalise(P1_TO_CENTER); - contact.penetration = C_PENETRATION; + contact.penetration = RADIUS - P1_TO_CENTER.Length(); contact.position = P1; - } - else - { - manifold.normal = -FACE_NORMAL; - contact.penetration = PENETRATION; - contact.position = CENTER - FACE_NORMAL * RADIUS; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; } + // Region D + manifold.normal = -FACE_NORMAL; + + contact.penetration = PENETRATION; + contact.position = CENTER - FACE_NORMAL * RADIUS; + manifold.contacts[numContacts++] = contact; manifold.numContacts = numContacts;