diff options
Diffstat (limited to 'Client/ThirdParty/Box2D/src/collision/b2_collide_edge.cpp')
| -rw-r--r-- | Client/ThirdParty/Box2D/src/collision/b2_collide_edge.cpp | 524 |
1 files changed, 524 insertions, 0 deletions
diff --git a/Client/ThirdParty/Box2D/src/collision/b2_collide_edge.cpp b/Client/ThirdParty/Box2D/src/collision/b2_collide_edge.cpp new file mode 100644 index 0000000..e06b900 --- /dev/null +++ b/Client/ThirdParty/Box2D/src/collision/b2_collide_edge.cpp @@ -0,0 +1,524 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_collision.h" +#include "box2d/b2_circle_shape.h" +#include "box2d/b2_edge_shape.h" +#include "box2d/b2_polygon_shape.h" + + +// Compute contact points for edge versus circle. +// This accounts for edge connectivity. +void b2CollideEdgeAndCircle(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2CircleShape* circleB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + // Compute circle in frame of edge + b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); + + b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; + b2Vec2 e = B - A; + + // Normal points to the right for a CCW winding + b2Vec2 n(e.y, -e.x); + float offset = b2Dot(n, Q - A); + + bool oneSided = edgeA->m_oneSided; + if (oneSided && offset < 0.0f) + { + return; + } + + // Barycentric coordinates + float u = b2Dot(e, B - Q); + float v = b2Dot(e, Q - A); + + float radius = edgeA->m_radius + circleB->m_radius; + + b2ContactFeature cf; + cf.indexB = 0; + cf.typeB = b2ContactFeature::e_vertex; + + // Region A + if (v <= 0.0f) + { + b2Vec2 P = A; + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to A? + if (edgeA->m_oneSided) + { + b2Vec2 A1 = edgeA->m_vertex0; + b2Vec2 B1 = A; + b2Vec2 e1 = B1 - A1; + float u1 = b2Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) + { + return; + } + } + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region B + if (u <= 0.0f) + { + b2Vec2 P = B; + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + // Is there an edge connected to B? + if (edgeA->m_oneSided) + { + b2Vec2 B2 = edgeA->m_vertex3; + b2Vec2 A2 = B; + b2Vec2 e2 = B2 - A2; + float v2 = b2Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) + { + return; + } + } + + cf.indexA = 1; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region AB + float den = b2Dot(e, e); + b2Assert(den > 0.0f); + b2Vec2 P = (1.0f / den) * (u * A + v * B); + b2Vec2 d = Q - P; + float dd = b2Dot(d, d); + if (dd > radius * radius) + { + return; + } + + if (offset < 0.0f) + { + n.Set(-n.x, -n.y); + } + n.Normalize(); + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_face; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = n; + manifold->localPoint = A; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; +} + +// This structure is used to keep track of the best separating axis. +struct b2EPAxis +{ + enum Type + { + e_unknown, + e_edgeA, + e_edgeB + }; + + b2Vec2 normal; + Type type; + int32 index; + float separation; +}; + +// This holds polygon B expressed in frame A. +struct b2TempPolygon +{ + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + int32 count; +}; + +// Reference face used for clipping +struct b2ReferenceFace +{ + int32 i1, i2; + b2Vec2 v1, v2; + b2Vec2 normal; + + b2Vec2 sideNormal1; + float sideOffset1; + + b2Vec2 sideNormal2; + float sideOffset2; +}; + +static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1) +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_edgeA; + axis.index = -1; + axis.separation = -FLT_MAX; + axis.normal.SetZero(); + + b2Vec2 axes[2] = { normal1, -normal1 }; + + // Find axis with least overlap (min-max problem) + for (int32 j = 0; j < 2; ++j) + { + float sj = FLT_MAX; + + // Find deepest polygon vertex along axis j + for (int32 i = 0; i < polygonB.count; ++i) + { + float si = b2Dot(axes[j], polygonB.vertices[i] - v1); + if (si < sj) + { + sj = si; + } + } + + if (sj > axis.separation) + { + axis.index = j; + axis.separation = sj; + axis.normal = axes[j]; + } + } + + return axis; +} + +static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2) +{ + b2EPAxis axis; + axis.type = b2EPAxis::e_unknown; + axis.index = -1; + axis.separation = -FLT_MAX; + axis.normal.SetZero(); + + for (int32 i = 0; i < polygonB.count; ++i) + { + b2Vec2 n = -polygonB.normals[i]; + + float s1 = b2Dot(n, polygonB.vertices[i] - v1); + float s2 = b2Dot(n, polygonB.vertices[i] - v2); + float s = b2Min(s1, s2); + + if (s > axis.separation) + { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + axis.normal = n; + } + } + + return axis; +} + +void b2CollideEdgeAndPolygon(b2Manifold* manifold, + const b2EdgeShape* edgeA, const b2Transform& xfA, + const b2PolygonShape* polygonB, const b2Transform& xfB) +{ + manifold->pointCount = 0; + + b2Transform xf = b2MulT(xfA, xfB); + + b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid); + + b2Vec2 v1 = edgeA->m_vertex1; + b2Vec2 v2 = edgeA->m_vertex2; + + b2Vec2 edge1 = v2 - v1; + edge1.Normalize(); + + // Normal points to the right for a CCW winding + b2Vec2 normal1(edge1.y, -edge1.x); + float offset1 = b2Dot(normal1, centroidB - v1); + + bool oneSided = edgeA->m_oneSided; + if (oneSided && offset1 < 0.0f) + { + return; + } + + // Get polygonB in frameA + b2TempPolygon tempPolygonB; + tempPolygonB.count = polygonB->m_count; + for (int32 i = 0; i < polygonB->m_count; ++i) + { + tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]); + tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]); + } + + float radius = polygonB->m_radius + edgeA->m_radius; + + b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1); + if (edgeAxis.separation > radius) + { + return; + } + + b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2); + if (polygonAxis.separation > radius) + { + return; + } + + // Use hysteresis for jitter reduction. + const float k_relativeTol = 0.98f; + const float k_absoluteTol = 0.001f; + + b2EPAxis primaryAxis; + if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol) + { + primaryAxis = polygonAxis; + } + else + { + primaryAxis = edgeAxis; + } + + if (oneSided) + { + // Smooth collision + // See https://box2d.org/posts/2020/06/ghost-collisions/ + + b2Vec2 edge0 = v1 - edgeA->m_vertex0; + edge0.Normalize(); + b2Vec2 normal0(edge0.y, -edge0.x); + bool convex1 = b2Cross(edge0, edge1) >= 0.0f; + + b2Vec2 edge2 = edgeA->m_vertex3 - v2; + edge2.Normalize(); + b2Vec2 normal2(edge2.y, -edge2.x); + bool convex2 = b2Cross(edge1, edge2) >= 0.0f; + + const float sinTol = 0.1f; + bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f; + + // Check Gauss Map + if (side1) + { + if (convex1) + { + if (b2Cross(primaryAxis.normal, normal0) > sinTol) + { + // Skip region + return; + } + + // Admit region + } + else + { + // Snap region + primaryAxis = edgeAxis; + } + } + else + { + if (convex2) + { + if (b2Cross(normal2, primaryAxis.normal) > sinTol) + { + // Skip region + return; + } + + // Admit region + } + else + { + // Snap region + primaryAxis = edgeAxis; + } + } + } + + b2ClipVertex clipPoints[2]; + b2ReferenceFace ref; + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->type = b2Manifold::e_faceA; + + // Search for the polygon normal that is most anti-parallel to the edge normal. + int32 bestIndex = 0; + float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]); + for (int32 i = 1; i < tempPolygonB.count; ++i) + { + float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]); + if (value < bestValue) + { + bestValue = value; + bestIndex = i; + } + } + + int32 i1 = bestIndex; + int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0; + + clipPoints[0].v = tempPolygonB.vertices[i1]; + clipPoints[0].id.cf.indexA = 0; + clipPoints[0].id.cf.indexB = static_cast<uint8>(i1); + clipPoints[0].id.cf.typeA = b2ContactFeature::e_face; + clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex; + + clipPoints[1].v = tempPolygonB.vertices[i2]; + clipPoints[1].id.cf.indexA = 0; + clipPoints[1].id.cf.indexB = static_cast<uint8>(i2); + clipPoints[1].id.cf.typeA = b2ContactFeature::e_face; + clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex; + + ref.i1 = 0; + ref.i2 = 1; + ref.v1 = v1; + ref.v2 = v2; + ref.normal = primaryAxis.normal; + ref.sideNormal1 = -edge1; + ref.sideNormal2 = edge1; + } + else + { + manifold->type = b2Manifold::e_faceB; + + clipPoints[0].v = v2; + clipPoints[0].id.cf.indexA = 1; + clipPoints[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index); + clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex; + clipPoints[0].id.cf.typeB = b2ContactFeature::e_face; + + clipPoints[1].v = v1; + clipPoints[1].id.cf.indexA = 0; + clipPoints[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index); + clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex; + clipPoints[1].id.cf.typeB = b2ContactFeature::e_face; + + ref.i1 = primaryAxis.index; + ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0; + ref.v1 = tempPolygonB.vertices[ref.i1]; + ref.v2 = tempPolygonB.vertices[ref.i2]; + ref.normal = tempPolygonB.normals[ref.i1]; + + // CCW winding + ref.sideNormal1.Set(ref.normal.y, -ref.normal.x); + ref.sideNormal2 = -ref.sideNormal1; + } + + ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1); + ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2); + + // Clip incident edge against reference face side planes + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int32 np; + + // Clip to side 1 + np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Clip to side 2 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2); + + if (np < b2_maxManifoldPoints) + { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + manifold->localNormal = ref.normal; + manifold->localPoint = ref.v1; + } + else + { + manifold->localNormal = polygonB->m_normals[ref.i1]; + manifold->localPoint = polygonB->m_vertices[ref.i1]; + } + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) + { + float separation; + + separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1); + + if (separation <= radius) + { + b2ManifoldPoint* cp = manifold->points + pointCount; + + if (primaryAxis.type == b2EPAxis::e_edgeA) + { + cp->localPoint = b2MulT(xf, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + } + else + { + cp->localPoint = clipPoints2[i].v; + cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; + cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; + cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; + cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; + } + + ++pointCount; + } + } + + manifold->pointCount = pointCount; +} |