Exemplo n.º 1
0
        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
        }