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); }
public void SetPsigUhatSquare(double x, double y, double value) { psig_u_hat_square.SetCell(x, y, value); }
public void SetPuHatSquare(double x, double y, double value) { pu_hat_square.SetCell(x, y, value); }
public void SetPuHat(double x, double y, double value) { pu_hat.SetCell(x, y, value); }
public void SetPijSum(double x, double y, double value) { pij_sum.SetCell(x, y, value); }
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); }