Skip to content
This repository was archived by the owner on Mar 3, 2019. It is now read-only.

Commit

Permalink
Bugfix D*
Browse files Browse the repository at this point in the history
  • Loading branch information
@PFGimenez
PFGimenez committed Oct 28, 2017
1 parent 5b06197 commit f704ab1
Show file tree
Hide file tree
Showing 3 changed files with 95 additions and 50 deletions.
6 changes: 4 additions & 2 deletions pc/src/BenchmarkPathfinding.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,15 +51,17 @@ public static void main(String[] args)
Cinematique depart = new Cinematique(-800, 350, Math.PI / 2, true, 0);
robot.setCinematique(depart);
cercle.set(GameElementNames.MINERAI_CRATERE_HAUT_GAUCHE, 230, 30, -30, 10, -10);
int nbtest = 1000;
int nbtest = 100;
log.debug("Début du test…");
for(int i = 0; i < nbtest; i++)
{
System.out.println(i);
astar.initializeNewSearchToCircle(true, state);
astar.process(chemin, false);
chemin.clear();
}
log.debug("Temps : " + (System.nanoTime() - avant) / (nbtest * 1000000.));

System.out.println("Temps : " + (System.nanoTime() - avant) / (nbtest * 1000000.));
container.destructor();
}
catch(PathfindingException | InterruptedException | ContainerException | MemoryManagerException e)
Expand Down
36 changes: 23 additions & 13 deletions pc/src/pathfinding/dstarlite/DStarLite.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
import table.RealTable;
import utils.Log;
import utils.Vec2RO;
import utils.Vec2RW;
import config.Config;
import config.ConfigInfo;
import container.Service;
Expand Down Expand Up @@ -145,10 +146,9 @@ private DStarLiteNode getFromMemoryUpdated(PointGridSpace gridpoint)
return null;

DStarLiteNode out = memory[gridpoint.hashcode];

if(out.nbPF != nbPF)
return null;

out.update(nbPF);

updateStart(gridpoint);
return out;
}

Expand Down Expand Up @@ -346,18 +346,26 @@ public synchronized void updateStart(Vec2RO positionRobot)
updateStart(pointManager.get(positionRobot));
}

private synchronized void updateStart(PointGridSpace p)
{
updateStart(getFromMemory(p));
}

/**
* Met à jour la position actuelle du robot
*
* @param positionRobot
*/
private synchronized void updateStart(PointGridSpace p)
private synchronized final void updateStart(DStarLiteNode p)
{
depart = getFromMemory(p);
km += distanceHeuristique(lastDepart);
lastDepart = depart.gridpoint;

computeShortestPath();
// p est consistent ssi il n'est pas l'openset
if(p.inOpenSet)
{
depart = p;
km += distanceHeuristique(lastDepart);
lastDepart = depart.gridpoint;
computeShortestPath();
}
}

/**
Expand Down Expand Up @@ -577,6 +585,8 @@ public synchronized Double heuristicCostCourbe(Cinematique c/*
return 1.3 * erreurDistance + 5 * erreurOrientation;
}

private Vec2RW tmpVec2 = new Vec2RW();

/**
* Fournit une heuristique de l'orientation à prendre en ce point
*
Expand Down Expand Up @@ -617,9 +627,9 @@ private double getOrientationHeuristique(PointGridSpace p)
// simple (trajet à vol
// d'oiseau)
{
directionX = arrivee.gridpoint.x - p.x;
directionY = arrivee.gridpoint.y - p.y;
n.heuristiqueOrientation = Math.atan2(directionY, directionX);
tmpVec2.setX(arrivee.gridpoint.x - p.x);
tmpVec2.setY(arrivee.gridpoint.y - p.y);
n.heuristiqueOrientation = tmpVec2.getFastArgument();
}

if(graphicHeuristique)
Expand Down
103 changes: 68 additions & 35 deletions pc/src/pathfinding/dstarlite/EnhancedPriorityQueue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ public class EnhancedPriorityQueue
{
private DStarLiteNode[] tab = new DStarLiteNode[PointGridSpace.NB_POINTS];
private int firstAvailable = 1;
private DStarLiteNode lastInserted = null;

/**
* Renvoie la racine et la supprime
Expand All @@ -40,11 +41,20 @@ public class EnhancedPriorityQueue
*/
public DStarLiteNode poll()
{
DStarLiteNode out = tab[1];
tab[1] = tab[--firstAvailable];
tab[1].indexPriorityQueue = 1;
percolateDown(tab[1]);
return out;
if(tab[1] == null || (lastInserted != null && tab[1].cle.greaterThan(lastInserted.cle)))
{
DStarLiteNode tmp = lastInserted;
lastInserted = null;
return tmp;
}
else
{
DStarLiteNode out = tab[1];
tab[1] = tab[--firstAvailable];
tab[1].indexPriorityQueue = 1;
percolateDown(tab[1]);
return out;
}
}

/**
Expand All @@ -54,6 +64,8 @@ public DStarLiteNode poll()
*/
public DStarLiteNode peek()
{
if(tab[1] == null || (lastInserted != null && tab[1].cle.greaterThan(lastInserted.cle)))
return lastInserted;
return tab[1];
}

Expand All @@ -63,6 +75,7 @@ public DStarLiteNode peek()
public void clear()
{
firstAvailable = 1;
lastInserted = null;
}

/**
Expand Down Expand Up @@ -112,18 +125,23 @@ private final int pere(int index)
{
return index >> 1;
}

/**
* Ajoute un nœuds au tas
*
* @param node
*/
public void add(DStarLiteNode node)
{
tab[firstAvailable] = node;
node.indexPriorityQueue = firstAvailable;
firstAvailable++;
percolateUp(node);
DStarLiteNode tmp = lastInserted;
lastInserted = node;
if(tmp != null)
{
tab[firstAvailable] = tmp;
tmp.indexPriorityQueue = firstAvailable;
firstAvailable++;
percolateUp(tmp);
}
}

/**
Expand All @@ -133,18 +151,23 @@ public void add(DStarLiteNode node)
*/
public void percolateDown(DStarLiteNode node)
{
DStarLiteNode n = node;
int fg, diff;
// diff < 0 si aucun enfant, diff = 0 si un enfant (le gauche), diff > 0
// si deux enfants
while((diff = (firstAvailable - 1 - (fg = 2 * n.indexPriorityQueue))) >= 0)
if(lastInserted == node)
add(null); // pour ajouter lastInserted dans le tas (fait automatique le percolateUp)
else
{
if(diff > 0 && tab[fg].cle.greaterThan(tab[fg + 1].cle))
fg++;
if(n.cle.greaterThan(tab[fg].cle))
swap(fg, n.indexPriorityQueue);
else
return;
DStarLiteNode n = node;
int fg, diff;
// diff < 0 si aucun enfant, diff = 0 si un enfant (le gauche), diff > 0
// si deux enfants
while((diff = (firstAvailable - 1 - (fg = 2 * n.indexPriorityQueue))) >= 0)
{
if(diff > 0 && tab[fg].cle.greaterThan(tab[fg + 1].cle))
fg++;
if(n.cle.greaterThan(tab[fg].cle))
swap(fg, n.indexPriorityQueue);
else
return;
}
}
}

Expand All @@ -155,15 +178,20 @@ public void percolateDown(DStarLiteNode node)
*/
public void remove(DStarLiteNode node)
{
DStarLiteNode last = tab[--firstAvailable];
tab[node.indexPriorityQueue] = last;
last.indexPriorityQueue = node.indexPriorityQueue;

if(node.cle.lesserThan(last.cle)) // ce qui a remplacé node est plus
// grand : on descend
percolateDown(last);
if(lastInserted == node)
lastInserted = null;
else
percolateUp(last);
{
DStarLiteNode last = tab[--firstAvailable];
tab[node.indexPriorityQueue] = last;
last.indexPriorityQueue = node.indexPriorityQueue;

if(node.cle.lesserThan(last.cle)) // ce qui a remplacé node est plus
// grand : on descend
percolateDown(last);
else
percolateUp(last);
}
}

/**
Expand All @@ -173,11 +201,16 @@ public void remove(DStarLiteNode node)
*/
public void percolateUp(DStarLiteNode node)
{
int p;
// si ce nœud n'est pas la racine et que son père est plus grand : on
// inverse
while(node.indexPriorityQueue > 1 && tab[p = pere(node.indexPriorityQueue)].cle.greaterThan(tab[node.indexPriorityQueue].cle))
swap(node.indexPriorityQueue, p);
if(lastInserted == node)
add(null); // pour ajouter lastInserted dans le tas (fait automatique le percolateUp)
else
{
int p;
// si ce nœud n'est pas la racine et que son père est plus grand : on
// inverse
while(node.indexPriorityQueue > 1 && tab[p = pere(node.indexPriorityQueue)].cle.greaterThan(tab[node.indexPriorityQueue].cle))
swap(node.indexPriorityQueue, p);
}
}

/**
Expand All @@ -187,7 +220,7 @@ public void percolateUp(DStarLiteNode node)
*/
public boolean isEmpty()
{
return firstAvailable == 1;
return firstAvailable == 1 && lastInserted == null;
}

/**
Expand Down

0 comments on commit f704ab1

Please sign in to comment.