diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index b405fac2..0b732f64 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -280,7 +280,7 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179} + Translate: {x: 0.524352431, y: 11.5, z: 0.0463808179} Rotate: {x: -0, y: 0.785398066, z: 0.785398185} Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} IsActive: true diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 57777cca..0fe0b3f7 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -107,7 +107,7 @@ namespace SHADE static bool buildMinkowskiFace (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; static bool isMinkowskiFace (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept; static float distanceBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; - static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) noexcept; + static SHVec3 findClosestPointBetweenEdges (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept; /* * TODO: diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 0f899443..5924ee73 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -83,6 +83,8 @@ namespace SHADE const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B; + uint32_t numContacts = 0; + // 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. // Artificially increase the depth of this collision by a small tolerance to tend towards picking a face query in the next frame. @@ -91,22 +93,31 @@ namespace SHADE 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 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; + // In this scenario, we only have one contact - - SHContactFeatures featurePair; - featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); - featurePair.indexA = HALF_EDGE_A.tailVertexIndex; - featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); - featurePair.indexB = HALF_EDGE_B.tailVertexIndex; - SHContact contact; - contact.featurePair = featurePair; + contact.featurePair.typeA = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); + contact.featurePair.indexA = HALF_EDGE_A.tailVertexIndex; + contact.featurePair.typeB = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX); + contact.featurePair.indexB = HALF_EDGE_B.tailVertexIndex; - contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal); + contact.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB); contact.penetration = EDGE_QUERY.bestDistance; - manifold.contacts[0] = contact; - manifold.numContacts = 1; + manifold.contacts[numContacts++] = contact; + manifold.numContacts = numContacts; return true; } @@ -237,8 +248,22 @@ namespace SHADE return SHVec3::Dot(normal, HEAD_B - HEAD_A); } - SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) noexcept + SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept { + /* + * The two edges can be parameterised in the form p + tv + * + * LA(r) = A + rVA + * LB(s) = B + sVB + * + * The vector between the closest points is the cross product of VA and VB. + * Since the cross product is orthogonal to VA and VB, we can generalise this as: + * (LB(s) - LA(r)) /dot VA = 0 + * (LB(s) - LA(r)) /dot VB = 0 + * + * Where LB(s) - LA(r) is the same vector as VB X VA. + */ + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); @@ -250,37 +275,35 @@ namespace SHADE const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A); const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B); - normal = SHVec3::Cross(VB, VA); - // Flip normal if need to ( A -> B) - if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f) - normal = -normal; - - /* * Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation - * C' = TAIL_B - TAIL_A - * U = VA / VB * - * a = VBxUx + VByUy + VBzUz - * b = -VAxUx - VAyUy - VAzUz - * c = (Cx'Ux + Cy'Uy + Cz'Uz) + * C' = TAIL_B - TAIL_A + * + * a = VB /dot U + * b = -VA /dot U + * c = C' /dot U + * + * U is either VA or VB */ const SHVec3 C = TAIL_B - TAIL_A; - const float A1 = VB.x * VA.x + VB.y * VA.y + VB.z * VA.z; - const float A2 = VB.x * VB.x + VB.y * VB.y + VB.z * VB.z; - const float B1 = -VA.x * VA.x + -VA.y * VA.y + -VA.z * VA.z; - const float B2 = -VA.x * VB.x + -VA.y * VB.y + -VA.z * VB.z; - const float C1 = C.x * VA.x + C.y + VA.y + C.z + VA.z; - const float C2 = C.x * VB.x + C.y + VB.y + C.z + VB.z; + const float VB_DOT_VA = SHVec3::Dot(VB, VA); + const float VB_DOT_VB = SHVec3::Dot(VB, VB); + const float AV_DOT_VA = SHVec3::Dot(-VA, VA); + const float AV_DOT_VB = SHVec3::Dot(-VA, VB); + const float C_DOT_VA = SHVec3::Dot(C, VA); + const float C_DOT_VB = SHVec3::Dot(C, VB); /* * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 ) * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1) + * + * We only need to solve for R */ - const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1); + const float R = -C_DOT_VB / (AV_DOT_VB - (VB_DOT_VB * (C_DOT_VA + AV_DOT_VA)) / VB_DOT_VA); // Just take a point from A since it's A -> B return TAIL_A + R * VA;