private void CalculateOptimalWayPoint()
{
//Heatmap StrategyHeatmap = StrategyHeatmap.Copy();
//Génération de la HeatMap
//Heatmap heatMap = new Heatmap(22, 14, 0.01);
sw.Reset();
sw.Start(); // début de la mesure
//Génération de la HeatMap
waypointHeatMap.ReInitHeatMapData();
int[] nbComputationsList = new int[waypointHeatMap.nbIterations];
//On construit le heatMap en mode multi-résolution :
//On commence par une heatmap très peu précise, puis on construit une heat map de taille réduite plus précise autour du point chaud,
//Puis on construit une heatmap très précise au cm autour du point chaud.
double optimizedAreaSize;
PointD OptimalPosition = new PointD(0, 0);
PointD OptimalPosInBaseHeatMapCoordinates = waypointHeatMap.GetBaseHeatMapPosFromFieldCoordinates(0, 0);
for (int n = 0; n < waypointHeatMap.nbIterations; n++)
{
double subSamplingRate = waypointHeatMap.SubSamplingRateList[n];
if (n >= 1)
{
optimizedAreaSize = waypointHeatMap.nbCellInSubSampledHeatMapWidthList[n] / waypointHeatMap.nbCellInSubSampledHeatMapWidthList[n - 1];
}
else
{
optimizedAreaSize = waypointHeatMap.nbCellInSubSampledHeatMapWidthList[n];
}
optimizedAreaSize /= 2;
double minY = Math.Max(OptimalPosInBaseHeatMapCoordinates.Y / subSamplingRate - optimizedAreaSize, 0);
double maxY = Math.Min(OptimalPosInBaseHeatMapCoordinates.Y / subSamplingRate + optimizedAreaSize, Math.Min(waypointHeatMap.nbCellInSubSampledHeatMapHeightList[n], waypointHeatMap.nbCellInBaseHeatMapHeight));
double minX = Math.Max(OptimalPosInBaseHeatMapCoordinates.X / subSamplingRate - optimizedAreaSize, 0);
double maxX = Math.Min(OptimalPosInBaseHeatMapCoordinates.X / subSamplingRate + optimizedAreaSize, Math.Min(waypointHeatMap.nbCellInSubSampledHeatMapWidthList[n], waypointHeatMap.nbCellInBaseHeatMapWidth));
double max = double.NegativeInfinity;
int maxXpos = 0;
int maxYpos = 0;
for (double y = minY; y < maxY; y += 1)
{
for (double x = minX; x < maxX; x += 1)
{
//Attention, le remplissage de la HeatMap se fait avec une inversion des coordonnées
//double value = Math.Max(0, 1 - Toolbox.Distance(theoreticalOptimalPos, heatMap.GetFieldPosFromSubSampledHeatMapCoordinates(x, y)) / 20.0);
var heatMapPos = waypointHeatMap.GetFieldPosFromSubSampledHeatMapCoordinates(x, y, n);
double pen = CalculPenalisation(heatMapPos);
//double value = EvaluateStrategyCostFunction(robotRole, heatMapPos);
//heatMap.SubSampledHeatMapData1[y, x] = value;
int yBase = (int)(y * subSamplingRate);
int xBase = (int)(x * subSamplingRate);
double value = StrategyHeatmap.BaseHeatMapData[yBase, xBase] - pen;
waypointHeatMap.BaseHeatMapData[yBase, xBase] = value;
nbComputationsList[n]++;
if (value > max)
{
max = value;
maxXpos = xBase;
maxYpos = yBase;
}
//Code ci-dessous utile si on veut afficher la heatmap complete(video), mais consommateur en temps
for (int i = 0; i < waypointHeatMap.SubSamplingRateList[n]; i += 1)
{
for (int j = 0; j < waypointHeatMap.SubSamplingRateList[n]; j += 1)
{
if ((xBase + j < waypointHeatMap.nbCellInBaseHeatMapWidth) && (yBase + i < waypointHeatMap.nbCellInBaseHeatMapHeight))
{
waypointHeatMap.BaseHeatMapData[yBase + i, xBase + j] = value;
}
}
}
}
}
//OptimalPosInBaseHeatMapCoordinates = heatMap.GetMaxPositionInBaseHeatMapCoordinates();
OptimalPosInBaseHeatMapCoordinates = new PointD(maxXpos, maxYpos);
}
//var OptimalPosition = heatMap.GetMaxPositionInBaseHeatMap();
OptimalPosition = waypointHeatMap.GetFieldPosFromBaseHeatMapCoordinates(OptimalPosInBaseHeatMapCoordinates.X, OptimalPosInBaseHeatMapCoordinates.Y);
//var OptimalPosition = destinationLocation;
OnHeatMap(robotId, waypointHeatMap);
if (OptimalPosition != null)
{
SetNextWayPoint(new Location((float)OptimalPosition.X, (float)OptimalPosition.Y, 0, 0, 0, 0));
}
sw.Stop(); // Fin de la mesure
//for (int n = 0; n < nbComputationsList.Length; n++)
//{
// Console.WriteLine("Calcul WayPoint - Nb Calculs Etape " + n + " : " + nbComputationsList[n]);
//}
//Console.WriteLine("Temps de calcul de la heatMap WayPoint : " + sw.Elapsed.TotalMilliseconds.ToString("N4")+" ms"); // Affichage de la mesure
}
public void ProcessStrategy()
{
//TestGPU.ActionWithClosure();
sw.Reset();
sw.Start(); // début de la mesure
//Génération de la HeatMap
heatMap.InitHeatMapData();
//On construit le heatMap en mode multi-résolution :
//On commence par une heatmap très peu précise, puis on construit une heat map de taille réduite plus précise autour du point chaud,
//Puis on construit une heatmap très précise au cm autour du point chaud.
double optimizedAreaSize;
PointD OptimalPosition = new PointD(0, 0);
PointD OptimalPosInBaseHeatMapCoordinates = heatMap.GetBaseHeatMapPosFromFieldCoordinates(new PointD(0, 0));
ParallelCalculateHeatMap(heatMap.BaseHeatMapData, heatMap.nbCellInBaseHeatMapWidth, heatMap.nbCellInBaseHeatMapHeight, (float)heatMap.FieldLength, (float)heatMap.FieldHeight, (float)robotDestination.X, (float)robotDestination.Y);
//gpuDll.GpuGenerateHeatMap("GPU_DLL_CUDA.dll", heatMap.BaseHeatMapData, heatMap.nbCellInBaseHeatMapWidth, heatMap.nbCellInBaseHeatMapHeight, (float)heatMap.FieldLength, (float)heatMap.FieldHeight, (float)robotDestination.X, (float)robotDestination.Y);
//ParallelCalculateHeatMap(heatMap.BaseHeatMapData, heatMap.nbCellInBaseHeatMapWidth, heatMap.nbCellInBaseHeatMapHeight, (float)heatMap.FieldLength, (float)heatMap.FieldHeight, (float)robotDestination.X, (float)robotDestination.Y);
double[] tabMax = new double[heatMap.nbCellInBaseHeatMapHeight];
int[] tabIndexMax = new int[heatMap.nbCellInBaseHeatMapHeight];
Parallel.For(0, heatMap.nbCellInBaseHeatMapHeight, i =>
//for (int i =0; i< heatMap.nbCellInBaseHeatMapHeight;i++)
{
tabMax[i] = 0;
tabIndexMax[i] = 0;
for (int j = 0; j < heatMap.nbCellInBaseHeatMapWidth; j++)
{
if (heatMap.BaseHeatMapData[i, j] > tabMax[i])
{
tabMax[i] = heatMap.BaseHeatMapData[i, j];
tabIndexMax[i] = j;
}
}
});
//Recherche du maximum
double max = 0;
int indexMax = 0;
for (int i = 0; i < heatMap.nbCellInBaseHeatMapHeight; i++)
{
if (tabMax[i] > max)
{
max = tabMax[i];
indexMax = i;
}
}
int maxYpos = indexMax; // indexMax % heatMap.nbCellInBaseHeatMapWidth;
int maxXpos = tabIndexMax[indexMax]; // indexMax / heatMap.nbCellInBaseHeatMapWidth;
OptimalPosInBaseHeatMapCoordinates = new PointD(maxXpos, maxYpos);
OptimalPosition = heatMap.GetFieldPosFromBaseHeatMapCoordinates(OptimalPosInBaseHeatMapCoordinates.X, OptimalPosInBaseHeatMapCoordinates.Y);
//Si la position optimale est très de la cible théorique, on prend la cible théorique
double seuilPositionnementFinal = 0.1;
if (Toolbox.Distance(new PointD(robotDestination.X, robotDestination.Y), new PointD(OptimalPosition.X, OptimalPosition.Y)) < seuilPositionnementFinal)
{
OptimalPosition = robotDestination;
}
OnHeatMap(robotId, heatMap);
SetDestination(new Location((float)OptimalPosition.X, (float)OptimalPosition.Y, (float)robotOrientation, 0, 0, 0));
//heatMap.Dispose();
sw.Stop(); // Fin de la mesure
//for (int n = 0; n < nbComputationsList.Length; n++)
//{
// Console.WriteLine("Calcul Strategy - Nb Calculs Etape " + n + " : " + nbComputationsList[n]);
//}
//Console.WriteLine("Temps de calcul de la heatMap de stratégie : " + sw.Elapsed.TotalMilliseconds.ToString("N4")+" ms"); // Affichage de la mesure
}