public static Vector2 GetVelocity(SteeringAgent2D agent, float feelerLength)
{
var feelers = CreateFeelers(agent, feelerLength);
Vector2 steeringForce = Vector2.zero;
foreach (var feeler in feelers)
{
int currentWallIndex = 0;
int closestWallIndex = -1;
float distToThisIP = 0f;
float distToClosestIP = float.MaxValue;
Vector2 point = Vector2.zero;
Vector2 closestIP = Vector2.zero;
Vector2 feelerSteeringForce = Vector2.zero;
foreach (var wall in World2D.Instance.Walls)
{
try {
Vector2 feelerFrom = agent.transform.position;
Vector2 feelerTo = feeler;
//if (MathfExtensions.SegmentIntersection2D(feelerFrom, feelerTo, wall.From, wall.To, out distToThisIP, out point)) {
if (MathfExtensions.LineSegementsIntersect(feelerFrom, feelerTo, wall.From, wall.To, out point))
{
distToThisIP = (feelerFrom - point).magnitude;
if (distToThisIP < distToClosestIP)
{
distToClosestIP = distToThisIP;
closestWallIndex = currentWallIndex;
closestIP = point;
}
}
currentWallIndex++;
} catch {
//Do nothing
}
} //Next wall
if (closestWallIndex >= 0)
{
Vector2 overShoot = (feeler - closestIP) * agent.Rigidbody2D.velocity.magnitude;
var closestWall = World2D.Instance.Walls[closestWallIndex];
var feelerDirection = feeler - agent.Rigidbody2D.position;
if (Vector2.Angle(feelerDirection, closestWall.Normal) > 90)
{
feelerSteeringForce = closestWall.Normal * overShoot.magnitude;
}
else
{
feelerSteeringForce = closestWall.InverseNormal * overShoot.magnitude;
}
}
steeringForce += feelerSteeringForce;
} //Next feeler
return(steeringForce);
}