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 }