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;
}
