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 IRectangularF ToRect(this ILocationF loc, float w, float h)
{
var left = loc.Left - w / 2;
var top = loc.Top - h / 2;
return(RectangularF.Create(left, top, w, h));
}
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 CalculateAngleTo(this ILocationF start, ILocationF end)
{
float dx = end.Left - start.Left;
float dy = end.Top - start.Top;
float d = CalculateDistanceTo(start, end);
if (dy == 0 && dx > 0)
{
return(0);
}
else if (dy == 0)
{
return(180);
}
else if (dx == 0 && dy > 0)
{
return(90);
}
else if (dx == 0)
{
return(270);
}
double radians, increment;
if (dx >= 0 && dy >= 0)
{
// Sin(a) = dy / d
radians = Math.Asin(dy / d);
increment = 0;
}
else if (dx < 0 && dy > 0)
{
// Sin(a) = dx / d
radians = Math.Asin(-dx / d);
increment = 90;
}
else if (dy < 0 && dx < 0)
{
radians = Math.Asin(-dy / d);
increment = 180;
}
else if (dx > 0 && dy < 0)
{
radians = Math.Asin(dx / d);
increment = 270;
}
else
{
throw new Exception($"Failed to calculate angle from {start?.Left},{start?.Top} to {end?.Left},{end?.Top}");
}
var ret = (float)(increment + radians * 180 / Math.PI);
if (ret == 360)
{
ret = 0;
}
return(ret);
}
public static float CalculateDistanceTo(this ILocationF start, ILocationF end) => CalculateDistanceTo(start.Left, start.Top, end.Left, end.Top);
public static float CalculateNormalizedDistanceTo(ILocationF a, ILocationF b) => NormalizeQuantity(a.CalculateDistanceTo(b), a.CalculateAngleTo(b), true);
public static IRectangularF Create(ILocationF location, ISizeF size) => new RectangularF(location.Left, location.Top, size.Width, size.Height);
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.MovingObjectPosition = options.MovingObject.CopyBounds();
prediction.Visibility = options.Visibility;
if (options.Visibility == 0)
{
prediction.Direction = Direction.None;
prediction.Type = HitType.None;
return(prediction);
}
var effectiveObstacles = options.Obstacles.Where(o => o.CalculateDistanceTo(options.MovingObject) <= options.Visibility + options.Precision).ToList();
var endPoint = options.MovingObject.MoveTowards(options.Angle, options.Visibility);
ILocationF lkg = null;
for (var dPrime = options.Precision; dPrime < options.Visibility; dPrime += options.Precision)
{
var testArea = options.MovingObject.MoveTowards(options.Angle, dPrime);
prediction.Path.Add(testArea);
var obstacleHit = effectiveObstacles.Where(o =>
{
var simpleTest = o.Touches(testArea) == true;
if (simpleTest == false)
{
return(false);
}
if (o.Touches(options.MovingObject))
{
var overlapBefore = options.MovingObject.NumberOfPixelsThatOverlap(o);
var overlapAfter = testArea.NumberOfPixelsThatOverlap(o);
if (overlapAfter < overlapBefore)
{
return(false);
}
else
{
return(true);
}
}
else
{
return(true);
}
}).FirstOrDefault();
if (obstacleHit != null)
{
prediction.Type = HitType.Obstacle;
prediction.ObstacleHit = obstacleHit;
prediction.LKG = lkg;
return(prediction);
}
else
{
lkg = testArea.TopLeft();
}
}
var obstacleHitFinal = effectiveObstacles.Where(o => o.Touches(endPoint) == true).FirstOrDefault();
if (obstacleHitFinal != null)
{
prediction.Type = HitType.Obstacle;
prediction.ObstacleHit = obstacleHitFinal;
prediction.LKG = lkg;
return(prediction);
}
prediction.Type = HitType.None;
return(prediction);
}
public static float CalculateDistanceTo(this ILocationF start, ILocationF end) => (float)Math.Sqrt(((start.Left - end.Left) * (start.Left - end.Left)) + ((start.Top - end.Top) * (start.Top - end.Top)));
