I'd rather do this differently somehow. I know this is already in RRTstar, and we can leave it for another time, but I'd rather have an internal planner flag that distanceFunction() uses to oreorder args, rather than changing the function pointer. Also, this seems to be a fragile solution. If the implementation of nearest neighbors is not consistent when calling the distance function, things won't work properly.