/// <summary>
/// Rotates towards the target, then moves to it, for one tick.
/// </summary>
/// <returns> true if the drone is at the target.</returns>
private bool MoveTo(Vector2 target, float speed)
{
Vector2 dir = (target - Position);
if (dir.LengthSquared() <= 4 * Game1.DeltaT * Game1.DeltaT * speed * speed)
{
return(true);
}
else
{
float targetDirection = (float)Math.Atan2(dir.Y, dir.X);
if (ConeEntity.ConvertAngle(targetDirection - Direction) <= 2 * Game1.DeltaT * TurnAngularVelocity || ConeEntity.ConvertAngle(Direction - targetDirection) <= 2 * Game1.DeltaT * TurnAngularVelocity)
{
Direction = targetDirection;
Velocity = dir;
Velocity.Normalize();
if (float.IsNaN(Velocity.X) || float.IsNaN(Velocity.Y))
{
Velocity = new Vector2(0);
return(true);
}
Velocity *= speed;
}
else
{
if (ConeEntity.ConvertAngle(targetDirection - Direction) < Math.PI)
{
Direction += Game1.DeltaT * TurnAngularVelocity;
}
else
{
Direction -= Game1.DeltaT * TurnAngularVelocity;
}
}
}
return(false);
}
//Dictionary is point to cw/ccw line
public SortedDictionary <float, Tuple <Vector2, Vector2> > BuildLOSHelper(RectangleF boundingBox, Vector2 pos, float radius, Vector2 dir1, Vector2 dir2, bool backgroundwalls = true)
{
float radiusSqr = radius * radius;
float Cross2(Vector2 x1, Vector2 x2) => x1.X * x2.Y - x1.Y * x2.X;
bool acute = Cross2(dir1, dir2) > 0;
float startingangle = ConeEntity.ConvertAngle((float)Math.Atan2(dir1.Y, dir1.X));
float endingangle = ConeEntity.ConvertAngle((float)Math.Atan2(dir2.Y, dir2.X));
float anglerange = ConeEntity.ConvertAngle(endingangle - startingangle);
float getAngle(Vector2 p)
{
return(ConeEntity.ConvertAngle((float)Math.Atan2(p.Y - pos.Y, p.X - pos.X) - startingangle));
}
bool inRange(Vector2 p)
{
Vector2 dirp = p - pos;
return(dirp.LengthSquared() <= radiusSqr);
}
bool inAngle(Vector2 p)
{
float angle = getAngle(p);
return(angle >= 0 && angle <= anglerange);
}
var points = new SortedDictionary <float, Tuple <Vector2, Vector2> >();
int minX = (int)Math.Max(Math.Floor((boundingBox.Left - BoundingBox.X) / TileSize), 0);
int minY = (int)Math.Max(Math.Floor((boundingBox.Bottom - BoundingBox.Y) / TileSize), 0);
int maxX = (int)Math.Min(Math.Floor((boundingBox.Right - BoundingBox.X) / TileSize) + 1, Width);
int maxY = (int)Math.Min(Math.Floor((boundingBox.Top - BoundingBox.Y) / TileSize) + 1, Height);
var offset = new Vector2(BoundingBox.X, BoundingBox.Y);
for (int x = minX; x < maxX; ++x)
{
for (int y = minY; y < maxY; ++y)
{
if (GetTile(x, y) == (uint)Colors.SolidPlatform || (backgroundwalls && GetTile(x, y) == (uint)Colors.BackgroundWall))
{
Vector2 tilePosition = offset + new Vector2(x + 0.5F, y + 0.5F) * TileSize;
var dir = pos - tilePosition;
var cornerOffset = new Vector2(dir.X > 0 ? 1 : -1, dir.Y > 0 ? 1 : -1);
Vector2 dircorner = dir - cornerOffset * TileSize / 2;
if (Math.Sign(dir.X) == Math.Sign(dircorner.X) &&
!(GetTile((int)cornerOffset.X + x, y) == (uint)Colors.SolidPlatform ||
(backgroundwalls && GetTile((int)cornerOffset.X + x, y) == (uint)Colors.BackgroundWall)))
{
Vector2 point1;
Vector2 point2;
if (cornerOffset.X == cornerOffset.Y)
{
point1 = tilePosition + cornerOffset * TileSize / 2;
point2 = tilePosition + new Vector2(cornerOffset.X, -cornerOffset.Y) * TileSize / 2;
}
else
{
point2 = tilePosition + cornerOffset * TileSize / 2;
point1 = tilePosition + new Vector2(cornerOffset.X, -cornerOffset.Y) * TileSize / 2;
}
float angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(point1.Y - pos.Y, point1.X - pos.X) - startingangle);
float angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(point2.Y - pos.Y, point2.X - pos.X) - startingangle);
if ((inAngle(point1) || inAngle(point2)) && (inRange(point1) || inRange(point2)))
{
if (!inAngle(point1))
{
angle1 = angle1 - 2 * (float)Math.PI;
}
if (!inRange(point1) || !inRange(point2))
{
if (ConeEntity.IntersectCircle(point1, point2, radius, pos, 1, out float t))
{
Vector2 newPoint = point1 + (point2 - point1) * t;
if (!inRange(point1))
{
point1 = newPoint;
angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
else
{
point2 = newPoint;
angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
}
}
if (points.TryGetValue(angle1, out var point1Entry) && point1Entry.Item1 != point1)
{
if ((point1 - pos).LengthSquared() < (point1Entry.Item2 - pos).LengthSquared())
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
}
else
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
if (points.TryGetValue(angle2, out var point2Entry))
{
if (point2Entry.Item1 != point2 && (point2 - pos).LengthSquared() < (point2Entry.Item2 - pos).LengthSquared())
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));;
}
}
else
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));
}
}
}
if (Math.Sign(dir.Y) == Math.Sign(dircorner.Y) &&
!(GetTile(x, (int)cornerOffset.Y + y) == (uint)Colors.SolidPlatform ||
(backgroundwalls && GetTile(x, (int)cornerOffset.Y + y) == (uint)Colors.BackgroundWall)))
{
Vector2 point1;
Vector2 point2;
if (cornerOffset.X == cornerOffset.Y)
{
point2 = tilePosition + cornerOffset * TileSize / 2;
point1 = tilePosition + new Vector2(-cornerOffset.X, cornerOffset.Y) * TileSize / 2;
}
else
{
point1 = tilePosition + cornerOffset * TileSize / 2;
point2 = tilePosition + new Vector2(-cornerOffset.X, cornerOffset.Y) * TileSize / 2;
}
float angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(point1.Y - pos.Y, point1.X - pos.X) - startingangle);
float angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(point2.Y - pos.Y, point2.X - pos.X) - startingangle);
if ((inAngle(point1) || inAngle(point2)) && (inRange(point1) || inRange(point2)))
{
if (!inAngle(point1))
{
angle1 = angle1 - 2 * (float)Math.PI;
}
if (!inRange(point1) || !inRange(point2))
{
if (ConeEntity.IntersectCircle(point1, point2, radius, pos, 1, out float t))
{
Vector2 newPoint = point1 + (point2 - point1) * t;
if (!inRange(point1))
{
point1 = newPoint;
angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
else
{
point2 = newPoint;
angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
}
}
if (points.TryGetValue(angle1, out var point1Entry) && point1Entry.Item1 != point1)
{
if ((point1 - pos).LengthSquared() < (point1Entry.Item2 - pos).LengthSquared())
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
}
else
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
if (points.TryGetValue(angle2, out var point2Entry))
{
if (point2Entry.Item1 != point2 && (point2 - pos).LengthSquared() < (point2Entry.Item2 - pos).LengthSquared())
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));;
}
}
else
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));
}
}
}
}
}
}
foreach (IOccludingEntity entity in OccludingEntities)
{
Vector2 entityPosition = entity.GetOcclusionBox().Center;
var dir = pos - entityPosition;
var cornerOffset = new Vector2(dir.X > 0 ? 1 : -1, dir.Y > 0 ? 1 : -1);
Vector2 entitySize = 0.5F * new Vector2(entity.GetOcclusionBox().Width, entity.GetOcclusionBox().Height);
Vector2 dircorner = dir - cornerOffset * entitySize;
if (Math.Sign(dir.X) == Math.Sign(dircorner.X))
{
Vector2 point1;
Vector2 point2;
if (cornerOffset.X == cornerOffset.Y)
{
point1 = entityPosition + cornerOffset * entitySize;
point2 = entityPosition + new Vector2(cornerOffset.X, -cornerOffset.Y) * entitySize;
}
else
{
point2 = entityPosition + cornerOffset * entitySize;
point1 = entityPosition + new Vector2(cornerOffset.X, -cornerOffset.Y) * entitySize;
}
float angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(point1.Y - pos.Y, point1.X - pos.X) - startingangle);
float angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(point2.Y - pos.Y, point2.X - pos.X) - startingangle);
if ((inAngle(point1) || inAngle(point2)) && (inRange(point1) || inRange(point2)))
{
if (!inAngle(point1))
{
angle1 = angle1 - 2 * (float)Math.PI;
}
if (!inRange(point1) || !inRange(point2))
{
if (ConeEntity.IntersectCircle(point1, point2, radius, pos, 1, out float t))
{
Vector2 newPoint = point1 + (point2 - point1) * t;
if (!inRange(point1))
{
point1 = newPoint;
angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
else
{
point2 = newPoint;
angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
}
}
if (points.TryGetValue(angle1, out var point1Entry) && point1Entry.Item1 != point1)
{
if ((point1 - pos).LengthSquared() < (point1Entry.Item2 - pos).LengthSquared())
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
}
else
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
if (points.TryGetValue(angle2, out var point2Entry))
{
if (point2Entry.Item1 != point2 && (point2 - pos).LengthSquared() < (point2Entry.Item2 - pos).LengthSquared())
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));;
}
}
else
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));
}
}
}
if (Math.Sign(dir.Y) == Math.Sign(dircorner.Y))
{
Vector2 point1;
Vector2 point2;
if (cornerOffset.X == cornerOffset.Y)
{
point2 = entityPosition + cornerOffset * entitySize;
point1 = entityPosition + new Vector2(-cornerOffset.X, cornerOffset.Y) * entitySize;
}
else
{
point1 = entityPosition + cornerOffset * entitySize;
point2 = entityPosition + new Vector2(-cornerOffset.X, cornerOffset.Y) * entitySize;
}
float angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(point1.Y - pos.Y, point1.X - pos.X) - startingangle);
float angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(point2.Y - pos.Y, point2.X - pos.X) - startingangle);
if ((inAngle(point1) || inAngle(point2)) && (inRange(point1) || inRange(point2)))
{
if (!inAngle(point1))
{
angle1 = angle1 - 2 * (float)Math.PI;
}
if (!inRange(point1) || !inRange(point2))
{
if (ConeEntity.IntersectCircle(point1, point2, radius, pos, 1, out float t))
{
Vector2 newPoint = point1 + (point2 - point1) * t;
if (!inRange(point1))
{
point1 = newPoint;
angle1 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
else
{
point2 = newPoint;
angle2 = ConeEntity.ConvertAngle((float)Math.Atan2(newPoint.Y - pos.Y, newPoint.X - pos.X) - startingangle);
}
}
}
if (points.TryGetValue(angle1, out var point1Entry) && point1Entry.Item1 != point1)
{
if ((point1 - pos).LengthSquared() < (point1Entry.Item2 - pos).LengthSquared())
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
}
else
{
points[angle1] = new Tuple <Vector2, Vector2>(point1, point2);
}
if (points.TryGetValue(angle2, out var point2Entry))
{
if (point2Entry.Item1 != point2 && (point2 - pos).LengthSquared() < (point2Entry.Item2 - pos).LengthSquared())
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));;
}
}
else
{
points[angle2] = new Tuple <Vector2, Vector2>(point2, new Vector2(float.NaN));
}
}
}
}
return(points);
}
