b2_collide_edge.cpp

Go to the documentation of this file.

// 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;
}