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