public int steps; // hány lépésben jutna ide a robot public Astar(NewmazonClasses field, NewmazonClasses target) { neighbours = new Astar[5]; blocked = new List <int>(); steps = 1000000; tile = field; td = Math.Abs(target.x - field.x) + Math.Abs(target.y - field.y); sd = 1000000; pi = null; dir = -1; }
public List <NewmazonClasses> PathSearch(NewmazonClasses destination) { //TODO return(null); }
public void CalculateRoute(Robot robot, NewmazonClasses target) // útvonaltervező függvény { List <Astar> prioQ = new List <Astar>(); List <int>[,] blocks = new List <int> [tableSize, tableSize]; // blokkolt időpontok mátrixa (tábla szerint) for (int i = 0; i < tableSize; ++i) { for (int j = 0; j < tableSize; ++j) { blocks[i, j] = new List <int>(); } } for (int i = 0; i < robots.Count; ++i) // minden roboton végigfut, és berakja a blokkolt időpontok mátrixába a megfelelő mezőkre, hogy mikor vannak blokkolva { if (paths[i].Count > 0) { int steps = 0; blocks[paths[i][0].x, paths[i][0].y].Add(steps - 1); blocks[paths[i][0].x, paths[i][0].y].Add(steps); blocks[paths[i][0].x, paths[i][0].y].Add(steps + 1); steps++; for (int j = 1; j < paths[i].Count; ++j) { if (paths[i][j].dir != paths[i][j - 1].dir) { blocks[paths[i][j - 1].x, paths[i][j - 1].y].Add(steps - 1); blocks[paths[i][j - 1].x, paths[i][j - 1].y].Add(steps); blocks[paths[i][j - 1].x, paths[i][j - 1].y].Add(steps + 1); steps++; } blocks[paths[i][j].x, paths[i][j].y].Add(steps - 1); blocks[paths[i][j].x, paths[i][j].y].Add(steps); blocks[paths[i][j].x, paths[i][j].y].Add(steps + 1); steps++; } } if (robots[i] != robot) { blocks[robots[i].x, robots[i].y].Add(0); blocks[robots[i].x, robots[i].y].Add(1); } } for (int i = 0; i < tableSize; ++i) // megnézi, hogy melyik mezőkhöz kell rendelni Node-ot (Pl a falhoz nem kell, mert oda nem léphet robot) { for (int j = 0; j < tableSize; ++j) { int ID = table[i, j].ID; if (((0 < ID && ID < 10001) || (20000 < ID && ID < 30001 && robot.polc == null)) && table[robot.x, robot.y].ID != ID) { Astar a = new Astar(table[i, j], target); a.blocked = blocks[i, j]; prioQ.Add(a); } else if (table[robot.x, robot.y].ID == ID) { Astar a = new Astar(table[i, j], target); a.blocked = blocks[i, j]; a.sd = 0; a.dir = robot.dir; a.steps = 0; prioQ.Add(a); } } } Astar fin = new Astar(target, target); // targethez külön hozzá kell rendelni egy Node-ot, mert pl az lehet töltőállomás, ami alapból nem kerül bele az algoritmusba fin.blocked = blocks[target.x, target.y]; prioQ.Add(fin); foreach (Astar a in prioQ) // szomszédok hozzárendelése egymáshoz { foreach (Astar b in prioQ) { if (a.tile.x == b.tile.x && a.tile.y == b.tile.y - 1) { a.neighbours[0] = b; } if (a.tile.x == b.tile.x && a.tile.y == b.tile.y + 1) { a.neighbours[2] = b; } if (a.tile.x == b.tile.x + 1 && a.tile.y == b.tile.y) { a.neighbours[1] = b; } if (a.tile.x == b.tile.x - 1 && a.tile.y == b.tile.y) { a.neighbours[3] = b; } } a.neighbours[4] = a; } prioQ = prioQ.OrderBy(o => (o.sd + o.td)).ToList(); // rendezés sd+td szerint Astar u = prioQ[0]; prioQ.RemoveAt(0); // mindig a legkisebb sd+td -vel rendelkező elem kivétele while (u.tile != target) // amíg meg nem találjuk a célt, addig fusson { Astar v = u.neighbours[0]; // mindig csak a szomszédok között "fut él", ezért csak azokat kell megnézni, hogy az adott mezőből könnyebben oda tudunk-e jutni int dirC = 0; int stepC = 0; if (v != null) // jobb szomszéd { switch (u.dir) { case 0: dirC = 0; stepC = 0; break; case 1: dirC = 1; stepC = 1; break; case 2: dirC = 2; stepC = 1; break; case 3: dirC = 1; stepC = 1; break; } if (u.sd + u.td + dirC + 1 < v.sd + v.td && !v.blocked.Contains(u.steps + stepC)) // ha gyorsabban idejutunk, és nicns blokkolva ebben az időben, akkor mostmár u-ból jöjjünk ide { v.sd = u.sd + dirC + 1; v.pi = u; v.dir = 0; v.steps = u.steps + stepC + 1; } } v = u.neighbours[1]; if (v != null) // felső szomszéd { switch (u.dir) { case 0: dirC = 1; stepC = 1; break; case 1: dirC = 0; stepC = 0; break; case 2: dirC = 1; stepC = 1; break; case 3: dirC = 2; stepC = 1; break; } if (u.sd + u.td + dirC + 1 < v.sd + v.td && !v.blocked.Contains(u.steps + stepC)) // ha gyorsabban idejutunk, és nicns blokkolva ebben az időben, akkor mostmár u-ból jöjjünk ide { v.sd = u.sd + dirC + 1; v.pi = u; v.dir = 1; v.steps = u.steps + stepC + 1; } } v = u.neighbours[2]; if (v != null) // bal szomszéd { switch (u.dir) { case 0: dirC = 2; stepC = 1; break; case 1: dirC = 1; stepC = 1; break; case 2: dirC = 0; stepC = 0; break; case 3: dirC = 1; stepC = 1; break; } if (u.sd + u.td + dirC + 1 < v.sd + v.td && !v.blocked.Contains(u.steps + stepC)) // ha gyorsabban idejutunk, és nicns blokkolva ebben az időben, akkor mostmár u-ból jöjjünk ide { v.sd = u.sd + dirC + 1; v.pi = u; v.dir = 2; v.steps = u.steps + stepC + 1; } } v = u.neighbours[3]; if (v != null) // alsó szomszéd { switch (u.dir) { case 0: dirC = 1; stepC = 1; break; case 1: dirC = 2; stepC = 1; break; case 2: dirC = 1; stepC = 1; break; case 3: dirC = 0; stepC = 0; break; } if (u.sd + u.td + dirC + 1 < v.sd + v.td && !v.blocked.Contains(u.steps + stepC)) // ha gyorsabban idejutunk, és nicns blokkolva ebben az időben, akkor mostmár u-ból jöjjünk ide { v.sd = u.sd + dirC + 1; v.pi = u; v.dir = 3; v.steps = u.steps + stepC + 1; } } prioQ = prioQ.OrderBy(o => o.sd + o.td).ToList(); // itt jön egy trükk: hogyha a blokkolt elemek miatt a PrioQ-ban a legkisebb prioritású elem még inicializálatlan, // azaz a pi még null, akkor ne vegye ki azt a Node-ot, helyette rakjon bele a queue-ba egy másik Node-ot, aminek // ugyanaz a koordinátája, mint az u-nak. // és ha éppen ez is blokkolt, akkor szimplán hívjon meg egy AddStop(robot, 1)-et, ami megállítja a robotot egy tickre, // így majd a léptető függvény elrendezi a potenciális ütközést. if (prioQ[0].sd > 10000) { Astar newA = new Astar(u.tile, target); newA.blocked = u.blocked; newA.steps = u.steps + 1; newA.neighbours = u.neighbours; if (u.neighbours[0] != null) { u.neighbours[0].neighbours[2] = newA; } if (u.neighbours[1] != null) { u.neighbours[1].neighbours[3] = newA; } if (u.neighbours[2] != null) { u.neighbours[2].neighbours[0] = newA; } if (u.neighbours[3] != null) { u.neighbours[3].neighbours[1] = newA; } if (!newA.blocked.Contains(newA.steps)) { newA.sd = u.sd + 1; newA.dir = u.dir; newA.pi = u; u = newA; } else { AddStop(robot, 1); return; } } // egyébként vegye ki a legkisebb prioritású elemet, DE attól még ugyanúgy rakjon bele a prioQ-ba egy másik Node-ot, // u koordinátájával. else { Astar newA = new Astar(u.tile, target); newA.blocked = u.blocked; newA.steps = u.steps + 1; newA.neighbours = u.neighbours; if (u.neighbours[0] != null) { u.neighbours[0].neighbours[2] = newA; } if (u.neighbours[1] != null) { u.neighbours[1].neighbours[3] = newA; } if (u.neighbours[2] != null) { u.neighbours[2].neighbours[0] = newA; } if (u.neighbours[3] != null) { u.neighbours[3].neighbours[1] = newA; } if (!newA.blocked.Contains(newA.steps)) { newA.sd = u.sd + 1; newA.dir = u.dir; newA.pi = u; } else { newA.sd = 1000000; newA.dir = -1; newA.pi = null; } prioQ.Add(newA); u = prioQ[0]; prioQ.RemoveAt(0); } } int endDir = u.dir; while (u.pi != null) { paths[robot.ID - 40001].Add(new Step(u.tile.x, u.tile.y, u.dir)); // ha megtaláltuk a targetet, akkor visszafelé a pi segítségével állítsuk elő az útvonalat u = u.pi; } paths[robot.ID - 40001].Reverse(); if (table[target.x, target.y].ID > 30000 && table[target.x, target.y].ID < 40001) // ha a target egy töltőállomás, akkor még álljon 4et hogy meglegyen az 5 { paths[robot.ID - 40001].Add(new Step(target.x, target.y, endDir)); paths[robot.ID - 40001].Add(new Step(target.x, target.y, endDir)); paths[robot.ID - 40001].Add(new Step(target.x, target.y, endDir)); paths[robot.ID - 40001].Add(new Step(target.x, target.y, endDir)); } if (table[target.x, target.y].ID > 10000 && table[target.x, target.y].ID < 20001) // ha pedig célállomás, akkor álljon még egyet. { paths[robot.ID - 40001].Add(new Step(target.x, target.y, endDir)); } }
public void StepSimulation() // Szimuláció léptetése { totalSteps++; for (int i = 0; i < robots.Count; ++i) // Minden robotra megnézni, hogyha üres az útja, akkor új út megtervezése { if (paths[i].Count > 0) { bool foglalt = false; for (int j = 0; j < robots.Count; ++j) // Megnézi, hogy a következő mezőn van-e robot, { if (paths[i][0].x == robots[j].x && paths[i][0].y == robots[j].y && i != j) { foglalt = true; } } if (!foglalt) // Ha nincs, akkor lép a robot { if (robots[i].x == paths[i][0].x && // Nem mozog a robot robots[i].y == paths[i][0].y && robots[i].dir == paths[i][0].dir) { paths[i].RemoveAt(0); } else if (robots[i].dir != paths[i][0].dir) // Ha nem jó a robot iránya, akkor fordul { robots[i].dir = paths[i][0].dir; robots[i].energy--; robotEnergyUsed[i]++; totalEnergyUsed++; } else { // egyébként lép a következő mezőre robots[i].x = paths[i][0].x; robots[i].y = paths[i][0].y; robots[i].energy--; robotEnergyUsed[i]++; totalEnergyUsed++; paths[i].RemoveAt(0); } } else // Ha következő mező foglalt, akkor kiüríti az útvonalát, és ügyel, // hogyha kapott utasítást hogy vegyen fel egy polcot, akkor azon a polcon lévő termékeket visszarakja a goods listába { if (table[paths[i].Last().x, paths[i].Last().y].goods != null && table[paths[i].Last().x, paths[i].Last().y].goods.Count > 0) { int[] g1 = new int[table[paths[i].Last().x, paths[i].Last().y].goods.Count]; for (int j = 0; j < table[paths[i].Last().x, paths[i].Last().y].goods.Count; ++j) { g1[j] = table[paths[i].Last().x, paths[i].Last().y].goods[j]; } goods.Add(new Goods(paths[i].Last().x, paths[i].Last().y, g1)); } paths[i].Clear(); } } else { DoStationaryThings(robots[i]); // Minden egy helyben megcsinálható dolgot itt végez el (Pl robot feltöltése, vagy polc lerakása, stb...) NewmazonClasses target = CalculateNextJob(robots[i]); // A következő célpont kiszámítása if (EverythingIsHome() && EverythingDelivered()) // Ha minden polc a helyén van, és nincs semmi már a polcokat, akkor vége a szimulációnak. { OnSimOver(); return; } CalculateRoute(robots[i], target); // Útvonal megtervezése (A* algoritmussal) } } }