public static ILocationF MoveTowards(this ILocationF a, float angle, float distance)
{
while (angle < 0)
{
angle += 360;
}
while (angle > 360)
{
angle -= 360;
}
distance = Geometry.NormalizeQuantity(distance, angle);
var forward = angle > 270 || angle < 90;
var up = angle > 180;
// convert to radians
angle = (float)(angle * Math.PI / 180);
float dy = (float)Math.Abs(distance * Math.Sin(angle));
float dx = (float)Math.Sqrt((distance * distance) - (dy * dy));
float x2 = forward ? a.Left + dx : a.Left - dx;
float y2 = up ? a.Top - dy : a.Top + dy;
var ret = LocationF.Create(x2, y2);
return(ret);
}
public static ILocationF MoveTowards(this ILocationF a, ILocationF b, float distance)
{
float slope = (a.Top - b.Top) / (a.Left - b.Left);
bool forward = a.Left <= b.Left;
bool up = a.Top <= b.Top;
float abDistance = Geometry.CalculateNormalizedDistanceTo(a, b);
double angle = Math.Asin(Math.Abs(b.Top - a.Top) / abDistance);
float dy = (float)Math.Abs(distance * Math.Sin(angle));
float dx = (float)Math.Sqrt((distance * distance) - (dy * dy));
float x2 = forward ? a.Left + dx : a.Left - dx;
float y2 = up ? a.Top + dy : a.Top - dy;
var ret = LocationF.Create(x2, y2);
return(ret);
}
public static float CalculateDistanceTo(this IRectangularF a, IRectangularF b)
{
var left = b.Right() < a.Left;
var right = a.Right() < b.Left;
var bottom = b.Bottom() < a.Top;
var top = a.Bottom() < b.Top;
if (top && left)
{
return(LocationF.Create(a.Left, a.Bottom()).CalculateDistanceTo(LocationF.Create(b.Right(), b.Top)));
}
else if (left && bottom)
{
return(LocationF.Create(a.Left, a.Top).CalculateDistanceTo(LocationF.Create(b.Right(), b.Bottom())));
}
else if (bottom && right)
{
return(LocationF.Create(a.Right(), a.Top).CalculateDistanceTo(LocationF.Create(b.Left, b.Bottom())));
}
else if (right && top)
{
return(LocationF.Create(a.Right(), a.Bottom()).CalculateDistanceTo(LocationF.Create(b.Left, b.Top)));
}
else if (left)
{
return(a.Left - b.Right());
}
else if (right)
{
return(b.Left - a.Right());
}
else if (bottom)
{
return(a.Top - b.Bottom());
}
else if (top)
{
return(b.Top - a.Bottom());
}
else
{
return(0);
}
}
private static ILocationF FindIntersectionPoint(Edge a, Edge b)
{
var x1 = a.From.Left;
var y1 = a.From.Top;
var x2 = a.To.Left;
var y2 = a.To.Top;
var x3 = b.From.Left;
var y3 = b.From.Top;
var x4 = b.To.Left;
var y4 = b.To.Top;
var den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
if (den == 0)
{
return(null);
}
var t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den;
if (t <= 0 || t >= 1)
{
return(null);
}
var u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den;
if (u > 0 && u < 1)
{
return(LocationF.Create(x1 + t * (x2 - x1), y1 + t * (y2 - y1)));
}
else
{
return(null);
}
}
public static ILocationF BottomRight(this IRectangularF rectangular) => LocationF.Create(rectangular.Right(), rectangular.Bottom());
public static ILocationF BottomLeft(this IRectangularF rectangular) => LocationF.Create(rectangular.Left, rectangular.Bottom());
public static ILocationF TopRight(this IRectangularF rectangular) => LocationF.Create(rectangular.Right(), rectangular.Top);
public static ILocationF TopLeft(this IRectangularF rectangular) => LocationF.Create(rectangular.Left, rectangular.Top);
public static ILocationF Center(this IRectangularF rectangular) => LocationF.Create(rectangular.CenterX(), rectangular.CenterY());
public static ILocationF GetOffsetByPixels(this ILocationF r, float dx, float dy)
{
return(LocationF.Create(r.Left + dx, r.Top + dy));
}
public static HitPrediction PredictHit(HitDetectionOptions options)
{
HitPrediction prediction = new HitPrediction();
prediction.LKG = options.MovingObject.CopyBounds().TopLeft();
prediction.MovingObjectPosition = options.MovingObject.CopyBounds();
prediction.Visibility = options.Visibility;
if (options.Visibility == 0)
{
prediction.Direction = Direction.None;
prediction.Type = HitType.None;
return(prediction);
}
var mov = options.MovingObject;
List <Edge> rays;
if (options.Mode == CastingMode.Precise)
{
rays = new List <Edge>()
{
new Edge()
{
From = mov.TopLeft(), To = mov.TopLeft().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.TopRight(), To = mov.TopRight().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.BottomLeft(), To = mov.BottomLeft().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.BottomRight(), To = mov.BottomRight().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
};
var granularity = .5f;
for (var x = mov.Left + granularity; x < mov.Left + mov.Width; x += granularity)
{
var top = LocationF.Create(x, mov.Top);
var bot = LocationF.Create(x, mov.Bottom());
rays.Add(new Edge()
{
From = top, To = top.MoveTowards(options.Angle, options.Visibility, normalized: false)
});
rays.Add(new Edge()
{
From = bot, To = bot.MoveTowards(options.Angle, options.Visibility, normalized: false)
});
}
for (var y = mov.Top + granularity; y < mov.Top + mov.Height; y += granularity)
{
var left = LocationF.Create(mov.Left, y);
var right = LocationF.Create(mov.Right(), y);
rays.Add(new Edge()
{
From = left, To = left.MoveTowards(options.Angle, options.Visibility, normalized: false)
});
rays.Add(new Edge()
{
From = right, To = right.MoveTowards(options.Angle, options.Visibility, normalized: false)
});
}
}
else if (options.Mode == CastingMode.Rough)
{
var center = options.MovingObject.Center();
rays = new List <Edge>()
{
new Edge()
{
From = mov.TopLeft(), To = mov.TopLeft().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.TopRight(), To = mov.TopRight().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.BottomLeft(), To = mov.BottomLeft().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = mov.BottomRight(), To = mov.BottomRight().MoveTowards(options.Angle, options.Visibility, normalized: false)
},
new Edge()
{
From = center, To = center.MoveTowards(options.Angle, options.Visibility, normalized: false)
}
};
}
else if (options.Mode == CastingMode.SingleRay)
{
var center = options.MovingObject.Center();
rays = new List <Edge>()
{
new Edge()
{
From = center, To = center.MoveTowards(options.Angle, options.Visibility, normalized: false)
}
};
}
else
{
throw new NotSupportedException("Unknown mide: " + options.Mode);
}
var closestIntersectionDistance = float.MaxValue;
IRectangularF closestIntersectingElement = null;
var closestEdgeIndex = -1;
var effectiveObstacles = options.Obstacles.ToArray();
for (var i = 0; i < effectiveObstacles.Length; i++)
{
var obstacle = effectiveObstacles[i];
for (var j = 0; j < obstacle.Edges.Length; j++)
{
var edge = obstacle.Edges[j];
for (var k = 0; k < rays.Count; k++)
{
var ray = rays[k];
var intersection = FindIntersectionPoint(ray, edge);
if (intersection != null)
{
var d = ray.From.CalculateDistanceTo(intersection);
if (d < closestIntersectionDistance && d <= options.Visibility)
{
closestIntersectionDistance = d;
closestIntersectingElement = obstacle;
closestEdgeIndex = j;
}
}
}
}
}
if (closestIntersectingElement != null)
{
prediction.ObstacleHit = closestIntersectingElement;
prediction.LKGD = closestIntersectionDistance - .1f;
prediction.LKG = options.MovingObject.MoveTowards(options.Angle, prediction.LKGD, normalized: false).TopLeft();
prediction.Type = HitType.Obstacle;
prediction.EdgeIndex = closestEdgeIndex;
}
return(prediction);
}
