public static ReedsSheppActionSet Solve(Pose start, Pose goal, float unit)
{
// Translate the goal so that the start position is at the origin
Pose newGoal = new Pose((goal.Position - start.Position) / unit, MathHelper.WrapAngle(goal.Orientation - start.Orientation));
// Rotate the goal so that the start orientation is 0
newGoal.Position = Vector2.Transform(newGoal.Position, Matrix.CreateRotationZ(-start.Orientation));
float bestPathLength = float.PositiveInfinity;
PathWords bestWord = 0;
float t = 0, u = 0, v = 0, _t, _u, _v;
for (int w = 0; w < NumPathWords; w++)
{
PathWords word = (PathWords)w;
float potentialLength = CalculatePathLength(newGoal, word, out _t, out _u, out _v);
if (potentialLength < bestPathLength)
{
bestPathLength = potentialLength;
bestWord = word;
t = _t;
u = _u;
v = _v;
}
}
if (float.IsPositiveInfinity(bestPathLength))
{
return(new ReedsSheppActionSet(float.PositiveInfinity));
}
return(GetPath(bestWord, t, u, v));
}
//Loop through all paths and find the shortest one (if one can be found)
private static float FindShortestPathLength(RSCar carEndMod, out PathSegmentLengths bestPathLengths, out PathWords bestWord)
{
//How many paths are we going to check
int numPathWords = Enum.GetNames(typeof(PathWords)).Length;
//Better than using float.MaxValue because we can use float.IsPositiveInfinity(bestPathLength) to test if its infinity
float shortestPathLength = float.PositiveInfinity;
bestWord = 0;
//Will keep track of the length of the best path
//Some Reeds-Shepp segments have 5 lengths, but 2 of those are known, so we only need 3 to find the shortest path
bestPathLengths = new PathSegmentLengths(0f, 0f, 0f);
//Loop through all paths that are enums to find the shortest
for (int w = 0; w < numPathWords; w++)
{
PathWords word = (PathWords)w;
PathSegmentLengths pathSegmentLengths;
float pathLength = PathLengthMath.GetLength(carEndMod, word, out pathSegmentLengths);
if (pathLength < shortestPathLength)
{
shortestPathLength = pathLength;
bestWord = word;
bestPathLengths = pathSegmentLengths;
}
}
return(shortestPathLength);
}
public static float CalculatePathLength(Pose goal, PathWords word, out float t, out float u, out float v)
{
switch (word)
{
// Reeds-Shepp 8.1: CSC, same turn
case PathWords.LfSfLf: return LfSfLf(goal, out t, out u, out v);
case PathWords.LbSbLb: return LfSfLf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfSfRf: return LfSfLf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbSbRb: return LfSfLf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.2: CSC, different turn
case PathWords.LfSfRf: return LfSfRf(goal, out t, out u, out v);
case PathWords.LbSbRb: return LfSfRf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfSfLf: return LfSfRf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbSbLb: return LfSfRf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.3: C|C|C
case PathWords.LfRbLf: return LfRbLf(goal, out t, out u, out v);
case PathWords.LbRfLb: return LfRbLf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLbRf: return LfRbLf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLfRb: return LfRbLf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.4: C|CC
case PathWords.LfRbLb: return LfRbLb(goal, out t, out u, out v);
case PathWords.LbRfLf: return LfRbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLbRb: return LfRbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLfRf: return LfRbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.4: CC|C
case PathWords.LfRfLb: return LfRfLb(goal, out t, out u, out v);
case PathWords.LbRbLf: return LfRfLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLfRb: return LfRfLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLbRf: return LfRfLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.7: CCu|CuC
case PathWords.LfRufLubRb: return LfRufLubRb(goal, out t, out u, out v);
case PathWords.LbRubLufRf: return LfRufLubRb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLufRubLb: return LfRufLubRb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLubRufLf: return LfRufLubRb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.8: C|CuCu|C
case PathWords.LfRubLubRf: return LfRubLubRf(goal, out t, out u, out v);
case PathWords.LbRufLufRb: return LfRubLubRf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLubRubLf: return LfRubLubRf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLufRufLb: return LfRubLubRf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.9: C|C(pi/2)SC, same turn
case PathWords.LfRbpi2SbLb: return LfRbpi2SbLb(goal, out t, out u, out v);
case PathWords.LbRfpi2SfLf: return LfRbpi2SbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLbpi2SbRb: return LfRbpi2SbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLfpi2SfRf: return LfRbpi2SbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.10: C|C(pi/2)SC, different turn
case PathWords.LfRbpi2SbRb: return LfRbpi2SbRb(goal, out t, out u, out v);
case PathWords.LbRfpi2SfRf: return LfRbpi2SbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLbpi2SbLb: return LfRbpi2SbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLfpi2SfLf: return LfRbpi2SbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.9 (reversed): CSC(pi/2)|C, same turn
case PathWords.LfSfRfpi2Lb: return LfSfRfpi2Lb(goal, out t, out u, out v);
case PathWords.LbSbRbpi2Lf: return LfSfRfpi2Lb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfSfLfpi2Rb: return LfSfRfpi2Lb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbSbLbpi2Rf: return LfSfRfpi2Lb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.LfSfLfpi2Rb: return LfSfLfpi2Rb(goal, out t, out u, out v);
case PathWords.LbSbLbpi2Rf: return LfSfLfpi2Rb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfSfRfpi2Lb: return LfSfLfpi2Rb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbSbRbpi2Lf: return LfSfLfpi2Rb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
// Reeds-Shepp 8.11: C|C(pi/2)SC(pi/2)|C
case PathWords.LfRbpi2SbLbpi2Rf: return LfRbpi2SbLbpi2Rf(goal, out t, out u, out v);
case PathWords.LbRfpi2SfLfpi2Rb: return LfRbpi2SbLbpi2Rf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RfLbpi2SbRbpi2Lf: return LfRbpi2SbLbpi2Rf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v);
case PathWords.RbLfpi2SfRfpi2Lb: return LfRbpi2SbLbpi2Rf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v);
}
t = 0; u = 0; v = 0;
return float.PositiveInfinity;
}
public static ReedsSheppActionSet GetPath(PathWords word, float t, float u, float v)
{
switch (word)
{
// Reeds-Shepp 8.1: CSC, same turn
case PathWords.LfSfLf: return LfSfLfpath(t, u, v);
case PathWords.LbSbLb: return timeflipTransform(LfSfLfpath(t, u, v));
case PathWords.RfSfRf: return reflectTransform(LfSfLfpath(t, u, v));
case PathWords.RbSbRb: return reflectTransform(timeflipTransform(LfSfLfpath(t, u, v)));
// Reeds-Shepp 8.2: CSC, different turn
case PathWords.LfSfRf: return LfSfRfpath(t, u, v);
case PathWords.LbSbRb: return timeflipTransform(LfSfRfpath(t, u, v));
case PathWords.RfSfLf: return reflectTransform(LfSfRfpath(t, u, v));
case PathWords.RbSbLb: return reflectTransform(timeflipTransform(LfSfRfpath(t, u, v)));
// Reeds-Shepp 8.3: C|C|C
case PathWords.LfRbLf: return LfRbLfpath(t, u, v);
case PathWords.LbRfLb: return timeflipTransform(LfRbLfpath(t, u, v));
case PathWords.RfLbRf: return reflectTransform(LfRbLfpath(t, u, v));
case PathWords.RbLfRb: return reflectTransform(timeflipTransform(LfRbLfpath(t, u, v)));
// Reeds-Shepp 8.4: C|CC
case PathWords.LfRbLb: return LfRbLbpath(t, u, v);
case PathWords.LbRfLf: return timeflipTransform(LfRbLbpath(t, u, v));
case PathWords.RfLbRb: return reflectTransform(LfRbLbpath(t, u, v));
case PathWords.RbLfRf: return reflectTransform(timeflipTransform(LfRbLbpath(t, u, v)));
// Reeds-Shepp 8.4: CC|C
case PathWords.LfRfLb: return LfRfLbpath(t, u, v);
case PathWords.LbRbLf: return timeflipTransform(LfRfLbpath(t, u, v));
case PathWords.RfLfRb: return reflectTransform(LfRfLbpath(t, u, v));
case PathWords.RbLbRf: return reflectTransform(timeflipTransform(LfRfLbpath(t, u, v)));
// Reeds-Shepp 8.7: CCu|CuC
case PathWords.LfRufLubRb: return LfRufLubRbpath(t, u, v);
case PathWords.LbRubLufRf: return timeflipTransform(LfRufLubRbpath(t, u, v));
case PathWords.RfLufRubLb: return reflectTransform(LfRufLubRbpath(t, u, v));
case PathWords.RbLubRufLf: return reflectTransform(timeflipTransform(LfRufLubRbpath(t, u, v)));
// Reeds-Shepp 8.8: C|CuCu|C
case PathWords.LfRubLubRf: return LfRubLubRfpath(t, u, v);
case PathWords.LbRufLufRb: return timeflipTransform(LfRubLubRfpath(t, u, v));
case PathWords.RfLubRubLf: return reflectTransform(LfRubLubRfpath(t, u, v));
case PathWords.RbLufRufLb: return reflectTransform(timeflipTransform(LfRubLubRfpath(t, u, v)));
// Reeds-Shepp 8.9: C|C(pi/2)SC, same turn
case PathWords.LfRbpi2SbLb: return LfRbpi2SbLbpath(t, u, v);
case PathWords.LbRfpi2SfLf: return timeflipTransform(LfRbpi2SbLbpath(t, u, v));
case PathWords.RfLbpi2SbRb: return reflectTransform(LfRbpi2SbLbpath(t, u, v));
case PathWords.RbLfpi2SfRf: return reflectTransform(timeflipTransform(LfRbpi2SbLbpath(t, u, v)));
// Reeds-Shepp 8.10: C|C(pi/2)SC, different turn
case PathWords.LfRbpi2SbRb: return LfRbpi2SbRbpath(t, u, v);
case PathWords.LbRfpi2SfRf: return timeflipTransform(LfRbpi2SbRbpath(t, u, v));
case PathWords.RfLbpi2SbLb: return reflectTransform(LfRbpi2SbRbpath(t, u, v));
case PathWords.RbLfpi2SfLf: return reflectTransform(timeflipTransform(LfRbpi2SbRbpath(t, u, v)));
// Reeds-Shepp 8.9 (reversed): CSC(pi/2)|C, same turn
case PathWords.LfSfRfpi2Lb: return LfSfRfpi2Lbpath(t, u, v);
case PathWords.LbSbRbpi2Lf: return timeflipTransform(LfSfRfpi2Lbpath(t, u, v));
case PathWords.RfSfLfpi2Rb: return reflectTransform(LfSfRfpi2Lbpath(t, u, v));
case PathWords.RbSbLbpi2Rf: return reflectTransform(timeflipTransform(LfSfRfpi2Lbpath(t, u, v)));
// Reeds-Shepp 8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.LfSfLfpi2Rb: return LfSfLfpi2Rbpath(t, u, v);
case PathWords.LbSbLbpi2Rf: return timeflipTransform(LfSfLfpi2Rbpath(t, u, v));
case PathWords.RfSfRfpi2Lb: return reflectTransform(LfSfLfpi2Rbpath(t, u, v));
case PathWords.RbSbRbpi2Lf: return reflectTransform(timeflipTransform(LfSfLfpi2Rbpath(t, u, v)));
// Reeds-Shepp 8.11: C|C(pi/2)SC(pi/2)|C
case PathWords.LfRbpi2SbLbpi2Rf: return LfRbpi2SbLbpi2Rfpath(t, u, v);
case PathWords.LbRfpi2SfLfpi2Rb: return timeflipTransform(LfRbpi2SbLbpi2Rfpath(t, u, v));
case PathWords.RfLbpi2SbRbpi2Lf: return reflectTransform(LfRbpi2SbLbpi2Rfpath(t, u, v));
case PathWords.RbLfpi2SfRfpi2Lb: return reflectTransform(timeflipTransform(LfRbpi2SbLbpi2Rfpath(t, u, v)));
}
return new ReedsSheppActionSet(float.PositiveInfinity);
}
public static float CalculatePathLength(Pose goal, PathWords word, out float t, out float u, out float v)
{
switch (word)
{
// Reeds-Shepp 8.1: CSC, same turn
case PathWords.LfSfLf: return(LfSfLf(goal, out t, out u, out v));
case PathWords.LbSbLb: return(LfSfLf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfSfRf: return(LfSfLf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbSbRb: return(LfSfLf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.2: CSC, different turn
case PathWords.LfSfRf: return(LfSfRf(goal, out t, out u, out v));
case PathWords.LbSbRb: return(LfSfRf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfSfLf: return(LfSfRf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbSbLb: return(LfSfRf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.3: C|C|C
case PathWords.LfRbLf: return(LfRbLf(goal, out t, out u, out v));
case PathWords.LbRfLb: return(LfRbLf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLbRf: return(LfRbLf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLfRb: return(LfRbLf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.4: C|CC
case PathWords.LfRbLb: return(LfRbLb(goal, out t, out u, out v));
case PathWords.LbRfLf: return(LfRbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLbRb: return(LfRbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLfRf: return(LfRbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.4: CC|C
case PathWords.LfRfLb: return(LfRfLb(goal, out t, out u, out v));
case PathWords.LbRbLf: return(LfRfLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLfRb: return(LfRfLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLbRf: return(LfRfLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.7: CCu|CuC
case PathWords.LfRufLubRb: return(LfRufLubRb(goal, out t, out u, out v));
case PathWords.LbRubLufRf: return(LfRufLubRb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLufRubLb: return(LfRufLubRb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLubRufLf: return(LfRufLubRb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.8: C|CuCu|C
case PathWords.LfRubLubRf: return(LfRubLubRf(goal, out t, out u, out v));
case PathWords.LbRufLufRb: return(LfRubLubRf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLubRubLf: return(LfRubLubRf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLufRufLb: return(LfRubLubRf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.9: C|C(pi/2)SC, same turn
case PathWords.LfRbpi2SbLb: return(LfRbpi2SbLb(goal, out t, out u, out v));
case PathWords.LbRfpi2SfLf: return(LfRbpi2SbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLbpi2SbRb: return(LfRbpi2SbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLfpi2SfRf: return(LfRbpi2SbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.10: C|C(pi/2)SC, different turn
case PathWords.LfRbpi2SbRb: return(LfRbpi2SbRb(goal, out t, out u, out v));
case PathWords.LbRfpi2SfRf: return(LfRbpi2SbLb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLbpi2SbLb: return(LfRbpi2SbLb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLfpi2SfLf: return(LfRbpi2SbLb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.9 (reversed): CSC(pi/2)|C, same turn
case PathWords.LfSfRfpi2Lb: return(LfSfRfpi2Lb(goal, out t, out u, out v));
case PathWords.LbSbRbpi2Lf: return(LfSfRfpi2Lb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfSfLfpi2Rb: return(LfSfRfpi2Lb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbSbLbpi2Rf: return(LfSfRfpi2Lb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.LfSfLfpi2Rb: return(LfSfLfpi2Rb(goal, out t, out u, out v));
case PathWords.LbSbLbpi2Rf: return(LfSfLfpi2Rb(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfSfRfpi2Lb: return(LfSfLfpi2Rb(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbSbRbpi2Lf: return(LfSfLfpi2Rb(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
// Reeds-Shepp 8.11: C|C(pi/2)SC(pi/2)|C
case PathWords.LfRbpi2SbLbpi2Rf: return(LfRbpi2SbLbpi2Rf(goal, out t, out u, out v));
case PathWords.LbRfpi2SfLfpi2Rb: return(LfRbpi2SbLbpi2Rf(new Pose(-goal.X, goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RfLbpi2SbRbpi2Lf: return(LfRbpi2SbLbpi2Rf(new Pose(goal.X, -goal.Y, -goal.Orientation), out t, out u, out v));
case PathWords.RbLfpi2SfRfpi2Lb: return(LfRbpi2SbLbpi2Rf(new Pose(-goal.X, -goal.Y, goal.Orientation), out t, out u, out v));
}
t = 0; u = 0; v = 0;
return(float.PositiveInfinity);
}
public static ReedsSheppActionSet GetPath(PathWords word, float t, float u, float v)
{
switch (word)
{
// Reeds-Shepp 8.1: CSC, same turn
case PathWords.LfSfLf: return(LfSfLfpath(t, u, v));
case PathWords.LbSbLb: return(timeflipTransform(LfSfLfpath(t, u, v)));
case PathWords.RfSfRf: return(reflectTransform(LfSfLfpath(t, u, v)));
case PathWords.RbSbRb: return(reflectTransform(timeflipTransform(LfSfLfpath(t, u, v))));
// Reeds-Shepp 8.2: CSC, different turn
case PathWords.LfSfRf: return(LfSfRfpath(t, u, v));
case PathWords.LbSbRb: return(timeflipTransform(LfSfRfpath(t, u, v)));
case PathWords.RfSfLf: return(reflectTransform(LfSfRfpath(t, u, v)));
case PathWords.RbSbLb: return(reflectTransform(timeflipTransform(LfSfRfpath(t, u, v))));
// Reeds-Shepp 8.3: C|C|C
case PathWords.LfRbLf: return(LfRbLfpath(t, u, v));
case PathWords.LbRfLb: return(timeflipTransform(LfRbLfpath(t, u, v)));
case PathWords.RfLbRf: return(reflectTransform(LfRbLfpath(t, u, v)));
case PathWords.RbLfRb: return(reflectTransform(timeflipTransform(LfRbLfpath(t, u, v))));
// Reeds-Shepp 8.4: C|CC
case PathWords.LfRbLb: return(LfRbLbpath(t, u, v));
case PathWords.LbRfLf: return(timeflipTransform(LfRbLbpath(t, u, v)));
case PathWords.RfLbRb: return(reflectTransform(LfRbLbpath(t, u, v)));
case PathWords.RbLfRf: return(reflectTransform(timeflipTransform(LfRbLbpath(t, u, v))));
// Reeds-Shepp 8.4: CC|C
case PathWords.LfRfLb: return(LfRfLbpath(t, u, v));
case PathWords.LbRbLf: return(timeflipTransform(LfRfLbpath(t, u, v)));
case PathWords.RfLfRb: return(reflectTransform(LfRfLbpath(t, u, v)));
case PathWords.RbLbRf: return(reflectTransform(timeflipTransform(LfRfLbpath(t, u, v))));
// Reeds-Shepp 8.7: CCu|CuC
case PathWords.LfRufLubRb: return(LfRufLubRbpath(t, u, v));
case PathWords.LbRubLufRf: return(timeflipTransform(LfRufLubRbpath(t, u, v)));
case PathWords.RfLufRubLb: return(reflectTransform(LfRufLubRbpath(t, u, v)));
case PathWords.RbLubRufLf: return(reflectTransform(timeflipTransform(LfRufLubRbpath(t, u, v))));
// Reeds-Shepp 8.8: C|CuCu|C
case PathWords.LfRubLubRf: return(LfRubLubRfpath(t, u, v));
case PathWords.LbRufLufRb: return(timeflipTransform(LfRubLubRfpath(t, u, v)));
case PathWords.RfLubRubLf: return(reflectTransform(LfRubLubRfpath(t, u, v)));
case PathWords.RbLufRufLb: return(reflectTransform(timeflipTransform(LfRubLubRfpath(t, u, v))));
// Reeds-Shepp 8.9: C|C(pi/2)SC, same turn
case PathWords.LfRbpi2SbLb: return(LfRbpi2SbLbpath(t, u, v));
case PathWords.LbRfpi2SfLf: return(timeflipTransform(LfRbpi2SbLbpath(t, u, v)));
case PathWords.RfLbpi2SbRb: return(reflectTransform(LfRbpi2SbLbpath(t, u, v)));
case PathWords.RbLfpi2SfRf: return(reflectTransform(timeflipTransform(LfRbpi2SbLbpath(t, u, v))));
// Reeds-Shepp 8.10: C|C(pi/2)SC, different turn
case PathWords.LfRbpi2SbRb: return(LfRbpi2SbRbpath(t, u, v));
case PathWords.LbRfpi2SfRf: return(timeflipTransform(LfRbpi2SbRbpath(t, u, v)));
case PathWords.RfLbpi2SbLb: return(reflectTransform(LfRbpi2SbRbpath(t, u, v)));
case PathWords.RbLfpi2SfLf: return(reflectTransform(timeflipTransform(LfRbpi2SbRbpath(t, u, v))));
// Reeds-Shepp 8.9 (reversed): CSC(pi/2)|C, same turn
case PathWords.LfSfRfpi2Lb: return(LfSfRfpi2Lbpath(t, u, v));
case PathWords.LbSbRbpi2Lf: return(timeflipTransform(LfSfRfpi2Lbpath(t, u, v)));
case PathWords.RfSfLfpi2Rb: return(reflectTransform(LfSfRfpi2Lbpath(t, u, v)));
case PathWords.RbSbLbpi2Rf: return(reflectTransform(timeflipTransform(LfSfRfpi2Lbpath(t, u, v))));
// Reeds-Shepp 8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.LfSfLfpi2Rb: return(LfSfLfpi2Rbpath(t, u, v));
case PathWords.LbSbLbpi2Rf: return(timeflipTransform(LfSfLfpi2Rbpath(t, u, v)));
case PathWords.RfSfRfpi2Lb: return(reflectTransform(LfSfLfpi2Rbpath(t, u, v)));
case PathWords.RbSbRbpi2Lf: return(reflectTransform(timeflipTransform(LfSfLfpi2Rbpath(t, u, v))));
// Reeds-Shepp 8.11: C|C(pi/2)SC(pi/2)|C
case PathWords.LfRbpi2SbLbpi2Rf: return(LfRbpi2SbLbpi2Rfpath(t, u, v));
case PathWords.LbRfpi2SfLfpi2Rb: return(timeflipTransform(LfRbpi2SbLbpi2Rfpath(t, u, v)));
case PathWords.RfLbpi2SbRbpi2Lf: return(reflectTransform(LfRbpi2SbLbpi2Rfpath(t, u, v)));
case PathWords.RbLfpi2SfRfpi2Lb: return(reflectTransform(timeflipTransform(LfRbpi2SbLbpi2Rfpath(t, u, v))));
}
return(new ReedsSheppActionSet(float.PositiveInfinity));
}
//Could maybe optimize sin(phi) and cos(phi) because they are always the same
public static float GetLength(RSCar car, PathWords word, out PathSegmentLengths pathLengths)
{
switch (word)
{
//8.1: CSC, same turn
case PathWords.Lf_Sf_Lf: return(Lf_Sf_Lf(car, out pathLengths));
case PathWords.Lb_Sb_Lb: return(Lf_Sf_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Rf: return(Lf_Sf_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Rb: return(Lf_Sf_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.2: CSC, different turn
case PathWords.Lf_Sf_Rf: return(Lf_Sf_Rf(car, out pathLengths));
case PathWords.Lb_Sb_Rb: return(Lf_Sf_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Lf: return(Lf_Sf_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Lb: return(Lf_Sf_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.3: C|C|C
case PathWords.Lf_Rb_Lf: return(Lf_Rb_Lf(car, out pathLengths));
case PathWords.Lb_Rf_Lb: return(Lf_Rb_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lb_Rf: return(Lf_Rb_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lf_Rb: return(Lf_Rb_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.4: C|CC
case PathWords.Lf_Rb_Lb: return(Lf_Rb_Lb(car, out pathLengths));
case PathWords.Lb_Rf_Lf: return(Lf_Rb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lb_Rb: return(Lf_Rb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lf_Rf: return(Lf_Rb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.4: CC|C
case PathWords.Lf_Rf_Lb: return(Lf_Rf_Lb(car, out pathLengths));
case PathWords.Lb_Rb_Lf: return(Lf_Rf_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lf_Rb: return(Lf_Rf_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lb_Rf: return(Lf_Rf_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.7: CCu|CuC
case PathWords.Lf_Ruf_Lub_Rb: return(Lf_Ruf_Lub_Rb(car, out pathLengths));
case PathWords.Lb_Rub_Luf_Rf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Luf_Rub_Lb: return(Lf_Ruf_Lub_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lub_Ruf_Lf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.8: C|CuCu|C
case PathWords.Lf_Rub_Lub_Rf: return(Lf_Rub_Lub_Rf(car, out pathLengths));
case PathWords.Lb_Ruf_Luf_Rb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lub_Rub_Lf: return(Lf_Rub_Lub_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Luf_Ruf_Lb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.9: C|C(pi/2)SC, same turn
case PathWords.Lf_Rbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Lb(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.10: C|C(pi/2)SC, different turn
case PathWords.Lf_Rbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Rb(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.9(reversed): CSC(pi / 2) | C, same turn
case PathWords.Lf_Sf_Rfpi2_Lb: return(Lf_Sf_Rfpi2_Lb(car, out pathLengths));
case PathWords.Lb_Sb_Rbpi2_Lf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Lfpi2_Rb: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Lbpi2_Rf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.Lf_Sf_Lfpi2_Rb: return(Lf_Sf_Lfpi2_Rb(car, out pathLengths));
case PathWords.Lb_Sb_Lbpi2_Rf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Rfpi2_Lb: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Rbpi2_Lf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.11: C | C(pi / 2)SC(pi / 2) | C
case PathWords.Lf_Rbpi2_Sb_Lbpi2_Rf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lfpi2_Rb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Rbpi2_Lf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Rfpi2_Lb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
}
pathLengths = new PathSegmentLengths(0f, 0f, 0f);
return(float.PositiveInfinity);
}
//Add waypoints to a given path
private static List <RSCar> AddWaypoints(
PathWords word, PathSegmentLengths pathSegmentLengths, RSCar carStart, RSCar carEnd, float wpDistance, float turningRadius, bool generateOneWp)
{
//Find the car settings we need to drive through the path
List <SegmentSettings> pathSettings = PathSettings.GetSettings(word, pathSegmentLengths);
if (pathSettings == null)
{
Debug.Log("Cant find settings for a path");
return(null);
}
//Generate the waypoints
//Data used when generating the path
//The pos and heading we will move along the path
Vector3 pos = carStart.pos;
float heading = carStart.HeadingInRad;
//The distance between each step we take when generating the path, the smaller the better, but is also slower
float stepDistance = 0.05f;
//To generate waypoints with a certain distance between them we need to know how far we have driven since the last wp
float driveDistance = 0f;
//The waypoints
List <RSCar> waypoints = new List <RSCar>();
//Add the first wp
waypoints.Add(new RSCar(pos, heading, pathSettings[0].gear, pathSettings[0].steering));
//Loop through all path 3-5 path segments
for (int i = 0; i < pathSettings.Count; i++)
{
SegmentSettings segmentSettings = pathSettings[i];
//How many steps will we take to generate this segment
//Will always be at least 2 no matter the stepDistance
int n = (int)Math.Ceiling((segmentSettings.length * turningRadius) / stepDistance);
//How far will we move each step?
float stepLength = (segmentSettings.length * turningRadius) / n;
//Change stuff depending on in which direction we are moving
float steeringWheelPos = 1f;
if (segmentSettings.steering == RSCar.Steering.Left)
{
steeringWheelPos = -1f;
}
//Invert steering if we are reversing
if (segmentSettings.gear == RSCar.Gear.Back)
{
steeringWheelPos *= -1f;
}
//Drive through this segment in small steps
for (int j = 0; j < n; j++)
{
//Update position
float dx = stepLength * Mathf.Sin(heading);
float dz = stepLength * Mathf.Cos(heading);
if (segmentSettings.gear == RSCar.Gear.Back)
{
dx = -dx;
dz = -dz;
}
pos = new Vector3(pos.x + dx, pos.y, pos.z + dz);
//Update heading if we are turning
if (segmentSettings.steering != RSCar.Steering.Straight)
{
heading = heading + (stepLength / turningRadius) * steeringWheelPos;
}
//Should we generate a new wp?
driveDistance += stepLength;
if (driveDistance > wpDistance)
{
waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));
driveDistance = driveDistance - wpDistance;
if (generateOneWp)
{
return(waypoints);
}
}
}
//We also need to add the last pos of this segment as waypoint or the path will not be the same
//if we for example are ignoring the waypoint where we change direction
waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));
}
//Move the last wp pos to the position of the goal car
//When we generate waypoints, the accuracy depends on the stepDistance, so is not always hitting the goal exactly
waypoints[waypoints.Count - 1].pos = carEnd.pos;
return(waypoints);
}
public static List <SegmentSettings> GetSettings(PathWords word, PathSegmentLengths pathLengths)
{
switch (word)
{
//8.1: CSC, same turn
case PathWords.Lf_Sf_Lf: return(Lf_Sf_Lf_path(pathLengths));
case PathWords.Lb_Sb_Lb: return(TimeFlip(Lf_Sf_Lf_path(pathLengths)));
case PathWords.Rf_Sf_Rf: return(Reflect(Lf_Sf_Lf_path(pathLengths)));
case PathWords.Rb_Sb_Rb: return(Reflect(TimeFlip(Lf_Sf_Lf_path(pathLengths))));
//8.2: CSC, different turn
case PathWords.Lf_Sf_Rf: return(Lf_Sf_Rf_path(pathLengths));
case PathWords.Lb_Sb_Rb: return(TimeFlip(Lf_Sf_Rf_path(pathLengths)));
case PathWords.Rf_Sf_Lf: return(Reflect(Lf_Sf_Rf_path(pathLengths)));
case PathWords.Rb_Sb_Lb: return(Reflect(TimeFlip(Lf_Sf_Rf_path(pathLengths))));
//8.3: C|C|C
case PathWords.Lf_Rb_Lf: return(Lf_Rb_Lf_path(pathLengths));
case PathWords.Lb_Rf_Lb: return(TimeFlip(Lf_Rb_Lf_path(pathLengths)));
case PathWords.Rf_Lb_Rf: return(Reflect(Lf_Rb_Lf_path(pathLengths)));
case PathWords.Rb_Lf_Rb: return(Reflect(TimeFlip(Lf_Rb_Lf_path(pathLengths))));
//8.4: C|CC
case PathWords.Lf_Rb_Lb: return(Lf_Rb_Lb_path(pathLengths));
case PathWords.Lb_Rf_Lf: return(TimeFlip(Lf_Rb_Lb_path(pathLengths)));
case PathWords.Rf_Lb_Rb: return(Reflect(Lf_Rb_Lb_path(pathLengths)));
case PathWords.Rb_Lf_Rf: return(Reflect(TimeFlip(Lf_Rb_Lb_path(pathLengths))));
//8.4: CC|C
case PathWords.Lf_Rf_Lb: return(Lf_Rf_Lb_path(pathLengths));
case PathWords.Lb_Rb_Lf: return(TimeFlip(Lf_Rf_Lb_path(pathLengths)));
case PathWords.Rf_Lf_Rb: return(Reflect(Lf_Rf_Lb_path(pathLengths)));
case PathWords.Rb_Lb_Rf: return(Reflect(TimeFlip(Lf_Rf_Lb_path(pathLengths))));
//8.7: CCu|CuC
case PathWords.Lf_Ruf_Lub_Rb: return(Lf_Ruf_Lub_Rb_path(pathLengths));
case PathWords.Lb_Rub_Luf_Rf: return(TimeFlip(Lf_Ruf_Lub_Rb_path(pathLengths)));
case PathWords.Rf_Luf_Rub_Lb: return(Reflect(Lf_Ruf_Lub_Rb_path(pathLengths)));
case PathWords.Rb_Lub_Ruf_Lf: return(Reflect(TimeFlip(Lf_Ruf_Lub_Rb_path(pathLengths))));
//8.8: C|CuCu|C
case PathWords.Lf_Rub_Lub_Rf: return(Lf_Rub_Lub_Rf_path(pathLengths));
case PathWords.Lb_Ruf_Luf_Rb: return(TimeFlip(Lf_Rub_Lub_Rf_path(pathLengths)));
case PathWords.Rf_Lub_Rub_Lf: return(Reflect(Lf_Rub_Lub_Rf_path(pathLengths)));
case PathWords.Rb_Luf_Ruf_Lb: return(Reflect(TimeFlip(Lf_Rub_Lub_Rf_path(pathLengths))));
//8.9: C|C(pi/2)SC, same turn
case PathWords.Lf_Rbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Lb_path(pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lf: return(TimeFlip(Lf_Rbpi2_Sb_Lb_path(pathLengths)));
case PathWords.Rf_Lbpi2_Sb_Rb: return(Reflect(Lf_Rbpi2_Sb_Lb_path(pathLengths)));
case PathWords.Rb_Lfpi2_Sf_Rf: return(Reflect(TimeFlip(Lf_Rbpi2_Sb_Lb_path(pathLengths))));
//8.10: C|C(pi/2)SC, different turn
case PathWords.Lf_Rbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Rb_path(pathLengths));
case PathWords.Lb_Rfpi2_Sf_Rf: return(TimeFlip(Lf_Rbpi2_Sb_Rb_path(pathLengths)));
case PathWords.Rf_Lbpi2_Sb_Lb: return(Reflect(Lf_Rbpi2_Sb_Rb_path(pathLengths)));
case PathWords.Rb_Lfpi2_Sf_Lf: return(Reflect(TimeFlip(Lf_Rbpi2_Sb_Rb_path(pathLengths))));
//8.9 (reversed): CSC(pi/2)|C, same turn
case PathWords.Lf_Sf_Rfpi2_Lb: return(Lf_Sf_Rfpi2_Lb_path(pathLengths));
case PathWords.Lb_Sb_Rbpi2_Lf: return(TimeFlip(Lf_Sf_Rfpi2_Lb_path(pathLengths)));
case PathWords.Rf_Sf_Lfpi2_Rb: return(Reflect(Lf_Sf_Rfpi2_Lb_path(pathLengths)));
case PathWords.Rb_Sb_Lbpi2_Rf: return(Reflect(TimeFlip(Lf_Sf_Rfpi2_Lb_path(pathLengths))));
//8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.Lf_Sf_Lfpi2_Rb: return(Lf_Sf_Lfpi2_Rb_path(pathLengths));
case PathWords.Lb_Sb_Lbpi2_Rf: return(TimeFlip(Lf_Sf_Lfpi2_Rb_path(pathLengths)));
case PathWords.Rf_Sf_Rfpi2_Lb: return(Reflect(Lf_Sf_Lfpi2_Rb_path(pathLengths)));
case PathWords.Rb_Sb_Rbpi2_Lf: return(Reflect(TimeFlip(Lf_Sf_Lfpi2_Rb_path(pathLengths))));
//8.11: C|C(pi/2)SC(pi/2)|C
case PathWords.Lf_Rbpi2_Sb_Lbpi2_Rf: return(Lf_Rbpi2_Sb_Lbpi2_Rf_path(pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lfpi2_Rb: return(TimeFlip(Lf_Rbpi2_Sb_Lbpi2_Rf_path(pathLengths)));
case PathWords.Rf_Lbpi2_Sb_Rbpi2_Lf: return(Reflect(Lf_Rbpi2_Sb_Lbpi2_Rf_path(pathLengths)));
case PathWords.Rb_Lfpi2_Sf_Rfpi2_Lb: return(Reflect(TimeFlip(Lf_Rbpi2_Sb_Lbpi2_Rf_path(pathLengths))));
}
return(null);
}