/// <summary>
/// A function for adding an edge to the graph. A node will be created when it does not exist.
/// </summary>
/// <param name="source"></param>
/// <param name="destination"></param>
/// <param name="cost"></param>
public void AddEdge(Vector2D source, Vector2D destination, double cost = 1)
{
var v = GetNode(source, create: true);
var w = GetNode(destination, create: true);
v.Adjacent.Add(new Edge(w, cost));
}
public void Draw(Graphics g)
{
Pen p = new Pen(Color.Gray, .5f);
var grid = new List<Vector2D>();
var cellSize = height / (height / this.cellSize);
for (int i = 0; i < width / this.cellSize; i++)
{
var vHorizontal = new Vector2D(0, height);
var vVertical = new Vector2D(width, 0);
vHorizontal *= Matrix.Translate(0, this.cellSize * i);
vVertical *= Matrix.Translate(this.cellSize * i, 0);
grid.Add(vHorizontal);
grid.Add(vVertical);
}
var rowY = 0;
var rowX = 0;
foreach (Vector2D v in grid)
{
if (grid.IndexOf(v) % 2 == 0)
{
g.DrawLine(p, v.x, rowY, v.x + width, rowY);
rowY += cellSize;
}
else
{
g.DrawLine(p, rowX, v.y, rowX, v.y + height);
rowX += cellSize;
}
}
}
public Vector2D Calculate()
{
//Used to record the average heading of the neighbours
Vector2D AverageHeading = new Vector2D();
var neighbours = this.MovingEntity.GetNeighbours(true);
int neighbourCount = 0;
foreach(IMovingEntity neighbour in neighbours)
{
//Make sure *this* agent isn't included in the calculations and that
//the agent being examind is close enough
if(neighbour != this.MovingEntity)
{
AverageHeading += neighbour.GetHeading();
++neighbourCount;
}
}
//If the neighbourhood contained one or more vehicles, average their
//heading vectors.
if(neighbourCount > 0)
{
AverageHeading /= (float)neighbourCount;
AverageHeading -= this.MovingEntity.GetHeading();
}
return AverageHeading;
}
public Vector2D Calculate()
{
//First find the centre of mass of all the agents
Vector2D centerOfMass = new Vector2D();
Vector2D steeringForce = new Vector2D();
int NeighbourCount = 0;
//iterate through the neighbours and sum up all the position vectors
foreach (IMovingEntity neighbour in this.MovingEntity.GetNeighbours(true))
{
//Make sure *this* agent isn't included in the calculations and that
//the agent begint examined is a neigbour
if (neighbour != this.MovingEntity)
{
centerOfMass += neighbour.GetPosition();
++NeighbourCount;
}
}
if (NeighbourCount > 0)
{
//The center of mass is the average of the sum of positions
centerOfMass /= (float)NeighbourCount;
//now Seek toward that position
steeringForce = new Seek(this.MovingEntity, centerOfMass).Calculate();
}
return steeringForce;
}
public Vector2D Calculate()
{
var neighbours = MovingEntity.GetNeighbours(true);
Vector2D SteeringForce = new Vector2D();
foreach (IMovingEntity neighbour in neighbours)
{
//Make sure this agent ins't included in the calculations
//and that the agent being examind is close enough
if (neighbour != this.MovingEntity)
{
Vector2D ToAgent = this.MovingEntity.GetPosition() - neighbour.GetPosition();
//Scale the force inversely proportional to the agent's distance
//from its neighbour
var temp = ToAgent.Normalize() / ToAgent.GetLength();
//temp x and y are NaN if two entities are on the exact same location.
if (!(float.IsNaN(temp.x) || float.IsNaN(temp.y)))
SteeringForce += temp;
}
}
return SteeringForce;
}
public Vector2D Calculate()
{
Vector2D ToTarget = TargetPosition - this.MovingEntity.GetPosition();
//Calculate the distance to the target position.
double distance = ToTarget.GetLength();
if (distance > 0)
{
//Because the decleration is enumerated as an int, this value is
//required to provide fin tweaking of the decleration.
const double DeclerationTweaker = 220;
//Calculate the speed required to reach the target given the
//desired decleration.
double speed = distance / ((double)Decleration.normal * DeclerationTweaker);
//Make sure the velocity does not exceed the max
if (speed > this.MovingEntity.GetMaxSpeed())
speed = this.MovingEntity.GetMaxSpeed();
//From here proceed just like Seek except we don't need to normalize the TargetPosition Vector,
//because we have already gone to the trouble of calculating its lenght: distance;
DesiredVelocity = ToTarget * (float)speed / (float)distance;
return (DesiredVelocity - this.MovingEntity.GetVelocity());
}
return new Vector2D();
}
public void GetLengthTest()
{
Vector2D v1 = new Vector2D(100, 200);
var length = v1.GetLength();
Assert.AreEqual(223.6068, Math.Round(length * 10000) / 10000);
}
public void NormalizeTest()
{
Vector2D v1 = new Vector2D(100, 200);
var normalizedVector = v1.Normalize();
Assert.AreEqual(0.447, Math.Round(normalizedVector.x * 1000) / 1000);
Assert.AreEqual(0.894, Math.Round(normalizedVector.y * 1000) / 1000);
}
public void PerpendicularTest()
{
Vector2D v1 = new Vector2D(100, 200);
Vector2D perp = v1.Perpendicular();
Assert.AreEqual(200, perp.x);
Assert.AreEqual(-100, perp.y);
}
public void TruncateTest()
{
Vector2D v1 = new Vector2D(100, 200);
v1.Truncate(99);
Assert.AreEqual(44.27, Math.Round(v1.x * 100) / 100);
Assert.AreEqual(88.55, Math.Round(v1.y * 100) / 100);
}
public void VectorMultiplyTest()
{
Vector2D v1 = new Vector2D(100, 200);
Vector2D result = v1 * 20;
Assert.AreEqual(2000, result.x);
Assert.AreEqual(4000, result.y);
}
public WallEntity(Vector2D point1, Vector2D point2)
: base(point1 - point2)
{
this.Point1 = point1;
this.Point2 = point2;
this.Normal = CalculateNormal();
}
public Vector2D Calculate()
{
Vector2D vector = new Vector2D();
foreach (ISteeringBehaviour behaviour in this.Behaviours)
vector += behaviour.Calculate();
return vector;
}
public HumanWhoLostItsContacts(Vector2D position)
: base(position)
{
this.MaxSpeed = 0.1f;
this.Scale = 50f;
this.ViewDistance = 200f;
this.SteeringBehaviour.AddSteeringBehaviour(new WallAvoidance(this, World.Instance.Walls));
}
public JeepEntity(Vector2D position)
: base(position)
{
this.MaxSpeed = 0.3f;
this.Scale = 50f;
this.ViewDistance = 200f;
this.CurrentGoal = new FollowPath(this, PathFactory.GetJeepPath());
this.SteeringBehaviour.AddSteeringBehaviour(new Separation(this));
}
public Vector2D Calculate()
{
Vector2D steeringForce = new Vector2D();
steeringForce += (seperation.Calculate() * seperationMultiplier);
steeringForce += (cohesion.Calculate() * cohesionMultiplier);
steeringForce += (alignment.Calculate() * alignmentMultiplier);
return steeringForce;
}
public Vector2D Normalize()
{
Vector2D v1 = new Vector2D();
float length = this.GetLength();
if (length != 0)
{
v1.x = this.x / length;
v1.y = this.y / length;
}
return v1;
}
public LionEntity(Vector2D position)
: base(position, 1, 1)
{
this.Mass = 300f;
this.MaxSpeed = .1f;
this.ViewDistance = 100;
this.Carnivore = true;
this.Scale = 50;
this.SteeringBehaviour.AddSteeringBehaviour(new WallAvoidance(this, World.Instance.Walls));
this.SteeringBehaviour.AddSteeringBehaviour(new Flee(this, 50));
}
public override void Activate()
{
Status = Goal_Status.ACTIVE;
RemoveAllSubGoals();
if (!destinationIsSet)
{
currentDestination = new Vector2D(50, 50);
destinationIsSet = true;
}
AddSubGoal(new MoveToPosition(Owner, currentDestination));
}
public Vector2D Calculate()
{
//A list of feelers. The feelers are like the whiskers of a cat.
var feelers = CreateFeelers(movingEntity);
double distanceToThisIP = 0.0;
double distanceToClosestIP = Double.MaxValue;
//This will hold an index into the vector of walls
int closestWall = -1;
Vector2D steeringForce = new Vector2D();
Vector2D point = new Vector2D();
Vector2D closestPoint = new Vector2D();
//Examine each feeler in turn
for (int flr = 0; flr < feelers.Count(); flr++)
{
//Run through each wall checking for any intersection points
for (int w = 0; w < walls.Count; w++)
{
//If the vehicle intersects with the line
if (LineIntersection(this.movingEntity.GetPosition(), feelers[flr], walls[w].From(), walls[w].To(), out distanceToThisIP, out point))
{
//Is this the closest found so far? If so keep a reord
if (distanceToThisIP < distanceToClosestIP)
{
distanceToClosestIP = distanceToThisIP;
closestPoint = point;
closestWall = w;
}
}
} // next wall
//If an intersection point has been detected, calculate a force
//that will direct the agent away
if (closestWall >= 0)
{
//Calculate by what distance the projected position of the agent
//will overshoot the wall
Vector2D overShoot = feelers[flr] - closestPoint;
//Create a force in the direction of the wall normal, with a
//magnitude of the overshoot
steeringForce += walls[closestWall].Normal() * overShoot.GetLength();
}
} // next feeler
return steeringForce;
}
public static bool LineIntersectsWithCirlce(Vector2D circlePosition, float circleRadius, Vector2D point1, Vector2D point2)
{
//Distance from point1 to the middle of the circle
float distanceToPoint1 = Distance(circlePosition, point1);
//Distance from point2 to the middle of the circle
float distanceToPoint2 = Distance(circlePosition, point2);
if (distanceToPoint1 < circleRadius || distanceToPoint2 < circleRadius)
return true;
return false;
}
/// <summary>
/// Het Dijkstra-algoritme kan alleen gebruikt worden wanneer er geen negatieve paden aanwezig zijn.
/// Zodra deze wel aanwezig zijn zal deze functie in een oneindige lus blijven hangen.
/// </summary>
/// <param name="startName">Het punt waar vanuit de korste routes berekent moeten worden.</param>
/// <returns>Returns all the considered positions</returns>
public List<Vector2D> DijkstraWithManhattan(Vector2D startPosition, Vector2D endPosition)
{
MinHeap<Path> minHeap = new MinHeap<Path>();
Node startNode = FindClosestNode(startPosition);
Node endNode = FindClosestNode(endPosition);
List<Vector2D> considerdPositions = new List<Vector2D>();
clearAll();
startNode.Distance = 0 + GetManhattanValue(startNode.Position, endNode.Position);
minHeap.Insert(new Path(startNode, startNode.Distance));
int nodeSeen = 0;
while (minHeap.Count != 0 && nodeSeen < nodeMap.Count)
{
Path vrec = minHeap.ExtractMin();
Node v = vrec.Destination;
if (v.Scratch != 0)
continue;
v.Scratch = 1;
nodeSeen++;
foreach (Edge e in v.Adjacent)
{
Node w = e.destination;
double cvw = e.cost;
if (cvw < 0)
throw new Exception("Graph has negative edges.");
float heuristic1 = GetManhattanValue(w.Position, endNode.Position);
float heuristic2 = GetManhattanValue(v.Position, endNode.Position);
if (w.Distance + heuristic1 > v.Distance + heuristic2 + cvw)
{
w.Distance = v.Distance + heuristic2 + cvw;
w.Previous = v;
considerdPositions.Add(w.Position);
if (w.Position == endNode.Position)
return considerdPositions;
minHeap.Insert(new Path(w, w.Distance));
}
}
}
return considerdPositions;
}
public Vector2D Calculate()
{
float rand = (Convert.ToSingle(new Random().Next(-10, 10)) / 10f);
Vector2D velocity = movingEntity.GetVelocity();
circleCenter = new Vector2D(velocity.x, velocity.y);
circleCenter.Normalize();
circleCenter *= circleDistance;
displacement = new Vector2D(0, -1);
displacement *= circleRadius;
displacement = SetAngle(displacement, displacementAngle);
displacementAngle += rand * circleAngleChange - circleAngleChange * .5f;
wanderForce = circleCenter + displacement;
return wanderForce;
}
public void Draw(Graphics g)
{
Pen p = new Pen(Color.Red, 5f);
var pentagon = new List<Vector2D>();
float c1 = Convert.ToSingle(Math.Cos((2 * Math.PI) / 5));
float c2 = Convert.ToSingle(Math.Cos(Math.PI / 5));
float s1 = Convert.ToSingle(Math.Sin((2 * Math.PI / 5)));
float s2 = Convert.ToSingle(Math.Sin((4 * Math.PI) / 5));
var vA = new Vector2D(1, 0);
var vB = new Vector2D(c1, s1);
var vC = new Vector2D(-c2, s2);
var vD = new Vector2D(-c2, -s2);
var vE = new Vector2D(c1, -s1);
Matrix rotationMatrix = Matrix.Rotate(Rotation);
pentagon.Add((vA * Scale) * rotationMatrix);
pentagon.Add((vB * Scale) * rotationMatrix);
pentagon.Add((vC * Scale) * rotationMatrix);
pentagon.Add((vD * Scale) * rotationMatrix);
pentagon.Add((vE * Scale) * rotationMatrix);
pentagon = Screen.Viewporttranformation(width, height, pentagon);
for (int i = 0; i < pentagon.Count; i++)
{
Vector2D previous;
Vector2D current = pentagon[i];
if (i == 0)
previous = pentagon[pentagon.Count - 1];
else
previous = pentagon[i - 1];
g.DrawLine(p, previous.x, previous.y, current.GetWidth(), current.GetHeight());
}
}
public WaterEntity(Vector2D position, float scale = 25, float bradius = 1)
: base(position, scale, bradius)
{
this.Bradius = 150;
}
public void DefaultVectorTest()
{
Vector2D v1 = new Vector2D(100, 200);
Assert.AreEqual(100, v1.x);
Assert.AreEqual(200, v1.y);
}
public RockEntity(Vector2D position)
: base(position, 1, 1)
{
Bradius = 50;
}
public static Graph GenerateWorldGraph(World world)
{
Graph graph = new Graph();
float maxWidth = World.Width;
float maxHeight = World.Height;
int edgeSize = 30;
//Add a little padding on the edge of the world. We don't want our entities to fall off the world.
int OffsetX = (World.Width % edgeSize) / 2;
int OffsetY = (World.Height % edgeSize) / 2;
for (int y = OffsetY; y < maxHeight; y += edgeSize)
{
for (int x = OffsetX; x < maxWidth; x += edgeSize)
{
Vector2D currentPosition = new Vector2D(x, y);
var staticObjects = world.GetAllStaticObjects();
//Check if new node fits on the world
if (currentPosition.x + edgeSize < maxWidth)
{
Vector2D promissingPoint = new Vector2D(x + edgeSize, y);
bool doesIntersect = false;
foreach (BaseEntity entity in staticObjects)
if (Util.LineIntersectsWithCirlce(entity.Position, entity.Bradius / 2, currentPosition, promissingPoint))
doesIntersect = true;
if (!doesIntersect)
{
//Add edges in two directions
// A --> B
// A <-- B
graph.AddEdge(currentPosition, promissingPoint);
graph.AddEdge(promissingPoint, currentPosition);
}
}
if (currentPosition.y + edgeSize < maxHeight)
{
Vector2D promissingPoint = new Vector2D(x, y + edgeSize);
bool doesIntersect = false;
foreach (BaseEntity entity in staticObjects)
if (Util.LineIntersectsWithCirlce(entity.Position, entity.Bradius / 2, currentPosition, promissingPoint))
doesIntersect = true;
if (!doesIntersect)
{
//Add edges in two directions
// A --> B
// A <-- B
graph.AddEdge(currentPosition, promissingPoint);
graph.AddEdge(promissingPoint, currentPosition);
}
}
}
}
return graph;
}
public Vector2D SetAngle(Vector2D v, float number)
{
var lenght = v.GetLength();
return new Vector2D(Convert.ToSingle(Math.Cos(number) * lenght),
Convert.ToSingle(Math.Sin(number) * lenght));
}
public Seek(IMovingEntity entity, Vector2D targetPosition)
{
this.movingEntity = entity;
this.TargetPosition = targetPosition;
}
