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