public float getThreat(Node origin, Node dest)
{
Vector2 d = dest.Pos - origin.Pos;
Vector2 f = origin.Pos - this.Pos;
float a = d.X * d.X + d.Y * d.Y;
float b = 2 * (f.X * d.X + f.Y * d.Y);
float c = f.X * f.X + f.Y * f.Y - Radius * Radius;
float discr = b * b - 4 * a * c;
if (discr < 0) { return 0; }
discr = (float)Math.Sqrt(discr);
float t1 = (-b + discr) / (2 * a);
float t2 = (-b - discr) / (2 * a);
Vector2 dir = dest.Pos - origin.Pos;
Vector2 inter0 = origin.Pos;
if (t1 >= 0 && t1 <= 1)
{
inter0 = origin.Pos + dir * t1;
}
Vector2 inter1 = origin.Pos;
if (t2 >= 0 && t2 <= 1)
{
inter1 = origin.Pos + dir * t2;
}
return (inter0 - inter1).Length() / (Radius * 2.0f);
}
public AStar(Node start, Node dest)
{
this.startNode = start;
this.destNode = dest;
this.Allies = new List<Agent>();
this.Enemies = new List<Agent>();
this.FrontierNodes = new List<Node>();
this.FrontierNodes.Add(start);
start.G = 0;
this.VisitedNodes = new List<Node>();
}
public BiDirectionAStar(Node start, Node dest)
{
this.startNode = start;
this.destNode = dest;
this.startToDest = new AStar(start, dest);
this.destToStart = new AStar(dest, start);
answer = new List<Node>();
stdSoFar = new List<Node>();
dtsSoFar = new List<Node>();
stdFrontier = this.startToDest.FrontierNodes;
dtsFrontier = this.destToStart.FrontierNodes;
counter = 0;
}
public static float h(Node origNode, Node destNode, List<Agent> enemies, List<Agent> allies)
{
float d = AStarHelper.Dist(origNode, destNode);
float penaltyDist = 0f;
foreach (Agent e in enemies)
{
penaltyDist += e.getThreat(origNode, destNode) * (e.Radius * 2);
}
float bonusDist = 0f;
foreach (Agent a in allies)
{
bonusDist += a.getThreat(origNode, destNode) * (a.Radius * 2);
}
float c1 = 0.1f;
float c2 = 0.9f;
return (d - penaltyDist - bonusDist) * c1 + c2 * penaltyDist;
}
private List<Node> getCameFrom(Node n, string index)
{
List<Node> temp = new List<Node>();
if (index.Equals("std"))
{
temp.Add(n);
if (n.CameFrom.ContainsKey(index))
{
temp.AddRange(getCameFrom(n.CameFrom[index], index));
}
}
else if (index.Equals("dts"))
{
if (n.CameFrom.ContainsKey(index))
{
temp.AddRange(getCameFrom(n.CameFrom[index], index));
}
temp.Add(n);
}
return temp;
}
public static float g(Node origNode, Node destNode, List<Agent> enemies, List<Agent> allies)
{
return h(origNode, destNode, enemies, allies);
}
public static float Dist(Node a, Node b)
{
return Vector2.Distance(a.Pos, b.Pos);
}
/// <summary>
/// Allows the game to perform any initialization it needs to before starting to run.
/// This is where it can query for any required services and load any non-graphic
/// related content. Calling base.Initialize will enumerate through any components
/// and initialize them as well.
/// </summary>
protected override void Initialize() {
// TODO: Add your initialization logic here
//Discretize space and create nodes.
int nodeWidth = (int)(AbstractDemo.WIDTH / this.delta);
int nodeHeight = (int)(AbstractDemo.HEIGHT / this.delta);
this.nodeArray = new Node[nodeWidth, nodeHeight];
Vector2 v = Vector2.Zero;
for (int x = 0; x < nodeWidth; x++) {
for (int y = 0; y < nodeHeight; y++) {
v.X = (x + 0.5f) * this.delta;
v.Y = (y + 0.5f) * this.delta;
Node node = new Node();
node.Pos = v;
this.nodeArray[x, y] = node;
}
}
//Link nodes to their neighbors.
for (int i = 0; i < nodeWidth; i++) {
for (int j = 0; j < nodeHeight; j++) {
Node node = this.nodeArray[i, j];
bool topEdge = j - 1 < 0;
bool botEdge = j + 1 >= nodeHeight;
bool ltEdge = i - 1 < 0;
bool rtEdge = i + 1 >= nodeWidth;
//NW
if (!topEdge && !ltEdge) node.Neighbors.Add(this.nodeArray[i - 1, j - 1]);
//N
if (!topEdge) node.Neighbors.Add(this.nodeArray[i, j - 1]);
//NE
if (!topEdge && !rtEdge) node.Neighbors.Add(this.nodeArray[i + 1, j - 1]);
//E
if (!rtEdge) node.Neighbors.Add(this.nodeArray[i + 1, j]);
//SE
if (!botEdge && !rtEdge) node.Neighbors.Add(this.nodeArray[i + 1, j + 1]);
//S
if (!botEdge) node.Neighbors.Add(this.nodeArray[i, j + 1]);
//SW
if (!botEdge && !ltEdge) node.Neighbors.Add(this.nodeArray[i - 1, j + 1]);
//W
if (!ltEdge) node.Neighbors.Add(this.nodeArray[i - 1, j]);
}
}
//Create agents.
this.threats = new List<Agent>();
this.allies = new List<Agent>();
Agent threat1 = new Agent(new Vector2(1 * AbstractDemo.WIDTH / 2f, 1 * AbstractDemo.HEIGHT / 4f), 96);
Agent threat2 = new Agent(new Vector2(3 * AbstractDemo.WIDTH / 4f, 1 * AbstractDemo.HEIGHT / 4f), 96);
Agent threat3 = new Agent(new Vector2(1 * AbstractDemo.WIDTH / 3f, 2 * AbstractDemo.HEIGHT / 3f), 96);
Agent threat4 = new Agent(new Vector2(7 * AbstractDemo.WIDTH / 8f, 3 * AbstractDemo.HEIGHT / 4f), 96);
this.threats.Add(threat1);
this.threats.Add(threat2);
this.threats.Add(threat3);
this.threats.Add(threat4);
Agent ally1 = new Agent(new Vector2(1 * AbstractDemo.WIDTH / 4f, 1 * AbstractDemo.HEIGHT / 2f), 96);
Agent ally2 = new Agent(new Vector2(1 * AbstractDemo.WIDTH / 3f, 3 * AbstractDemo.HEIGHT / 4f), 96);
Agent ally3 = new Agent(new Vector2(1 * AbstractDemo.WIDTH / 2f, 3 * AbstractDemo.HEIGHT / 4f), 96);
Agent ally4 = new Agent(new Vector2(2 * AbstractDemo.WIDTH / 3f, 1 * AbstractDemo.HEIGHT / 2f), 96);
this.allies.Add(ally1);
this.allies.Add(ally2);
this.allies.Add(ally3);
this.allies.Add(ally4);
//Create search engine.
this.pather = new BiDirectionAStar(this.nodeArray[0, 0], this.nodeArray[nodeWidth - 1, nodeHeight - 1]);
this.pather.Enemies = this.threats;
this.pather.Allies = this.allies;
base.Initialize();
}
public bool Equals(Node n)
{
return this.Pos.X == n.Pos.X && this.Pos.Y == n.Pos.Y;
}
public float h(Node destNode, List<Agent> enemies, List<Agent> allies)
{
return AStarHelper.h(this, destNode, enemies, allies);
}
