Exemplo n.º 1
0
        public void UpdateHeight(double x, double y)
        {
            double largeU   = pu_hat.GetCell(x, y) / pij_sum.GetCell(x, y);
            double largeSig = (psig_u_hat_square.GetCell(x, y) + pu_hat_square.GetCell(x, y)) / pij_sum.GetCell(x, y) - largeU * largeU;

            uhatGM.SetCell(x, y, largeU);
            sigSqrGM.SetCell(x, y, largeSig);
        }
Exemplo n.º 2
0
 public void SetPsigUhatSquare(double x, double y, double value)
 {
     psig_u_hat_square.SetCell(x, y, value);
 }
Exemplo n.º 3
0
 public void SetPuHatSquare(double x, double y, double value)
 {
     pu_hat_square.SetCell(x, y, value);
 }
Exemplo n.º 4
0
 public void SetPuHat(double x, double y, double value)
 {
     pu_hat.SetCell(x, y, value);
 }
Exemplo n.º 5
0
 public void SetPijSum(double x, double y, double value)
 {
     pij_sum.SetCell(x, y, value);
 }
Exemplo n.º 6
0
        public IPath GetPathToGoal(IPath[] otherPaths, out IPath sparsePath, out bool inObstacle, out bool wpInObstacle)
        {
            inObstacle   = false;
            wpInObstacle = false;
            sparsePath   = null;

            if (waypoints == null)
            {
                return(null);
            }
            if (waypointsAchieved >= waypoints.Count)
            {
                return(null);
            }
            if (og == null)
            {
                return(null);
            }
            if (bloatedObstacles == null)
            {
                Console.WriteLine("Convolved obstacles fail!");
                return(null);
            }

            // Add current pose position to filteredWaypoints. (FilteredWaypoints is what we will be
            // planning over. Waypoints is list of user given waypoints.)
            List <Waypoint> filteredWaypoints = new List <Waypoint>();

            filteredWaypoints.Add(new Waypoint(pose.ToVector2(), true, 0));

            lock (ogLock)
            {
                if (og.GetCell(pose.x, pose.y) == 255)
                {
                    inObstacle = true;
                    og.SetCell(pose.x, pose.y, 0);
                }
            }

            // Only add user waypoints to PathPoints if they have not been achieved yet.
            // If they are inside an obstacle, return a null path.
            foreach (Waypoint wp in waypoints)
            {
                if (!wp.Achieved)
                {
                    lock (bloatedObstacleLock)
                    {
                        foreach (Polygon p in bloatedObstacles)
                        {
                            if (p.IsInside(wp.Coordinate))
                            {
                                wpInObstacle = true;
                                return(null);
                            }
                        }
                    }
                    filteredWaypoints.Add(wp);
                }
            }

            // Plan each segment of the filteredWaypoints individually and stitch them together.
            List <Waypoint> completeRawPath = new List <Waypoint>();

            for (int i = 0; i < filteredWaypoints.Count - 1; i++)
            {
                bool            pathOk;
                List <Waypoint> rawPath;

                lock (ogLock)
                {
                    rawPath = dstar.FindPath(filteredWaypoints[i], filteredWaypoints[i + 1], og, out pathOk);
                }

                if (!pathOk)
                {
                    return(null);
                }
                else
                {
                    int j;

                    // We want to only add the first path waypoint if it is the first
                    // segment of the whole path.
                    if (i == 0)
                    {
                        j = 0;
                    }
                    else
                    {
                        j = 1;
                    }

                    for (; j < rawPath.Count; j++)
                    {
                        completeRawPath.Add(rawPath.ElementAt(j));
                    }
                }
            }

            if (completeRawPath.Count > 0)
            {
                IPath     bezierPath;
                PointPath segmentedBezierPath;

                lock (bloatedObstacleLock)
                {
                    bezierPath = smoother.Wp2BezierPath(completeRawPath, bloatedObstacles, collapseThreshold);
                }

                sparsePath          = new PointPath(completeRawPath);
                segmentedBezierPath = smoother.ConvertBezierPathToPointPath(bezierPath, bezierSegmentation);

                // We want to check our sparse path against other sparse paths to see
                // if there are any intersections. If there are, we want to know if the
                // intersections will be a problem. If it is a problem, bloat that sparse path,
                // add it to obstacles, and replan.
                for (int i = 0; i < otherPaths.Length; i++)
                {
                    if (otherPaths[i] != null && otherPaths[i].Count > 0 && i + 1 != robotID)
                    {
                        double intersectDist = DoPathsCollide(sparsePath, otherPaths[i]);

                        if (intersectDist != -1)
                        {
                            PointOnPath collisionPt;
                            int         collisionSegmentIndex;

                            double tempDist = intersectDist - collideBackoff;
                            tempDist = Math.Max(0, tempDist);

                            collisionPt           = segmentedBezierPath.AdvancePoint(segmentedBezierPath.StartPoint, ref tempDist);
                            collisionSegmentIndex = segmentedBezierPath.IndexOf(collisionPt.segment);

                            segmentedBezierPath.RemoveRange(collisionSegmentIndex, segmentedBezierPath.Count - collisionSegmentIndex);

                            tempDist = intersectDist - collideBackoff;
                            tempDist = Math.Max(0, tempDist);

                            collisionPt           = sparsePath.AdvancePoint(sparsePath.StartPoint, ref tempDist);
                            collisionSegmentIndex = sparsePath.IndexOf(collisionPt.segment);

                            sparsePath.RemoveRange(collisionSegmentIndex, sparsePath.Count - collisionSegmentIndex - 1);
                            sparsePath[collisionSegmentIndex].End = collisionPt.pt;
                        }
                    }
                }

                return(segmentedBezierPath);
            }

            return(null);
        }