public void GenerateSearchPathPolygon(GMapPolygon Polygon, PointLatLngAlt PosInitial) { double d = 10000000; PointLatLngAlt startingPoint = new PointLatLngAlt(); rectangle = new List <PointLatLngAlt>(); double maxLat = -1000; double minLat = 1000; double maxLon = -1000; double minLon = 1000; double pitchAngle = .13; for (int i = 0; i < Polygon.Points.Count; i++) { maxLat = Polygon.Points[i].Lat > maxLat ? Polygon.Points[i].Lat : maxLat; maxLon = Polygon.Points[i].Lng > maxLon ? Polygon.Points[i].Lng : maxLon; minLat = Polygon.Points[i].Lat < minLat ? Polygon.Points[i].Lat : minLat; minLon = Polygon.Points[i].Lng < minLon ? Polygon.Points[i].Lng : minLon; } rectangle.Add(new PointLatLngAlt(maxLat, maxLon, 100)); rectangle.Add(new PointLatLngAlt(maxLat, minLon, 100)); rectangle.Add(new PointLatLngAlt(minLat, minLon, 100)); rectangle.Add(new PointLatLngAlt(minLat, maxLon, 100)); rectangle.Sort((x, y) => x.Lng.CompareTo(y.Lng)); rectangle.Sort((x, y) => x.Lat.CompareTo(y.Lat)); PointLatLngAlt p_aux = Rectangle[2]; rectangle[2] = Rectangle[3]; rectangle[3] = p_aux; rectangle.Reverse(); foreach (PointLatLngAlt p in rectangle) { double d1 = mapTools.GetDistance(PosInitial, p); if (d1 < d) { d = d1; startingPoint = p; } } double distance = 0; double distance1 = mapTools.GetDistance(Rectangle[0], Rectangle[1]); double distance2 = mapTools.GetDistance(Rectangle[0], Rectangle[3]); bool right = true; if (distance1 > distance2) { distance = distance1; if (startingPoint.Equals(Rectangle[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, -turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, -turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } } else { distance = distance2; if (startingPoint.Equals(Rectangle[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, 1, 0);; pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, 1, 0); pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, -1, 0); pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, -1, 0); pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = true; } } pathGenerator.newPath(); pathGenerator.TurnRadius = turnRadius; pathGenerator.StraightLine(pathGenerator.TurnRadius); while (!mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } pathGenerator.newPath(); while (mapTools.IsInside(pathGenerator.InitialPosition, Rectangle)) { if (right) { pathGenerator.InitialPitch = pitchAngle; } else { pathGenerator.InitialPitch = pitchAngle; } while (mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } //pathGenerator.StraightLine(distance * 1000 - 2 * pathGenerator.TurnRadius); pathGenerator.Turn(right ? Navigation.PathGenerator3D.Direction.right : Navigation.PathGenerator3D.Direction.left, Math.PI); right = !right; if (right) { pathGenerator.InitialPitch = pitchAngle; } else { pathGenerator.InitialPitch = pitchAngle; } pathGenerator.StraightLine(pathGenerator.TurnRadius); while (!mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points) && mapTools.IsInside(pathGenerator.InitialPosition, Rectangle)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } } pointListObjectives = pathGenerator.RoutePoints; cCurvature = pathGenerator.cCurvature; // Curvatura de curva //arcLength = new List<double>(); // pathGenerator.ArcLength; // Arc-length //currentArcLength = 0; ResetArc(); psi_f = pathGenerator.Psi_f; // Angulo tangencial con respecto a la normal. pointListObjectives_o = pathGenerator.RoutePoints; cCurvature_o = pathGenerator.cCurvature; // Curvatura de curva //arcLength_o = pathGenerator.ArcLength; // Arc-length psi_f_o = pathGenerator.Psi_f; // Angulo tangencial con respecto a la normal. //((Map)parent.GetDocks[typeof(Map).ToString()]).AddRoute(1, pathGenerator.Route, Color.Black); }
public void GenerateSearchPathPolygon(GMapPolygon Polygon, PointLatLngAlt PosInitial, double Velocity) { double d = 10000000; PointLatLngAlt startingPoint = new PointLatLngAlt(); rectangle = new List <PointLatLngAlt>(); double maxLat = -1000; double minLat = 1000; double maxLon = -1000; double minLon = 1000; double pitchAngle = .13; for (int i = 0; i < Polygon.Points.Count; i++) { maxLat = Polygon.Points[i].Lat > maxLat ? Polygon.Points[i].Lat : maxLat; maxLon = Polygon.Points[i].Lng > maxLon ? Polygon.Points[i].Lng : maxLon; minLat = Polygon.Points[i].Lat < minLat ? Polygon.Points[i].Lat : minLat; minLon = Polygon.Points[i].Lng < minLon ? Polygon.Points[i].Lng : minLon; } rectangle.Add(new PointLatLngAlt(maxLat, maxLon, 100)); rectangle.Add(new PointLatLngAlt(maxLat, minLon, 100)); rectangle.Add(new PointLatLngAlt(minLat, minLon, 100)); rectangle.Add(new PointLatLngAlt(minLat, maxLon, 100)); rectangle.Sort((x, y) => x.Lng.CompareTo(y.Lng)); rectangle.Sort((x, y) => x.Lat.CompareTo(y.Lat)); PointLatLngAlt p_aux = Rectangle[2]; rectangle[2] = Rectangle[3]; rectangle[3] = p_aux; rectangle.Reverse(); foreach (PointLatLngAlt p in rectangle) { double d1 = mapTools.GetDistance(PosInitial, p); if (d1 < d) { d = d1; startingPoint = p; } } double distance = 0; double distance1 = mapTools.GetDistance(Rectangle[0], Rectangle[1]); double distance2 = mapTools.GetDistance(Rectangle[0], Rectangle[3]); bool right = true; if (distance1 > distance2) { distance = distance1; if (startingPoint.Equals(Rectangle[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, -turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, -turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } } else { distance = distance2; if (startingPoint.Equals(Rectangle[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, 1, 0);; pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, 1, 0); pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, -1, 0); pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, -1, 0); pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = true; } } pathGenerator.newPath(); pathGenerator.TurnRadius = turnRadius; pathGenerator.CurrentVelocity = Velocity; pathGenerator.StraightLine(pathGenerator.TurnRadius); while (!mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } pathGenerator.newPath(); while (mapTools.IsInside(pathGenerator.InitialPosition, Rectangle)) { if (right) { pathGenerator.InitialPitch = pitchAngle; } else { pathGenerator.InitialPitch = pitchAngle; } while (mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } pathGenerator.Turn(right ? Navigation.PathGenerator4D.Direction.right : Navigation.PathGenerator4D.Direction.left, Math.PI); right = !right; if (right) { pathGenerator.InitialPitch = pitchAngle; } else { pathGenerator.InitialPitch = pitchAngle; } pathGenerator.StraightLine(pathGenerator.TurnRadius); while (!mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Polygon.Points) && mapTools.IsInside(pathGenerator.InitialPosition, Rectangle)) { pathGenerator.StraightLine(pathGenerator.TurnRadius); } } currentPath = pathGenerator.Path; ResetArc(); originalPath = currentPath; }
public void GenerateSearchPath(GMapPolygon Rectangle, PointLatLngAlt PosInitial) { double d = 10000000; PointLatLngAlt startingPoint = new PointLatLngAlt(); List <PointLatLngAlt> points = new List <PointLatLngAlt>(); points.Add(new PointLatLngAlt(Rectangle.Points[0].Lat, Rectangle.Points[0].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[1].Lat, Rectangle.Points[1].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[2].Lat, Rectangle.Points[2].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[3].Lat, Rectangle.Points[3].Lng, PosInitial.Alt)); foreach (PointLatLngAlt p in points) { double d1 = mapTools.GetDistance(PosInitial, p); if (d1 < d) { d = d1; startingPoint = p; } } double distance = 0; double distance1 = mapTools.GetDistance(Rectangle.Points[0], Rectangle.Points[1]); double distance2 = mapTools.GetDistance(Rectangle.Points[0], Rectangle.Points[3]); bool right = true; if (distance1 > distance2) { distance = distance1; if (startingPoint.Equals(Rectangle.Points[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, -turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, -turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } } else { distance = distance2; if (startingPoint.Equals(Rectangle.Points[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, 1, 0);; pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, 1, 0); pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, -1, 0);; pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, -1, 0);; pathGenerator.InitialPitch = 0; pathGenerator.InitialYaw = 0; right = true; } } pathGenerator.newPath(); pathGenerator.TurnRadius = turnRadius; pathGenerator.StraightLine(pathGenerator.TurnRadius); while (mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Rectangle.Points)) { if (right) { pathGenerator.InitialPitch = 0.26; } else { pathGenerator.InitialPitch = -0.26; } pathGenerator.StraightLine(distance * 1000 - 2 * pathGenerator.TurnRadius); pathGenerator.InitialPitch = 0; pathGenerator.Turn(right ? Navigation.PathGenerator3D.Direction.right : Navigation.PathGenerator3D.Direction.left, Math.PI); right = !right; } pointListObjectives = pathGenerator.RoutePoints; cCurvature = pathGenerator.cCurvature; // Curvatura de curva //arcLength = new List<double>(); // pathGenerator.ArcLength; // Arc-length //currentArcLength = 0; ResetArc(); psi_f = pathGenerator.Psi_f; // Angulo tangencial con respecto a la normal. pointListObjectives_o = pathGenerator.RoutePoints; cCurvature_o = pathGenerator.cCurvature; // Curvatura de curva //arcLength_o = pathGenerator.ArcLength; // Arc-length psi_f_o = pathGenerator.Psi_f; // Angulo tangencial con respecto a la normal. //((Map)parent.GetDocks[typeof(Map).ToString()]).AddRoute(1, pathGenerator.Route, Color.Black); }
public void GenerateSearchPath(GMapPolygon Rectangle, PointLatLngAlt PosInitial, double Velocity) { double d = 10000000; PointLatLngAlt startingPoint = new PointLatLngAlt(); List <PointLatLngAlt> points = new List <PointLatLngAlt>(); points.Add(new PointLatLngAlt(Rectangle.Points[0].Lat, Rectangle.Points[0].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[1].Lat, Rectangle.Points[1].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[2].Lat, Rectangle.Points[2].Lng, PosInitial.Alt)); points.Add(new PointLatLngAlt(Rectangle.Points[3].Lat, Rectangle.Points[3].Lng, PosInitial.Alt)); foreach (PointLatLngAlt p in points) { double d1 = mapTools.GetDistance(PosInitial, p); if (d1 < d) { d = d1; startingPoint = p; } } double distance = 0; double distance1 = mapTools.GetDistance(Rectangle.Points[0], Rectangle.Points[1]); double distance2 = mapTools.GetDistance(Rectangle.Points[0], Rectangle.Points[3]); bool right = true; if (distance1 > distance2) { distance = distance1; if (startingPoint.Equals(Rectangle.Points[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, -turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2;; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, -turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -1, turnRadius, 0); pathGenerator.InitialYaw = -Math.PI / 2; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, 1, turnRadius, 0); pathGenerator.InitialYaw = Math.PI / 2; pathGenerator.InitialPitch = 0; right = false; } } else { distance = distance2; if (startingPoint.Equals(Rectangle.Points[0])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, 1, 0);; pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[1])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, 1, 0); pathGenerator.InitialYaw = Math.PI; pathGenerator.InitialPitch = 0; right = true; } if (startingPoint.Equals(Rectangle.Points[2])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, -turnRadius, -1, 0);; pathGenerator.InitialYaw = 0; pathGenerator.InitialPitch = 0; right = false; } if (startingPoint.Equals(Rectangle.Points[3])) { pathGenerator.InitialPosition = mapTools.OffsetInMeters(startingPoint, turnRadius, -1, 0);; pathGenerator.InitialPitch = 0; pathGenerator.InitialYaw = 0; right = true; } } pathGenerator.newPath(); pathGenerator.CurrentVelocity = Velocity; pathGenerator.TurnRadius = turnRadius; pathGenerator.StraightLine(pathGenerator.TurnRadius); while (mapTools.IsInside(pathGenerator.InitialPosition.GetPointLatLng(), Rectangle.Points)) { if (right) { pathGenerator.InitialPitch = 0.26; } else { pathGenerator.InitialPitch = -0.26; } pathGenerator.StraightLine(distance * 1000 - 2 * pathGenerator.TurnRadius); pathGenerator.InitialPitch = 0; pathGenerator.Turn(right ? Navigation.PathGenerator4D.Direction.right : Navigation.PathGenerator4D.Direction.left, Math.PI); right = !right; } currentPath = pathGenerator.Path; ResetArc(); originalPath = pathGenerator.Path; }