public static NonholonomiHeuristicInfo Calculate(float neighborhoodSize, float cellSize, int numOrientations)
        {
            orientationSize = MathHelper.TwoPi / numOrientations;
            numCells        = (int)Math.Ceiling(neighborhoodSize / cellSize);
            if (numCells % 2 == 0)
            {
                numCells++;
            }

            offset = (float)Math.Floor(numCells / 2f) * cellSize;
            Pose goal = new Pose(0f, 0f, 0f);

            float[, ,] heur = new float[numCells, numCells, numOrientations];
            int   totalCells   = numCells * numCells * numOrientations;
            int   count        = 0;
            float maxHeuristic = float.MinValue;

            for (int c = 0; c < numCells; c++)
            {
                for (int r = 0; r < numCells; r++)
                {
                    for (int o = 0; o < numOrientations; o++)
                    {
                        Pose pose = new Pose(c * cellSize - offset, r * cellSize - offset, o * orientationSize);
                        ReedsSheppActionSet actions = ReedsSheppSolver.Solve(pose, goal, VehicleModel.TurnRadius);
                        heur[c, r, o] = actions.CalculateCost(VehicleModel.TurnRadius, 1f, 0f);
                        if (actions.Length > maxHeuristic)
                        {
                            maxHeuristic = actions.Length;
                        }
                        count++;

                        if (CellCalculated != null)
                        {
                            CellCalculated(null, new CalculatedArgs()
                            {
                                C        = c,
                                R        = r,
                                O        = o,
                                Progress = (float)count / totalCells
                            });
                        }
                    }
                }
            }

            return(new NonholonomiHeuristicInfo()
            {
                CellSize = cellSize,
                NumCells = numCells,
                OrientationSize = orientationSize,
                NumOrientations = numOrientations,
                Heuristic = heur,
                MaxHeuristic = maxHeuristic
            });
        }
Exemple #2
0
 private void calcRS()
 {
     maxLength = float.MinValue;
     paths     = new ArrayList <ArrayList <Pose> >();
     actions   = new ArrayList <ReedsSheppActionSet>();
     for (int x = -cells / 2; x <= cells / 2; x++)
     {
         for (int y = -cells / 2; y <= cells / 2; y++)
         {
             Pose start = new Pose(x * cellSize, y * cellSize, orientation);
             ReedsSheppActionSet actionSet = ReedsSheppSolver.Solve(start, goal, VehicleModel.TurnRadius);
             actionSet.Length = actionSet.CalculateCost(VehicleModel.TurnRadius, HybridAStar.reverseFactor, HybridAStar.switchPenalty);
             if (actionSet.Length > maxLength)
             {
                 maxLength = actionSet.Length;
             }
             actions.Add(actionSet);
             paths.Add(ReedsSheppDriver.Discretize(start, actionSet, VehicleModel.TurnRadius, 1f));
         }
     }
 }
Exemple #3
0
        private static Node getReedsSheppChild(Pose pose, Pose goal)
        {
            ReedsSheppActionSet actions = ReedsSheppSolver.Solve(pose, goal, VehicleModel.TurnRadius);
            bool safe = true;

            foreach (Pose s in ReedsSheppDriver.Discretize(pose, actions, VehicleModel.TurnRadius, GridResolution))
            {
                if (!grid.IsPointInGrid(s.Position) || !grid.IsSafe(s, SafetyFactor))
                {
                    safe = false;
                    break;
                }
            }

            if (safe)
            {
                return(new Node(Cell.None, actions, goal, 0, 0, null));
            }

            return(null);
        }