public List <PointLatLngAlt> GeneratePath(PointLatLngAlt InitialPosition, double InitialYaw, PointLatLngAlt FinalPosition, double FinalYaw)
{
//Seleccionar la distancia menor de entre las circunferencias generadas
DubinsPath dubinsPath = DubinsPath.None;
List <PointLatLngAlt> PathPoints = new List <PointLatLngAlt>();
if (FinalYaw > Math.PI)
{
FinalYaw -= 2 * Math.PI;
}
else if (FinalYaw < -Math.PI)
{
FinalYaw += 2 * Math.PI;
}
PointLatLngAlt Ci1 = mapTools.OffsetInMeters(InitialPosition, Math.Cos(InitialYaw) * turnRadius, -Math.Sin(InitialYaw) * turnRadius, 0); // Right
PointLatLngAlt Ci2 = mapTools.OffsetInMeters(InitialPosition, -Math.Cos(InitialYaw) * turnRadius, Math.Sin(InitialYaw) * turnRadius, 0); // Left
PointLatLngAlt Cf1 = mapTools.OffsetInMeters(FinalPosition, Math.Cos(FinalYaw) * turnRadius, -Math.Sin(FinalYaw) * turnRadius, 0); // Right
PointLatLngAlt Cf2 = mapTools.OffsetInMeters(FinalPosition, -Math.Cos(FinalYaw) * turnRadius, Math.Sin(FinalYaw) * turnRadius, 0); // Left
PathPoints.Add(Ci1);
PathPoints.Add(Ci2);
PathPoints.Add(Cf1);
PathPoints.Add(Cf2);
List <double> listD = new List <double>();
listD.Add(mapTools.GetDistanceMeters(Ci1, Cf1));
listD.Add(mapTools.GetDistanceMeters(Ci2, Cf1));
listD.Add(mapTools.GetDistanceMeters(Ci1, Cf2));
listD.Add(mapTools.GetDistanceMeters(Ci2, Cf2));
int index = -1;
double minDistance = 10000000;
int index2 = -1;
double minDistance2 = 10000000;
foreach (double distance in listD)
{
if (distance < minDistance)
{
index = listD.IndexOf(distance);
minDistance = distance;
}
}
foreach (double distance in listD)
{
if (distance < minDistance2 && listD.IndexOf(distance) != index)
{
index2 = listD.IndexOf(distance);
minDistance2 = distance;
}
}
switch (index)
{
case 0:
PathPoints.Add(Ci1);
PathPoints.Add(Cf1);
dubinsPath = DubinsPath.RSR;
break;
case 1:
PathPoints.Add(Ci2);
PathPoints.Add(Cf1);
dubinsPath = DubinsPath.LSR;
break;
case 2:
PathPoints.Add(Ci1);
PathPoints.Add(Cf2);
dubinsPath = DubinsPath.RSL;
break;
case 3:
PathPoints.Add(Ci2);
PathPoints.Add(Cf2);
dubinsPath = DubinsPath.LSL;
break;
}
if (dubinsPath == DubinsPath.LSL)
{
this.initialPosition = InitialPosition;
this.initialYaw = InitialYaw;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope;
this.initialPitch = FindPitch(InitialPosition, FinalPosition, InitialYaw, finalHeading, FinalYaw, mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
TurnUntil(Direction.left, finalHeading);
StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
if (finalHeading < FinalYaw)
{
TurnUntil(Direction.right, FinalYaw);
}
else
{
TurnUntil(Direction.left, FinalYaw);
}
}
else if (dubinsPath == DubinsPath.LSR)
{
this.initialPosition = InitialPosition;
this.initialYaw = InitialYaw;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope + angle - Math.PI / 2;
this.initialPitch = FindPitch(InitialPosition, FinalPosition, InitialYaw, finalHeading, FinalYaw, Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.left, finalHeading);
StraightLine(Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.right, FinalYaw);
}
else if (dubinsPath == DubinsPath.RSR)
{
this.initialPosition = InitialPosition;
this.initialYaw = InitialYaw;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope;
this.initialPitch = FindPitch(InitialPosition, FinalPosition, InitialYaw, finalHeading, FinalYaw, mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
TurnUntil(Direction.right, finalHeading);
StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
TurnUntil(Direction.right, FinalYaw);
}
else if (dubinsPath == DubinsPath.RSL)
{
this.initialPosition = InitialPosition;
this.initialYaw = InitialYaw;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope - angle + Math.PI / 2;
this.initialPitch = FindPitch(InitialPosition, FinalPosition, InitialYaw, finalHeading, FinalYaw, Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.right, finalHeading);
StraightLine(Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.left, FinalYaw);
}
return(PathPoints);
}
public List <PointLatLng> GeneratePath(PointLatLng InitialPosition, double InitialHeading, PointLatLng FinalPosition, double FinalHeading)
{
//Seleccionar la distancia menor de entre las circunferencias generadas
DubinsPath dubinsPath = DubinsPath.None;
List <PointLatLng> PathPoints = new List <PointLatLng>();
if (FinalHeading > Math.PI)
{
FinalHeading -= 2 * Math.PI;
}
else if (FinalHeading < -Math.PI)
{
FinalHeading += 2 * Math.PI;
}
PointLatLng Ci1 = mapTools.OffsetInMeters(InitialPosition, Math.Cos(InitialHeading) * turnRadius, -Math.Sin(InitialHeading) * turnRadius); // Right
PointLatLng Ci2 = mapTools.OffsetInMeters(InitialPosition, -Math.Cos(InitialHeading) * turnRadius, Math.Sin(InitialHeading) * turnRadius); // Left
PointLatLng Cf1 = mapTools.OffsetInMeters(FinalPosition, Math.Cos(FinalHeading) * turnRadius, -Math.Sin(FinalHeading) * turnRadius); // Right
PointLatLng Cf2 = mapTools.OffsetInMeters(FinalPosition, -Math.Cos(FinalHeading) * turnRadius, Math.Sin(FinalHeading) * turnRadius); // Left
PathPoints.Add(Ci1);
PathPoints.Add(Ci2);
PathPoints.Add(Cf1);
PathPoints.Add(Cf2);
List <double> listD = new List <double>();
listD.Add(mapTools.GetDistanceMeters(Ci1, Cf1));
listD.Add(mapTools.GetDistanceMeters(Ci2, Cf1));
listD.Add(mapTools.GetDistanceMeters(Ci1, Cf2));
listD.Add(mapTools.GetDistanceMeters(Ci2, Cf2));
int index = -1;
double minDistance = 10000000;
int index2 = -1;
double minDistance2 = 10000000;
foreach (double distance in listD)
{
if (distance < minDistance)
{
index = listD.IndexOf(distance);
minDistance = distance;
}
}
foreach (double distance in listD)
{
if (distance < minDistance2 && listD.IndexOf(distance) != index)
{
index2 = listD.IndexOf(distance);
minDistance2 = distance;
}
}
switch (index)
{
case 0:
PathPoints.Add(Ci1);
PathPoints.Add(Cf1);
dubinsPath = DubinsPath.RSR;
break;
case 1:
PathPoints.Add(Ci2);
PathPoints.Add(Cf1);
dubinsPath = DubinsPath.LSR;
break;
case 2:
PathPoints.Add(Ci1);
PathPoints.Add(Cf2);
dubinsPath = DubinsPath.RSL;
break;
case 3:
PathPoints.Add(Ci2);
PathPoints.Add(Cf2);
dubinsPath = DubinsPath.LSL;
break;
}
#region old
//if (dubinsPath == DubinsPath.LSL)
//{
// this.initialPosition = InitialPosition;
// this.initialHeading = InitialHeading;
// newPath();
// double slope = mapTools.GetSlopeFromNord(PathPoints[4], PathPoints[5]);
// double angle2turn = -InitialHeading + Math.PI / 2 - slope;
// double angle2turn2 = Math.Atan2(Math.Sin(slope), Math.Cos(slope)) - Math.Atan2(Math.Sin(InitialHeading), Math.Cos(InitialHeading));
// double angle2turn3 = slope - InitialHeading;
// if (Math.Sign(InitialHeading) != Math.Sign(slope))
// {
// if (Math.Sign(slope) > 0)
// {
// angle2turn = angle2turn - 2 * Math.PI;
// if (angle2turn > 3 * Math.PI/2)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < 3 * Math.PI / 2)
// angle2turn += 2 * Math.PI;
// }
// else
// {
// //double initial = InitialHeading + Math.PI;
// angle2turn = angle2turn - 2 * Math.PI;
// if (angle2turn > 2 * Math.PI)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < -2 * Math.PI)
// angle2turn += 2 * Math.PI;
// }
// }
// else
// {
// if (angle2turn > Math.PI)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < -Math.PI)
// angle2turn += 2 * Math.PI;
// }
// Turn(MNAV3D.Controler.PathGenerator.Direction.left, Math.Abs(angle2turn));
// StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
// //double vectorSlope = FinalHeading;
// //double finalTurn = this.initialHeading - vectorSlope;
// //Turn(MNAV3D.Controler.PathGenerator.Direction.left, finalTurn);
//}
//else if (dubinsPath == DubinsPath.LSR)
//{
// this.initialPosition = InitialPosition;
// this.initialHeading = InitialHeading;
// newPath();
// double angle = Math.Acos(turnRadius * 2 / minDistance);
// double slope2 = (mapTools.GetSlopeFromNord(PathPoints[4], PathPoints[5]));
// double angle2turn = InitialHeading + Math.PI / 2 + slope2 - Math.PI + Math.PI / 2 - angle;
// angle2turn = InitialHeading + slope2 - angle;
// if (Math.Sign(InitialHeading) != Math.Sign(slope2 + angle))
// {
// if (Math.Sign(slope2 + angle) > 0)
// {
// //double initial = InitialHeading + Math.PI;
// angle2turn = InitialHeading + slope2 - angle + 2 * Math.PI;
// if (angle2turn > 2 * Math.PI)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < -2 * Math.PI)
// angle2turn += 2 * Math.PI;
// }
// else
// {
// //double initial = InitialHeading - Math.PI;
// angle2turn = InitialHeading + slope2 - angle + 2 * Math.PI;
// if(angle2turn > 2*Math.PI)
// angle2turn -= 2*Math.PI;
// else if(angle2turn < -2*Math.PI)
// angle2turn += 2*Math.PI;
// }
// }
// else
// {
// if (angle2turn > 3*Math.PI/2)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < 3 * Math.PI / 2)
// angle2turn += 2 * Math.PI;
// }
// Turn(MNAV3D.Controler.PathGenerator.Direction.left, Math.Abs(angle2turn));
// StraightLine(Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
// //double vectorSlope = FinalHeading;
// //double finalTurn = this.initialHeading - vectorSlope;
// //Turn(MNAV3D.Controler.PathGenerator.Direction.right, -finalTurn);
//}
//else if (dubinsPath == DubinsPath.RSR)
//{
// this.initialPosition = InitialPosition;
// this.initialHeading = InitialHeading;
// newPath();
// double slope2 = Math.PI / 2 - (mapTools.GetSlope(PathPoints[4], PathPoints[5]));
// if (slope2 > Math.PI)
// slope2 -= 2 * Math.PI;
// else if (slope2 < -Math.PI)
// slope2 += 2 * Math.PI;
// double angle2turn = -InitialHeading + Math.PI / 2 - slope2;
// if (angle2turn > Math.PI)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < -Math.PI)
// angle2turn += 2 * Math.PI;
// Turn(MNAV3D.Controler.PathGenerator.Direction.right, Math.Abs(angle2turn));
// StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
// double vectorSlope = FinalHeading;
// double finalTurn = this.initialHeading - vectorSlope;
// if (finalTurn > Math.PI || (this.initialHeading > 0 && vectorSlope > 0 && finalTurn > Math.PI))
// finalTurn -= 2 * Math.PI;
// else if (finalTurn < -Math.PI)
// finalTurn += 2 * Math.PI;
// Turn(MNAV3D.Controler.PathGenerator.Direction.right, Math.Abs(finalTurn));
//}
//else if (dubinsPath == DubinsPath.RSL)
//{
// this.initialPosition = InitialPosition;
// this.initialHeading = InitialHeading;
// newPath();
// double angle = Math.Acos(turnRadius * 2 / minDistance);
// double slope2 = Math.PI / 2 - (mapTools.GetSlope(PathPoints[4], PathPoints[5]));
// if (slope2 > Math.PI)
// slope2 -= 2 * Math.PI;
// else if (slope2 < -Math.PI)
// slope2 += 2 * Math.PI;
// double angle2turn = -InitialHeading + Math.PI / 2 - slope2 + Math.PI/2 - angle;
// if (angle2turn > Math.PI)
// angle2turn -= 2 * Math.PI;
// else if (angle2turn < -Math.PI)
// angle2turn += 2 * Math.PI;
// Turn(MNAV3D.Controler.PathGenerator.Direction.right, Math.Abs(angle2turn));
// StraightLine(Math.Sqrt(Math.Pow(minDistance,2) - Math.Pow(turnRadius*2,2)));
// double vectorSlope = FinalHeading;
// double finalTurn = this.initialHeading - vectorSlope;
// if (finalTurn > 5 * Math.PI / 4 || (this.initialHeading < 0 && vectorSlope > 0 && finalTurn > 0) || (this.initialHeading < 0 && vectorSlope < 0 && finalTurn > Math.PI/2))
// finalTurn -= 2 * Math.PI;
// else if (finalTurn < -5 * Math.PI / 4 || (this.initialHeading < 0 && vectorSlope > 0 && finalTurn < 0) || (this.initialHeading < 0 && vectorSlope < 0 && finalTurn < -Math.PI/2))
// finalTurn += 2 * Math.PI;
// Turn(MNAV3D.Controler.PathGenerator.Direction.left, Math.Abs(finalTurn));
//}
#endregion
if (dubinsPath == DubinsPath.LSL)
{
this.initialPosition = InitialPosition;
this.initialHeading = InitialHeading;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope;
TurnUntil(Direction.left, finalHeading);
StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
if (finalHeading < FinalHeading)
{
TurnUntil(Direction.right, FinalHeading);
}
else
{
TurnUntil(Direction.left, FinalHeading);
}
}
else if (dubinsPath == DubinsPath.LSR)
{
this.initialPosition = InitialPosition;
this.initialHeading = InitialHeading;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope + angle - Math.PI / 2;
TurnUntil(Direction.left, finalHeading);
StraightLine(Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.right, FinalHeading);
}
else if (dubinsPath == DubinsPath.RSR)
{
this.initialPosition = InitialPosition;
this.initialHeading = InitialHeading;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope;
TurnUntil(Direction.right, finalHeading);
StraightLine(mapTools.GetDistanceMeters(PathPoints[4], PathPoints[5]));
TurnUntil(Direction.right, FinalHeading);
}
else if (dubinsPath == DubinsPath.RSL)
{
this.initialPosition = InitialPosition;
this.initialHeading = InitialHeading;
double slope = mapTools.GetAngleMeassuredFromNord(PathPoints[4], PathPoints[5]);
double angle = Math.Acos(turnRadius * 2 / minDistance);
double finalHeading = slope - angle + Math.PI / 2;
TurnUntil(Direction.right, finalHeading);
StraightLine(Math.Sqrt(Math.Pow(minDistance, 2) - Math.Pow(turnRadius * 2, 2)));
TurnUntil(Direction.left, FinalHeading);
}
return(PathPoints);
}
