public static double Trace(AABB aabb, Vector2f move, AABB[] obstacles, out Vector2f normal)
{
normal = Vector2f.Zero;
var tMin = double.PositiveInfinity;
// no movement, nothing to trace
if (move.X.Eq(0) && move.Y.Eq(0))
{
return(tMin);
}
var movedAabb = aabb + move;
if (move.X.Eq(0) || move.Y.Eq(0))
{
var path = new AABB(
new Vector2f(Math.Min(aabb.Right, movedAabb.Left), Math.Min(aabb.Top, movedAabb.Bottom)),
new Vector2f(Math.Max(aabb.Left, movedAabb.Right), Math.Max(aabb.Bottom, movedAabb.Top)));
var obstaclesInPath = obstacles.Where(o => o.Overlap(path)).ToArray();
if (obstaclesInPath.Length != 0)
{
normal = move.Inv().Normalize();
// find closest obstacle in path
if (move.X > 0)
{
tMin = Math.Abs(obstaclesInPath.Min(o => o.Left) - aabb.Right);
}
else if (move.X < 0)
{
tMin = Math.Abs(obstaclesInPath.Max(o => o.Right) - aabb.Left);
}
else if (move.Y > 0)
{
tMin = Math.Abs(obstaclesInPath.Min(o => o.Bottom) - aabb.Top);
}
else if (move.Y < 0)
{
tMin = Math.Abs(obstaclesInPath.Max(o => o.Top) - aabb.Bottom);
}
}
}
else
{
var v1 = aabb.Min;
var v2 = aabb.Max;
var v3 = movedAabb.Min;
var v4 = movedAabb.Max;
if (move.X * move.Y > 0)
{
v1 = new Vector2f(aabb.Right, aabb.Bottom);
v2 = new Vector2f(aabb.Left, aabb.Top);
v3 = new Vector2f(movedAabb.Right, movedAabb.Bottom);
v4 = new Vector2f(movedAabb.Left, movedAabb.Top);
}
var t1 = new Triangle(v1, v3, v4);
var t2 = new Triangle(v1, v2, v4);
var obstaclesInPath = obstacles.Where(o => movedAabb.Overlap(o) || t1.Overlap(o) || t2.Overlap(o))
.ToArray();
if (obstaclesInPath.Length != 0)
{
tMin = double.NegativeInfinity;
if (move.X > 0)
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Left.GrEq(aabb.Right))
.Select(o => (o.Left - aabb.Right) / (movedAabb.Right - aabb.Right)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Left;
}
}
else
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Right.LeEq(aabb.Left))
.Select(o => (o.Right - aabb.Left) / (movedAabb.Right - aabb.Right)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Right;
}
}
if (move.Y > 0)
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Bottom.GrEq(aabb.Top))
.Select(o => (o.Bottom - aabb.Top) / (movedAabb.Top - aabb.Top)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Down;
}
}
else
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Top.LeEq(aabb.Bottom))
.Select(o => (o.Top - aabb.Bottom) / (movedAabb.Top - aabb.Top)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Up;
}
}
if (tMin < 0)
{
// this should never happen
tMin = double.PositiveInfinity;
}
else
{
tMin *= move.Length();
}
}
}
return(tMin);
}