Circle LineSegment Collision C++ (UE4)

Question

Hello I am trying to compute Circle - LineSegment collision (in 2D coordinate space).

I want to detect collision when my agent is moving so I am sub stepping its movement -> agentPosition is where the agent now and agentPosition + agentDelta is where it wants to be in this step move.

The line is defined FVertex (Yes Bad name I know) testEdge (it has 2 points A and B).

The circle is defined as (agentPosition + agentDelta) as a center and agentRadius as it's radius.

I am trying to find a collision point (which would be an average of 2 possible intersection points) and compute the needed parameters:

Time to collision ({0.0f, 1.0f}) CollisionPoint (where the position of the circle at collision) ImpactPoint (the averagePoint of collision)

Here is what I have tried, but I have no luck with this :( I am getting negative or greater than 1.0f time, also sometimes the collisionPoint is on the other side of the line segment and other interesting artifacts.

I would appreciate if you could help me to find what am I doing wrong here.

static bool CheckAgentEdgeCollision(FVertex testEdge, FVector agentPosition, FVector agentDelta, float agentRadius, FCollisionResult2Dplease& outCollisionResult, UWorld* world = nullptr)
{
    agentPosition.Z = 0.0f;
    agentDelta.Z = 0.0f;
    testEdge.B.Z = 0.0f; testEdge.A.Z = 0.0f;

    FVector D = testEdge.B - testEdge.A;
    FVector d = testEdge.A - (agentPosition + agentDelta);

    float a = D | D;            // Operator | is Dot Product
    float b = (d | D) * 2.0f;   
    float c = (d | d) - FMath::Square(agentRadius);

    float disc = b * b - 4.0f * a * c;
    if (disc < KINDA_SMALL_NUMBER) 
    {
        return false;
    }

    float sqrtDisc = FastSQRoot(disc);

    float invA = 1.0f / ( a * 2.0f );

    float t0 = (-b - sqrtDisc) * invA;
    float t1 = (-b + sqrtDisc) * invA;

    FVector poin1 = FVector::ZeroVector;
    FVector poin2 = FVector::ZeroVector;

    poin1 = testEdge.A + t0 * D;
    poin2 = testEdge.A + t1 * D;

    bool p1 = true;
    bool p2 = true;

    if(t0 > 1.0f || t0 < 0.0f)
    {
        //disregard
        p1 = false;
    }

    if (t1 > 1.0f || t1 < 0.0f)
    {
        p2 = false;
    }

    if(!p1 && !p2)
    {
        return false;
    }
    else if(!p1)
    {
        poin1 = poin2;
    }
    else if (!p2)
    {
        poin2 = poin1;
    }

    float invRadius = 1.0f / agentRadius;

    agentRadius += 5.0f; 

    //Average the points:
    FVector impactPoint = (poin1 + poin2) / 2.0f;


    FVector directionToCircle = agentPosition - impactPoint;  
    FastNormalize(directionToCircle);

    FVector collisionPoint = directionToCircle * agentRadius + impactPoint;

    float distToCollision = FastSQRoot(agentPosition.DistSquared2D(agentPosition, collisionPoint));
    float speed = FastSQRoot(agentDelta.SizeSquared2D());

    float outTime = 0.0f;
    if (speed != 0.0f)
    {
        outTime = distToCollision / speed;
    }

    outCollisionResult.m_bIsPawn = false;
    outCollisionResult.m_edge = testEdge;
    outCollisionResult.m_normal = directionToCircle;
    outCollisionResult.m_collisionPoint = collisionPoint;
    outCollisionResult.m_time = outTime;
    outCollisionResult.m_bHit = true;
    outCollisionResult.m_impactPoint = impactPoint;

    outCollisionResult.m_binPenetration = outTime < 0.0f;
    return true;
}

Answer 1

Hint:

If the segment is fixed, you can recast the problem by "deflating" the circle while inflating the segment, which gives you a moving point vs. a "capsule". As you see, the collision occurs at the intersection point between the trajectory of the center and the outline of the capsule, which can occur along a straight or a circular edge.

The computation will be made easier by rotating the scene so that the segment comes to the X axis.