while((distance_to_target > branch_length) || hasObstacle(target, Xnew) || count < 2000)

{
// Code to generate new random node
// ...

// Check if new node is closer to target than current node
if (distance_to_new < distance_to_target) {
// Check if path from current node to new node is obstacle-free
if (!hasObstacle(Xnearest, Xnew)) {
// Update target node and branch length
target = Xnew;
branch_length = distance_to_new;
}
}

// Increment iteration count
count++;
}

// Code to return path from start to target node
// ...