diff --git a/Assets/Scenes/SS_Playground.shade b/Assets/Scenes/SS_Playground.shade index beee48b3..4f3ba0c8 100644 --- a/Assets/Scenes/SS_Playground.shade +++ b/Assets/Scenes/SS_Playground.shade @@ -23,15 +23,39 @@ Transform Component: Translate: {x: 0, y: 0, z: 0} Rotate: {x: 0, y: 0, z: 0} - Scale: {x: 2, y: 1, z: 1} + Scale: {x: 1, y: 1, z: 1} + 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: ~ +- EID: 3 + Name: Default + IsActive: true + NumberOfChildren: 0 + Components: + Transform Component: + Translate: {x: 0, y: 2, z: 0} + Rotate: {x: 0, y: 0.785398185, z: 0} + Scale: {x: 1, y: 1, z: 1} IsActive: true RigidBody Component: - Type: Static + Type: Dynamic Auto Mass: false - Mass: .inf + Mass: 1 Drag: 0.00999999978 Angular Drag: 0.00999999978 - Use Gravity: false + Use Gravity: true Gravity Scale: 1 Interpolate: true Sleeping Enabled: true @@ -55,45 +79,4 @@ Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} IsActive: true - Scripts: ~ -- EID: 2 - Name: Default - IsActive: true - NumberOfChildren: 0 - Components: - Transform Component: - Translate: {x: 1.14999998, y: 3, z: 0.550000012} - Rotate: {x: 0, y: 0, z: 0} - Scale: {x: 1, y: 1, z: 1} - IsActive: true - RigidBody Component: - Type: Dynamic - Auto Mass: false - Mass: 1 - Drag: 0.00999999978 - Angular Drag: 0.00999999978 - Use Gravity: true - Gravity Scale: 0.25 - 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: 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: ~ \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 9e7685fc..09b5284d 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -39,7 +39,7 @@ namespace SHADE int32_t findIncidentFace (const SHConvexPolyhedron&, const SHVec3& normal) noexcept; bool findFaceContacts (SHManifold&, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept; SHCollisionUtils::ClipVertices clipPolygonWithPlane (const SHCollisionUtils::ClipVertices& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept; - //std::vector reduceContacts (const std::vector& in, const SHVec3& faceNormal) noexcept; + std::vector reduceContacts (const std::vector& in, const SHVec3& faceNormal) noexcept; /*-----------------------------------------------------------------------------------*/ /* Public Member Functions Definitions */ @@ -67,140 +67,143 @@ namespace SHADE bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - //static constexpr float ABSOLUTE_TOLERANCE = 0.01f; - //static constexpr float RELATIVE_TOLERANCE = 0.95f; + static constexpr float ABSOLUTE_TOLERANCE = 0.01f; + static constexpr float RELATIVE_TOLERANCE = 0.95f; - //const SHConvexPolyhedron& POLY_A = dynamic_cast(A); - //const SHConvexPolyhedron& POLY_B = dynamic_cast(B); + const SHConvexPolyhedron& POLY_A = dynamic_cast(A); + const SHConvexPolyhedron& POLY_B = dynamic_cast(B); - //if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) - // return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); + const SHCollisionUtils::ShapeTransform TF_A { POLY_A.GetWorldCentroid(), POLY_A.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_B { POLY_B.GetWorldCentroid(), POLY_B.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse(); + const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse(); - //SHSATInfo cachedInfo; + if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID) + return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B); - //const SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); - //if (FACE_QUERY_A.bestDistance > 0.0f) - //{ - // // cache the info - // cachedInfo.type = SHSATInfo::Type::FACE; - // cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; - // cachedInfo.info.refFace = FACE_QUERY_A.closestFace; + SHSATInfo cachedInfo; - // manifold.cachedSATInfo = cachedInfo; + const SHCollisionUtils::FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); + if (FACE_QUERY_A.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; + cachedInfo.info.refFace = FACE_QUERY_A.closestFace; - // return false; - //} - // + manifold.cachedSATInfo = cachedInfo; - //const SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); - //if (FACE_QUERY_B.bestDistance > 0.0f) - //{ - // // cache the info - // cachedInfo.type = SHSATInfo::Type::FACE; - // cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; - // cachedInfo.info.refFace = FACE_QUERY_B.closestFace; + return false; + } + - // manifold.cachedSATInfo = cachedInfo; + const SHCollisionUtils::FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); + if (FACE_QUERY_B.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; + cachedInfo.info.refFace = FACE_QUERY_B.closestFace; - // return false; - //} + manifold.cachedSATInfo = cachedInfo; - //const SHCollisionUtils::EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); - //if (EDGE_QUERY.bestDistance > 0.0f) - //{ - // // cache the info - // cachedInfo.type = SHSATInfo::Type::EDGE; - // cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; - // cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; + return false; + } - // manifold.cachedSATInfo = cachedInfo; + const SHCollisionUtils::EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); + if (EDGE_QUERY.bestDistance > 0.0f) + { + // cache the info + cachedInfo.type = SHSATInfo::Type::EDGE; + cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; + cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; - // return false; - //} + manifold.cachedSATInfo = cachedInfo; - //// Apply weight to improve frame coherence of normal directions. - //// We want a normal in the direction from A -> B, so we flip the normal if needed. - //bool flipNormal = false; - //const SHConvexPolyhedron* referencePoly = nullptr; - //const SHConvexPolyhedron* incidentPoly = nullptr; + return false; + } - //SHCollisionUtils::FaceQuery minFaceQuery; - //if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) - //{ - // minFaceQuery = FACE_QUERY_A; - // referencePoly = &POLY_A; - // incidentPoly = &POLY_B; - // flipNormal = false; + // Apply weight to improve frame coherence of normal directions. + // We want a normal in the direction from A -> B, so we flip the normal if needed. + bool flipNormal = false; + const SHConvexPolyhedron* referencePoly = nullptr; + const SHConvexPolyhedron* incidentPoly = nullptr; - // cachedInfo.type = SHSATInfo::Type::FACE; - // cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; - // cachedInfo.info.refFace = minFaceQuery.closestFace; - //} - //else - //{ - // minFaceQuery = FACE_QUERY_B; - // referencePoly = &POLY_B; - // incidentPoly = &POLY_A; - // flipNormal = true; + SHCollisionUtils::FaceQuery minFaceQuery; + if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE) + { + minFaceQuery = FACE_QUERY_A; + referencePoly = &POLY_A; + incidentPoly = &POLY_B; + flipNormal = false; - // cachedInfo.type = SHSATInfo::Type::FACE; - // cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; - // cachedInfo.info.refFace = minFaceQuery.closestFace; - //} + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::A; + cachedInfo.info.refFace = minFaceQuery.closestFace; + } + else + { + minFaceQuery = FACE_QUERY_B; + referencePoly = &POLY_B; + incidentPoly = &POLY_A; + flipNormal = true; - // + cachedInfo.type = SHSATInfo::Type::FACE; + cachedInfo.info.refPoly = SHSATInfo::ShapeID::B; + cachedInfo.info.refFace = minFaceQuery.closestFace; + } - //// If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on - //// each edge and use that as the contact point. - //if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE) - //{ - // const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); - // const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); + // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on + // each edge and use that as the contact point. + if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE) + { + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); - // const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex); - // const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex); - // const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex); - // const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex); + const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex); + const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex); + const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex); + const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex); - // const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); - // const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); + const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); + const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); - // manifold.normal = SHVec3::Cross(VB, VA); - // // Flip normal if need to ( A -> B) - // if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) - // manifold.normal = -manifold.normal; + manifold.normal = SHVec3::Cross(VB, VA); + // Flip normal if need to ( A -> B) + if (SHVec3::Dot(manifold.normal, TAIL_A - A.GetWorldCentroid()) < 0.0f) + manifold.normal = -manifold.normal; - // // In this scenario, we only have one contact - // uint32_t numContacts = 0; + // In this scenario, we only have one contact + uint32_t numContacts = 0; - // const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); + const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(POLY_A, POLY_B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); - // SHContact contact; - // contact.featurePair.key = EDGE_QUERY.axis; - // contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f); - // contact.penetration = -EDGE_QUERY.bestDistance; + SHContact contact; + contact.featurePair.key = EDGE_QUERY.axis; + contact.penetration = -EDGE_QUERY.bestDistance; - // manifold.contacts[numContacts++] = contact; - // manifold.numContacts = numContacts; + contact.localPointA = INV_TF_A * CONTACTS.closestPointB; + contact.localPointB = INV_TF_B * CONTACTS.closestPointA; - // // Cache the info - // cachedInfo.type = SHSATInfo::Type::EDGE; - // cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; - // cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; - // manifold.cachedSATInfo = cachedInfo; + // Cache the info + cachedInfo.type = SHSATInfo::Type::EDGE; + cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA; + cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB; - // return true; - //} + manifold.cachedSATInfo = cachedInfo; - //const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); - //const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); + return true; + } - //manifold.cachedSATInfo = cachedInfo; - //return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); + const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace); + const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL); - return false; + manifold.cachedSATInfo = cachedInfo; + return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal); } /*-----------------------------------------------------------------------------------*/ @@ -295,6 +298,7 @@ namespace SHADE const float ADC = SHVec3::Dot(a, DXC); const float BDC = SHVec3::Dot(b, DXC); + // Short circuit may be faster without storing the booleans return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f; } @@ -311,17 +315,17 @@ namespace SHADE const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A); const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B); - // Check if the edges are parallel (abs dot product is 1) - const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B)); - if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f)) + // Check if the edges are parallel (cross product length close to 0) + if (SHMath::CompareFloat(SHVec3::Cross(DIR_A, DIR_B).LengthSquared(), 0.0f)) return std::numeric_limits::lowest(); SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B); - // Flip normal if need to ( A -> B) - if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) + + // Flip normal if need to (A -> B) + if (SHVec3::Dot(normal, TAIL_A - A.GetWorldCentroid()) < 0.0f) normal = -normal; - return SHVec3::Dot(normal, HEAD_B - HEAD_A); + return SHVec3::Dot(normal, TAIL_B - TAIL_A); } SHCollisionUtils::EdgeContacts findClosestPointBetweenEdges(const SHConvexPolyhedron& polyA, const SHConvexPolyhedron& polyB, int32_t edgeA, int32_t edgeB) noexcept @@ -347,7 +351,7 @@ namespace SHADE const float A = D1.LengthSquared(); // a const float E = D2.LengthSquared(); // e - const float B = SHVec3::Dot(D1, D2); // b + const float B = SHVec3::Dot(D1, D2); // b const float C = SHVec3::Dot(D1, R); // c const float F = SHVec3::Dot(D2, R); // f @@ -425,144 +429,159 @@ namespace SHADE bool findFaceContacts(SHManifold& manifold, const SHConvexPolyhedron& incPoly, int32_t incFace, const SHConvexPolyhedron& refPoly, int32_t refFace, bool flip) noexcept { - //const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); - //const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); + const SHCollisionUtils::ShapeTransform TF_INC { incPoly.GetWorldCentroid(), incPoly.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_REF { refPoly.GetWorldCentroid(), refPoly.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform INV_TF_INC = TF_INC.GetInverse(); + const SHCollisionUtils::ShapeTransform INV_TF_REF = TF_REF.GetInverse(); - //const int32_t NUM_INCIDENT_VERTICES = static_cast(INCIDENT_FACE.vertexIndices.size()); - //const int32_t NUM_REFERENCE_VERTICES = static_cast(REFERENCE_FACE.vertexIndices.size()); - //// Build incoming vertices to clip - //SHCollisionUtils::ClipVertices clipIn; - //clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{}); + const SHHalfEdgeStructure::Face& INCIDENT_FACE = incPoly.GetFace(incFace); + const SHHalfEdgeStructure::Face& REFERENCE_FACE = refPoly.GetFace(refFace); - //int32_t numClipIn = 0; - //for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i) - //{ - // const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1; + const int32_t NUM_INCIDENT_VERTICES = static_cast(INCIDENT_FACE.vertexIndices.size()); + const int32_t NUM_REFERENCE_VERTICES = static_cast(REFERENCE_FACE.vertexIndices.size()); - // const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index; + const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); + const SHVec3 LOCAL_REF_NORMAL = SHVec3::Rotate(REF_NORMAL, INV_TF_REF.orientation); - // // The incoming id is the previous edge - // // The outgoing id is the current edge (where this vertex is the tail of) + // Build incoming vertices to clip + // But it in the reference local-space + SHCollisionUtils::ClipVertices clipIn; + clipIn.resize(NUM_INCIDENT_VERTICES * 2, SHCollisionUtils::ClipVertex{}); - // SHCollisionUtils::ClipVertex v; - // v.position = incPoly.GetVertex(V_INDEX); - // v.featurePair.inI = INCIDENT_FACE.vertexIndices[prevI].edgeIndex; - // v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex; - // v.featurePair.inR = 0; - // v.featurePair.outR = 0; + int32_t numClipIn = 0; + for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i) + { + const int32_t PREV_I = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1; + const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index; - // clipIn[numClipIn++] = v; - //} + // The incoming id is the previous edge + // The outgoing id is the current edge (where this vertex is the tail of) - //// Clip the vertices against the reference face side planes. - //// Number of side planes == number of edges == number of vertices - //const SHVec3 REF_NORMAL = refPoly.GetNormal(refFace); - //for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i) - //{ - // // Side plane can be built with the vertex on the edge and the plane's normal - // // Plane normal = faceNormal X tangent (v2 - v1) + SHCollisionUtils::ClipVertex v; + v.position = INV_TF_REF * incPoly.GetVertex(V_INDEX); + v.featurePair.inI = INCIDENT_FACE.vertexIndices[PREV_I].edgeIndex; + v.featurePair.outI = INCIDENT_FACE.vertexIndices[i].edgeIndex; + v.featurePair.inR = 0; + v.featurePair.outR = 0; - // const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index; - // const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index; + clipIn[numClipIn++] = v; + } - // const SHVec3 V1 = refPoly.GetVertex(V1_INDEX); - // const SHVec3 V2 = refPoly.GetVertex(V2_INDEX); + // Clip the vertices against the reference face side planes. + // Number of side planes == number of edges == number of vertices + for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i) + { + // Side plane can be built with the vertex on the edge and the plane's normal + // Plane normal = faceNormal X tangent (v2 - v1) - // const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); - // const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REF_NORMAL, TANGENT) }; + const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index; + const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index; - // SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); - // if (clipOut.empty()) - // return false; + // Get reference vertices in reference local space + const SHVec3 V1 = INV_TF_REF * refPoly.GetVertex(V1_INDEX); + const SHVec3 V2 = INV_TF_REF * refPoly.GetVertex(V2_INDEX); - // // Replace the clip container's contents with the clipped points for the next clipping pass - // const int32_t NUM_CLIPPED = static_cast(clipOut.size()); - // for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex) - // { - // clipIn[clippedIndex].position = clipOut[clippedIndex].position; - // clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key; - // } - // numClipIn = NUM_CLIPPED; - //} + const SHVec3 TANGENT = SHVec3::Normalise(V2 - V1); + const SHVec3 PLANE_NORMAL = SHVec3::Cross(LOCAL_REF_NORMAL, TANGENT); + const SHPlane CLIP_PLANE { V1, PLANE_NORMAL }; - //// From the final set of clipped points, only keep the points that are below the reference plane. - //const SHPlane REFERENCE_PLANE{ refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index), REF_NORMAL }; + SHCollisionUtils::ClipVertices clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex); + if (clipOut.empty()) + return false; - //uint32_t numContacts = 0; + // Replace the clip container's contents with the clipped points for the next clipping pass + const int32_t NUM_CLIPPED = static_cast(clipOut.size()); + for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex) + { + clipIn[clippedIndex].position = clipOut[clippedIndex].position; + clipIn[clippedIndex].featurePair.key = clipOut[clippedIndex].featurePair.key; + } + numClipIn = NUM_CLIPPED; + } - //std::vector contacts; - //for (int32_t i = 0; i < numClipIn; ++i) - //{ - // const SHVec3 POS = clipIn[i].position; - // const float DIST = REFERENCE_PLANE.SignedDistance(POS); - // if (DIST <= 0.0f) - // { - // SHContact contact; - // contact.position = POS; - // contact.penetration = -DIST; + // From the final set of clipped points, only keep the points that are below the reference plane. + const SHVec3 LOCAL_REF_POINT = INV_TF_REF * refPoly.GetVertex(REFERENCE_FACE.vertexIndices.front().index); + const SHPlane REFERENCE_PLANE{ LOCAL_REF_POINT, LOCAL_REF_NORMAL }; - // if (flip) - // { - // contact.featurePair.inI = clipIn[i].featurePair.inR; - // contact.featurePair.inR = clipIn[i].featurePair.inI; - // contact.featurePair.outI = clipIn[i].featurePair.outR; - // contact.featurePair.outR = clipIn[i].featurePair.outI; - // } - // else - // { - // contact.featurePair.key = clipIn[i].featurePair.key; - // } + uint32_t numContacts = 0; - // contacts.emplace_back(contact); - // ++numContacts; - // } - //} + std::vector contacts; + for (int32_t i = 0; i < numClipIn; ++i) + { + const SHVec3 POS = clipIn[i].position; + const float DIST = REFERENCE_PLANE.SignedDistance(POS); + if (DIST <= 0.0f) + { + SHContact contact; + contact.penetration = -DIST; - //// Reduce contact manifold if more than 4 points - //if (numContacts > 4) - //{ - // const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL); - // std::vector reducedContacts; + // A: Incident, B: Reference + // Project the clipped point onto the reference place for localPointB + contact.localPointA = INV_TF_INC * (TF_REF * POS); + contact.localPointB = POS + LOCAL_REF_NORMAL * SHVec3::Dot(LOCAL_REF_NORMAL, POS - LOCAL_REF_POINT); - // const int32_t NUM_REDUCED = static_cast(INDICES_TO_KEEP.size()); - // for (int32_t i = 0; i < NUM_REDUCED; ++i) - // reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]); + if (flip) + { + contact.featurePair.inI = clipIn[i].featurePair.inR; + contact.featurePair.inR = clipIn[i].featurePair.inI; + contact.featurePair.outI = clipIn[i].featurePair.outR; + contact.featurePair.outR = clipIn[i].featurePair.outI; - // contacts.clear(); - // // Copy contacts to main container - // for (auto& contact : reducedContacts) - // contacts.emplace_back(contact); - //} + std::swap(contact.localPointA, contact.localPointB); + } + else + { + contact.featurePair.key = clipIn[i].featurePair.key; + } - //// Remove potential duplicate contact points - //// No way about this being an n^2 loop - //static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE; - //for (auto i = contacts.begin(); i != contacts.end(); ++i) - //{ - // for (auto j = i + 1; j != contacts.end();) - // { - // const float D2 = SHVec3::DistanceSquared(i->position, j->position); - // if (D2 < THRESHOLD) - // j = contacts.erase(j); - // else - // ++j; - // } - //} + contacts.emplace_back(contact); + ++numContacts; + } + } - //// Copy final contacts into the manifold - //numContacts = static_cast(contacts.size()); - //for (int32_t i = 0; i < numContacts; ++i) - // manifold.contacts[i] = contacts[i]; + // Reduce contact manifold if more than 4 points + if (numContacts > 4) + { + const auto INDICES_TO_KEEP = reduceContacts(contacts, REF_NORMAL); + std::vector reducedContacts; - //manifold.numContacts = numContacts; - //manifold.normal = REF_NORMAL; - //if (flip) - // manifold.normal = -manifold.normal; + const int32_t NUM_REDUCED = static_cast(INDICES_TO_KEEP.size()); + for (int32_t i = 0; i < NUM_REDUCED; ++i) + reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]); - //return true; + contacts.clear(); + // Copy contacts to main container + for (auto& contact : reducedContacts) + contacts.emplace_back(contact); + } - return false; + // Remove potential duplicate contact points + // No way about this being an n^2 loop + static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE; + for (auto i = contacts.begin(); i != contacts.end(); ++i) + { + for (auto j = i + 1; j != contacts.end();) + { + const float D2 = SHVec3::DistanceSquared(i->localPointA, j->localPointA); + if (D2 < THRESHOLD) + j = contacts.erase(j); + else + ++j; + } + } + + // Copy final contacts into the manifold + numContacts = static_cast(contacts.size()); + for (int32_t i = 0; i < numContacts; ++i) + manifold.contacts[i] = contacts[i]; + + manifold.numContacts = numContacts; + manifold.normal = REF_NORMAL; + if (flip) + manifold.normal = -manifold.normal; + + return true; } @@ -591,9 +610,12 @@ namespace SHADE { SHCollisionUtils::ClipVertex intersection; + const SHVec3 PLANE_NORMAL = -plane.GetNormal(); + // In case the edge is parallel, the intersection is just the start point - const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f); - const float ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos); + const float DOT = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL); + const bool IS_PARALLEL = SHMath::CompareFloat(DOT, 0.0f); + const float ALPHA = IS_PARALLEL ? 0.0f : (plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT; intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); intersection.featurePair.inI = in[v1Index].featurePair.outI; @@ -610,9 +632,12 @@ namespace SHADE { SHCollisionUtils::ClipVertex intersection; - // In case the edge is parallel, the intersection is just the start point - const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f); - const float ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos); + const SHVec3 PLANE_NORMAL = plane.GetNormal(); + + // In case the edge is parallel, the intersection is just the end point + const float DOT = SHVec3::Dot(v2Pos - v1Pos, PLANE_NORMAL); + const bool IS_PARALLEL = SHMath::CompareFloat(DOT, 0.0f); + const float ALPHA = IS_PARALLEL ? 1.0f : (-plane.GetDistance() - SHVec3::Dot(v1Pos, PLANE_NORMAL)) / DOT; intersection.position = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA); intersection.featurePair.inI = 0; @@ -638,220 +663,225 @@ namespace SHADE return out; } - //std::vector reduceContacts(const std::vector& in, const SHVec3& faceNormal) noexcept - //{ - // std::vector indicesToKeep; + std::vector reduceContacts(const std::vector& in, const SHVec3& faceNormal) noexcept + { + std::vector indicesToKeep; - // // Use a map to temporarily store and track the contacts we want - // std::unordered_map contactMap; - // const int32_t NUM_CONTACTS = static_cast(in.size()); - // for (int32_t i = 0; i < NUM_CONTACTS; ++i) - // contactMap.emplace(i, &in[i]); + // Use a map to temporarily store and track the contacts we want + std::unordered_map contactMap; + const int32_t NUM_CONTACTS = static_cast(in.size()); + for (int32_t i = 0; i < NUM_CONTACTS; ++i) + contactMap.emplace(i, &in[i]); - // // Find the furthest point in a given direction - // int32_t indexToKeep = -1; - // float bestDistance = std::numeric_limits::lowest(); + // Find the furthest point in a given direction + int32_t indexToKeep = -1; + float bestDistance = std::numeric_limits::lowest(); - // for (const auto& [index, contact] : contactMap) - // { - // const float DIST = SHVec3::Dot(contact->position, SHVec3::One); - // if (DIST > bestDistance) - // { - // bestDistance = DIST; - // indexToKeep = index; - // } - // } + for (const auto& [index, contact] : contactMap) + { + const float DIST = SHVec3::Dot(contact->localPointA, SHVec3::One); + if (DIST > bestDistance) + { + bestDistance = DIST; + indexToKeep = index; + } + } - // indicesToKeep.emplace_back(indexToKeep); - // contactMap.erase(indexToKeep); + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); - // indexToKeep = -1; - // bestDistance = std::numeric_limits::lowest(); + indexToKeep = -1; + bestDistance = std::numeric_limits::lowest(); - // // Find point furthest away from the first index - // const SHVec3& FIRST_POS = in[indicesToKeep.back()].position; - // for (const auto& [index, contact] : contactMap) - // { - // const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->position); - // if (DIST_SQUARED > bestDistance) - // { - // bestDistance = DIST_SQUARED; - // indexToKeep = index; - // } - // } + // Find point furthest away from the first index + const SHVec3& FIRST_POS = in[indicesToKeep.back()].localPointA; + for (const auto& [index, contact] : contactMap) + { + const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->localPointA); + if (DIST_SQUARED > bestDistance) + { + bestDistance = DIST_SQUARED; + indexToKeep = index; + } + } - // indicesToKeep.emplace_back(indexToKeep); - // contactMap.erase(indexToKeep); + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); - // indexToKeep = -1; + indexToKeep = -1; - // // We compute the triangle with the largest area. - // // The area can be positive or negative depending on the winding order. + // We compute the triangle with the largest area. + // The area can be positive or negative depending on the winding order. - // float maxArea = std::numeric_limits::lowest(); - // float minArea = std::numeric_limits::max(); + float maxArea = std::numeric_limits::lowest(); + float minArea = std::numeric_limits::max(); - // int32_t maxAreaIndex = -1; - // int32_t minAreaIndex = -1; + int32_t maxAreaIndex = -1; + int32_t minAreaIndex = -1; - // const SHVec3& SECOND_POS = in[indicesToKeep.back()].position; - // for (const auto& [index, contact] : contactMap) - // { - // const SHVec3& POS = contact->position; - // const SHVec3 TO_P1 = FIRST_POS - POS; - // const SHVec3 TO_P2 = SECOND_POS - POS; + const SHVec3& SECOND_POS = in[indicesToKeep.back()].localPointA; + for (const auto& [index, contact] : contactMap) + { + const SHVec3& POS = contact->localPointA; + const SHVec3 TO_P1 = FIRST_POS - POS; + const SHVec3 TO_P2 = SECOND_POS - POS; - // const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; + const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; - // if (AREA > maxArea) - // { - // maxArea = AREA; - // maxAreaIndex = index; - // } + if (AREA > maxArea) + { + maxArea = AREA; + maxAreaIndex = index; + } - // if (AREA < minArea) - // { - // minArea = AREA; - // minAreaIndex = index; - // } - // } + if (AREA < minArea) + { + minArea = AREA; + minAreaIndex = index; + } + } - // // Compare which triangle creates the largest area - // bool isAreaPositive = false; - // if (maxArea > (-minArea)) - // { - // isAreaPositive = true; - // indexToKeep = maxAreaIndex; - // } - // else - // { - // isAreaPositive = false; - // indexToKeep = minAreaIndex; - // } + // Compare which triangle creates the largest area + bool isAreaPositive = false; + if (maxArea > (-minArea)) + { + isAreaPositive = true; + indexToKeep = maxAreaIndex; + } + else + { + isAreaPositive = false; + indexToKeep = minAreaIndex; + } - // indicesToKeep.emplace_back(indexToKeep); - // contactMap.erase(indexToKeep); + indicesToKeep.emplace_back(indexToKeep); + contactMap.erase(indexToKeep); - // indexToKeep = -1; + indexToKeep = -1; - // // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle - // // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa. - // float bestArea = 0.0f; + // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle + // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa. + float bestArea = 0.0f; - // const SHVec3& THIRD_POS = in[indicesToKeep.back()].position; - // const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS }; + const SHVec3& THIRD_POS = in[indicesToKeep.back()].localPointA; + const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS }; - // for (const auto& [index, contact] : contactMap) - // { - // const SHVec3& Q = contact->position; + for (const auto& [index, contact] : contactMap) + { + const SHVec3& Q = contact->localPointA; - // for (int i = 0; i < 3; ++i) - // { - // const int P1 = i; - // const int P2 = (i + 1) % 3; + for (int i = 0; i < 3; ++i) + { + const int P1 = i; + const int P2 = (i + 1) % 3; - // const SHVec3 TO_P1 = ABC[P1] - Q; - // const SHVec3 TO_P2 = ABC[P2] - Q; + const SHVec3 TO_P1 = ABC[P1] - Q; + const SHVec3 TO_P2 = ABC[P2] - Q; - // const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; + const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f; - // if (isAreaPositive && AREA < bestArea) - // { - // bestArea = AREA; - // indexToKeep = index; - // } + if (isAreaPositive && AREA < bestArea) + { + bestArea = AREA; + indexToKeep = index; + } - // if (!isAreaPositive && AREA > bestArea) - // { - // bestArea = AREA; - // indexToKeep = index; - // } - // } - // } + if (!isAreaPositive && AREA > bestArea) + { + bestArea = AREA; + indexToKeep = index; + } + } + } - // indicesToKeep.emplace_back(indexToKeep); - // return indicesToKeep; - //} + indicesToKeep.emplace_back(indexToKeep); + return indicesToKeep; + } bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept { - //if (cachedInfo.type == SHSATInfo::Type::FACE) - //{ - // SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!") + if (cachedInfo.type == SHSATInfo::Type::FACE) + { + SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!") - // // Assume the reference poly was A - // const SHConvexPolyhedron* incPoly = &B; - // const SHConvexPolyhedron* refPoly = &A; - // bool flip = false; + // Assume the reference poly was A + const SHConvexPolyhedron* incPoly = &B; + const SHConvexPolyhedron* refPoly = &A; + bool flip = false; - // // Swap if it was B - // if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B) - // { - // refPoly = &B; - // incPoly = &A; - // flip = true; - // } + // Swap if it was B + if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B) + { + refPoly = &B; + incPoly = &A; + flip = true; + } - // const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace); - // const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace); + const SHHalfEdgeStructure::Face& REF_FACE = refPoly->GetFace(cachedInfo.info.refFace); + const SHVec3 REF_NORMAL = refPoly->GetNormal(cachedInfo.info.refFace); - // const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL); + const SHVec3 SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL); - // const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL }; - // const float DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT); + const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL }; + const float DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT); - // if (DISTANCE > 0.0f) - // return false; + if (DISTANCE > 0.0f) + return false; - // // Find the incident face - // const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL); + // Find the incident face + const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL); - // return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip); - //} + return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip); + } - //if (cachedInfo.type == SHSATInfo::Type::EDGE) - //{ - // const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); - // if (DISTANCE > 0.0f) - // return false; + if (cachedInfo.type == SHSATInfo::Type::EDGE) + { + const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); + if (DISTANCE > 0.0f) + return false; - // const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA); - // const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB); + const SHCollisionUtils::ShapeTransform TF_A { A.GetWorldCentroid(), A.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_B { B.GetWorldCentroid(), B.GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse(); + const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse(); - // const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex); - // const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex); - // const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex); - // const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.edgeB); - // const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); - // const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); + const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex); + const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex); + const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex); + const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex); - // manifold.normal = SHVec3::Cross(VB, VA); - // // Flip normal if need to ( A -> B) - // if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) - // manifold.normal = -manifold.normal; + const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); + const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); - // // In this scenario, we only have one contact - // uint32_t numContacts = 0; + manifold.normal = SHVec3::Cross(VB, VA); + // Flip normal if need to ( A -> B) + if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) + manifold.normal = -manifold.normal; - // const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); + // In this scenario, we only have one contact + uint32_t numContacts = 0; - // SHContact contact; - // // Take feature pair key from previous manifold - // contact.featurePair.key = manifold.contacts[0].featurePair.key; - // contact.position = SHVec3::ClampedLerp(CONTACTS.closestPointA, CONTACTS.closestPointB, 0.5f); - // contact.penetration = -DISTANCE; + const SHCollisionUtils::EdgeContacts CONTACTS = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB); - // manifold.contacts[numContacts++] = contact; - // manifold.numContacts = numContacts; + SHContact contact; + // Take feature pair key from previous manifold + contact.featurePair.key = manifold.contacts[0].featurePair.key; + contact.penetration = -DISTANCE; - // return true; - //} + contact.localPointA = INV_TF_A * CONTACTS.closestPointB; + contact.localPointB = INV_TF_B * CONTACTS.closestPointA; + + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; + + return true; + } // Should never reach this. return false; } - - } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 68217d8b..4ced77d7 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -238,7 +238,7 @@ namespace SHADE // Build plane equation // Use first vertex to build the plane - const SHPlane FACE_PLANE { polyhedron.GetVertex(FACE.vertexIndices[0].index), polyhedron.GetNormal(i) }; + const SHPlane FACE_PLANE { polyhedron.GetVertex(FACE.vertexIndices[0].index), -polyhedron.GetNormal(i) }; // Find signed distance of center to plane const float SIGNED_DIST = FACE_PLANE.SignedDistance(CENTER); diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp index 210c4f43..caf974dc 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsSphere.cpp @@ -68,8 +68,8 @@ namespace SHADE const SHCollisionUtils::ShapeTransform TF_A = { SPHERE_A.GetWorldCentroid(), SPHERE_A.GetWorldOrientation() }; const SHCollisionUtils::ShapeTransform TF_B = { SPHERE_B.GetWorldCentroid(), SPHERE_B.GetWorldOrientation() }; - - + const SHCollisionUtils::ShapeTransform INV_TF_A = TF_A.GetInverse(); + const SHCollisionUtils::ShapeTransform INV_TF_B = TF_B.GetInverse(); // Only populate the manifold if there is a collision @@ -83,16 +83,16 @@ namespace SHADE if (SHMath::CompareFloat(DISTANCE_BETWEEN_CENTERS_SQUARED, 0.0f)) { manifold.normal = SHVec3::UnitY; - contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, TF_A.GetInverse().orientation); - contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, TF_B.GetInverse().orientation); + contact.localPointA = RADIUS_A * SHVec3::Rotate(manifold.normal, INV_TF_A.orientation); + contact.localPointB = RADIUS_B * SHVec3::Rotate(manifold.normal, INV_TF_B.orientation); manifold.contacts[numContacts++] = contact; } else { manifold.normal = SHVec3::Normalise(A_TO_B); - contact.localPointA = RADIUS_A * SHVec3::Normalise(TF_A.GetInverse() * CENTER_B); - contact.localPointB = RADIUS_B * SHVec3::Normalise(TF_B.GetInverse() * CENTER_A); + contact.localPointA = RADIUS_A * SHVec3::Normalise(INV_TF_A * CENTER_B); + contact.localPointB = RADIUS_B * SHVec3::Normalise(INV_TF_B * CENTER_A); manifold.contacts[numContacts++] = contact; diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp index c70728f3..5560c509 100644 --- a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp +++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp @@ -130,8 +130,8 @@ namespace SHADE * Right: 1 (1,5,6,2) Normal: X * Back: 2 (5,4,7,6) Normal: Z * Left: 3 (4,0,3,7) Normal: -X - * Top: 4 (3,2,6,7) Normal: Y - * Bottom: 5 (4,5,1,0) Normal: -Y + * Bottom: 4 (0,4,5,1) Normal: -Y + * Top: 5 (2,6,7,3) Normal: Y * */ @@ -143,18 +143,18 @@ namespace SHADE , SHVec3::UnitX , SHVec3::UnitZ , -SHVec3::UnitX - , SHVec3::UnitY , -SHVec3::UnitY + , SHVec3::UnitY }; const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE] { { 0, 1, 2, 3 } - , { 5, 6, 2, 1 } - , { 5, 4, 7, 6 } - , { 0, 3, 7, 4 } + , { 1, 5, 6, 2 } + , { 4, 7, 6, 5 } + , { 4, 0, 3, 7 } + , { 0, 4, 5, 1 } , { 2, 6, 7, 3 } - , { 5, 1, 0, 4 } }; for (int i = 0; i < NUM_FACES; ++i) diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index 818ed62a..54fc531c 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -52,18 +52,15 @@ namespace SHADE , .normal = manifold.normal }; - const auto* SHAPE_A = manifold.shapeA; const auto* SHAPE_B = manifold.shapeB; - - const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; for (uint32_t i = 0; i < manifold.numContacts; ++i) { - const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA; + const SHVec3 WORLD_POINT_A = TF_B * manifold.contacts[i].localPointA; const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB; - collisionEvent.contactPoints[i] = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f); + collisionEvent.contactPoints[i] = WORLD_POINT_A; } collisionEvents.emplace_back(collisionEvent); @@ -92,12 +89,12 @@ namespace SHADE for (uint32_t i = 0; i < manifold.numContacts; ++i) { // Contact position will be the middle of worldPointA & worldPointB - const SHVec3 WORLD_POINT_A = TF_A * manifold.contacts[i].localPointA; + const SHVec3 WORLD_POINT_A = TF_B * manifold.contacts[i].localPointA; const SHVec3 WORLD_POINT_B = TF_B * manifold.contacts[i].localPointB; const ContactInfo INFO { - .position = SHVec3::ClampedLerp(WORLD_POINT_A, WORLD_POINT_B, 0.5f) + .position = WORLD_POINT_A , .normal = manifold.normal }; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index ff4212b6..e0f45826 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -100,8 +100,7 @@ namespace SHADE // Collision data newConstraint.numContacts = manifold.numContacts; - - newConstraint.normal = manifold.normal; + newConstraint.normal = manifold.normal; static constexpr size_t TANGENTS_SIZE = sizeof(SHVec3) * SHContact::NUM_TANGENTS; static constexpr size_t CONTACTS_SIZE = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS; @@ -109,14 +108,14 @@ namespace SHADE memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE); memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE); - const SHCollisionUtils::ShapeTransform TF_A = { SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; - const SHCollisionUtils::ShapeTransform TF_B = { SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; + // Compute rA & rB for contacts with the local centroids + const SHVec3 LOCAL_COM_A = BODY_A ? BODY_A->localCentroid : SHVec3::Zero; + const SHVec3 LOCAL_COM_B = BODY_B ? BODY_B->localCentroid : SHVec3::Zero; - // Compute rA & rB for contacts for (uint32_t i = 0; i < newConstraint.numContacts; ++i) { - newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA; - newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB; + newConstraint.contacts[i].rA = newConstraint.contacts[i].localPointA - LOCAL_COM_A; + newConstraint.contacts[i].rB = newConstraint.contacts[i].localPointB - LOCAL_COM_B; } } diff --git a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp index f49a9c8a..efb412f7 100644 --- a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp +++ b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsDebugDrawRoutine.cpp @@ -80,7 +80,7 @@ namespace SHADE for (auto& contactPoint : CONTACT_POINTS) { const SHMatrix TRS = SHMatrix::Transform(contactPoint.position, SHQuaternion::Identity, SHVec3{ 0.1f }); - debugDrawSystem->DrawSphere(TRS, CONTACT_COLOUR); + debugDrawSystem->DrawWireSphere(TRS, CONTACT_COLOUR); debugDrawSystem->DrawLine(contactPoint.position, contactPoint.position + contactPoint.normal * 0.5f, CONTACT_COLOUR, true); } }