private double FindClosesUndiscovered(Pose startPose) { List <Pose> candidates = new List <Pose>(); List <Pose> newCandidates = new List <Pose>(); int[,] distMap = Matrix.Create <int>(Platform.Map.Rows, Platform.Map.Columns, int.MaxValue); candidates.Add(startPose); int maxDeep = 1000; distMap[startPose.X, startPose.Y] = 0; for (int k = 1; k < maxDeep; k++) { newCandidates.Clear(); foreach (Pose cp in candidates) { RegionLimits limits = Platform.Map.CalculateLimits(cp.X, cp.Y, 1); List <Pose> poses = limits.GetPosesWithinLimits(); foreach (Pose p in poses) { if ((p.X == cp.X) && (p.Y == cp.Y)) { continue; } if (Platform.Map.GetPlace(p) == MapPlaceIndicator.Undiscovered) { return(k); } //if ((Platform.Map.GetPlace(p) == MapPlaceIndicator.Discovered) || (Platform.Map.GetPlace(p) == MapPlaceIndicator.Platform)) if ((Platform.Map.GetPlace(p) != MapPlaceIndicator.Obstacle) && (distMap[p.X, p.Y] == int.MaxValue)) { distMap[p.X, p.Y] = k; newCandidates.Add(p); } } } candidates = new List <Pose>(newCandidates); } return(int.MaxValue); }
public List <Tuple <int, Pose> > CalculateBinsInFOV(Pose pcent, MapObject map, double FOV = -1) { if (FOV < 0) { FOV = this.FieldOfViewRadius; } List <Pose> candidates = new List <Pose>(); List <Tuple <int, Pose> > visited = new List <Tuple <int, Pose> >(); List <Pose> newCandidates = new List <Pose>(); candidates.Add(pcent); for (int k = 0; k < FOV; k++) { newCandidates.Clear(); foreach (Pose cp in candidates) { RegionLimits limits = this.Map.CalculateLimits(cp.X, cp.Y, 1); List <Pose> poses = limits.GetPosesWithinLimits(); foreach (Pose p in poses) { if (visited.Exists(ps => ps.Item2.Equals(p))) { continue; } if ((p.X == cp.X) && (p.Y == cp.Y)) { continue; } visited.Add(new Tuple <int, Pose>(k, p)); if (map.MapMatrix[p.X, p.Y] < this.OccupiedThreshold) { newCandidates.Add(p); } } } candidates = new List <Pose>(newCandidates); } return(visited); }
public override void Next() { Platform.Measure(); RegionLimits limits = Platform.Map.CalculateLimits(Platform.Pose, 1); List <Pose> poses = limits.GetPosesWithinLimits(); //Remove those possible poses that are obstacles List <Pose> nextPoses = new List <Pose>(); foreach (Pose p in poses) { if ((Platform.Map.GetPlace(p) != MapPlaceIndicator.Obstacle) && (Platform.Map.GetPlace(p) != MapPlaceIndicator.Platform) && (Platform.Map.GetPlace(p) != MapPlaceIndicator.NoBackVisist)) { nextPoses.Add(p); } } // Find closest undiscovered point double minVal = Double.PositiveInfinity; Pose minPose = Platform.Pose; foreach (Pose p in nextPoses) { double cmin = FindClosesUndiscovered(p); if (cmin < minVal) { minVal = cmin; minPose = p; } } if (minVal == int.MaxValue) { Platform.SendLog("No undiscovered area!"); } Platform.Move(minPose.X - Platform.Pose.X, minPose.Y - Platform.Pose.Y); }