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
}