diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index d0985215..b405fac2 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -45,9 +45,9 @@ NumberOfChildren: 0 Components: Camera Component: - Position: {x: 1, y: 10, z: 3} + Position: {x: -0.5, y: 10, z: -3} Pitch: 0 - Yaw: 0 + Yaw: 180 Roll: 0 Width: 1920 Height: 1080 @@ -84,7 +84,7 @@ Freeze Rotation Z: false IsActive: true Collider Component: - DrawColliders: false + DrawColliders: true Colliders: - Is Trigger: false Collision Tag: 1 @@ -95,7 +95,7 @@ Density: 1 Position Offset: {x: 0, y: 0, z: 0} Rotation Offset: {x: 0, y: 0, z: 0} - - Is Trigger: false + - Is Trigger: true Collision Tag: 1 Type: Sphere Radius: 0.5 @@ -280,9 +280,9 @@ NumberOfChildren: 0 Components: Transform Component: - Translate: {x: 0.899999976, y: 10, z: 0} - Rotate: {x: -0, y: 0, z: 0.785398185} - Scale: {x: 1, y: 1, z: 1} + Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179} + Rotate: {x: -0, y: 0.785398066, z: 0.785398185} + Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881} IsActive: true RigidBody Component: Type: Dynamic @@ -295,7 +295,7 @@ Interpolate: true Sleeping Enabled: true Freeze Position X: false - Freeze Position Y: true + Freeze Position Y: false Freeze Position Z: false Freeze Rotation X: false Freeze Rotation Y: false @@ -304,7 +304,7 @@ Collider Component: DrawColliders: false Colliders: - - Is Trigger: true + - Is Trigger: false Collision Tag: 1 Type: Box Half Extents: {x: 1, y: 1, z: 1} @@ -322,13 +322,13 @@ Components: Transform Component: Translate: {x: -0.5, y: 10, z: 0} - Rotate: {x: -0, y: 0.785398185, z: -0} + Rotate: {x: -0, y: 0, z: -0} Scale: {x: 1, y: 1, z: 1} IsActive: true RigidBody Component: - Type: Dynamic + Type: Static Auto Mass: false - Mass: 1 + Mass: .inf Drag: 0.00999999978 Angular Drag: 0.00999999978 Use Gravity: true @@ -345,7 +345,7 @@ Collider Component: DrawColliders: false Colliders: - - Is Trigger: true + - Is Trigger: false Collision Tag: 1 Type: Box Half Extents: {x: 1, y: 1, z: 1} diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h index 8a465d4a..57777cca 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h @@ -101,12 +101,13 @@ namespace SHADE // Convex VS Convex - static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; - static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; + static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; + static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept; - 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 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; /* * TODO: diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp index 5eedc2c3..0f899443 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp @@ -17,6 +17,7 @@ #include "Math/SHMathHelpers.h" #include "Math/Geometry/SHPlane.h" #include "Physics/Collision/Shapes/SHConvexPolyhedron.h" +#include "Tools/Utilities/SHUtilities.h" namespace SHADE { @@ -65,6 +66,55 @@ namespace SHADE * */ + const SHConvexPolyhedron& POLY_A = dynamic_cast(A); + const SHConvexPolyhedron& POLY_B = dynamic_cast(B); + + const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B); + if (FACE_QUERY_A.bestDistance > 0.0f) + return false; + + const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A); + if (FACE_QUERY_B.bestDistance > 0.0f) + return false; + + const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B); + if (EDGE_QUERY.bestDistance > 0.0f) + return false; + + const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B; + + // 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. + if (EDGE_QUERY.bestDistance > BEST_FACE_QUERY.bestDistance) + { + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB); + + // 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.position = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal); + contact.penetration = EDGE_QUERY.bestDistance; + + manifold.contacts[0] = contact; + manifold.numContacts = 1; + + return true; + } + + // Use a bias to favour a normal in the direction of A -> B + + + return false; } @@ -187,4 +237,54 @@ 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 + { + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA); + const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB); + + 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 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) + */ + + 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; + + /* + * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 ) + * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1) + */ + + const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1); + + // Just take a point from A since it's A -> B + return TAIL_A + R * VA; + } + + } // namespace SHADE \ No newline at end of file