Poleciłbym alternatywne podejście: szybko eksplorujące losowe drzewo (RRT) . Jedną fajną rzeczą jest to, że możesz ominąć zakręty lub eksplodować we wszystkich kierunkach.
Algorytm jest naprawdę prosty:
// Returns a random tree containing the start and the goal.
// Grows the tree for a maximum number of iterations.
Tree RRT(Node start, Node goal, int maxIters)
{
// Initialize a tree with a root as the start node.
Tree t = new Tree();
t.Root = start;
bool reachedGoal = false;
int iter = 0;
// Keep growing the tree until it contains the goal and we've
// grown for the required number of iterations.
while (!reachedGoal || iter < maxIters)
{
// Get a random node somewhere near the goal
Node random = RandomSample(goal);
// Get the closest node in the tree to the sample.
Node closest = t.GetClosestNode(random);
// Create a new node between the closest node and the sample.
Node extension = ExtendToward(closest, random);
// If we managed to create a new node, add it to the tree.
if (extension)
{
closest.AddChild(extension);
// If we haven't yet reached the goal, and the new node
// is very near the goal, add the goal to the tree.
if(!reachedGoal && extension.IsNear(goal))
{
extension.AddChild(goal);
reachedGoal = true;
}
}
iter++;
}
return t;
}
Modyfikując funkcje RandomSample
i ExtendToward
, możesz uzyskać bardzo różne drzewa. Jeśli RandomSample
wszędzie jednolicie próbki, drzewo będzie rosło równomiernie we wszystkich kierunkach. Jeśli jest tendencyjny do celu, drzewo będzie miało tendencję do wzrostu w kierunku celu. Jeśli zawsze próbkuje bramkę, drzewo będzie od początku do linii prostej.
ExtendToward
pozwala także robić interesujące rzeczy na drzewie. Po pierwsze, jeśli masz przeszkody (takie jak ściany), możesz sprawić, aby drzewo rosło wokół nich po prostu odrzucając rozszerzenia kolidujące ze ścianami.
Oto, jak to wygląda, gdy nie przesądzasz próbkowania w kierunku celu:
(źródło: uiuc.edu )
A oto jak to wygląda ze ścianami
Kilka fajnych właściwości RRT po jego zakończeniu:
- RRT nigdy się nie przekroczy
- RRT ostatecznie obejmie całą przestrzeń coraz mniejszymi gałęziami
- Ścieżka od początku do celu może być całkowicie losowa i dziwna.