I am working on writing code for a coordinated multi-robot rapidly exploring random tree (RRT) based planner, which would naturally involve a lot of sampling, nearest neighbor searching and 'radius' searching. Because this is a coordinated/cooperative planning step, all robots' paths are incrementally created in every iteration until all robots reach their respective goals, and the planner needs all the robots' positions during the planning phase.
I have a basic framework for this working in MATLAB, which was only a proof of concept and not really efficient: but I am not sure what the best way to program it in, say, C++ would be. Normally for an RRT, I would go for a KD tree implementation, but in a multi-robot point of view, the environment would be a joint configuration space and this would mean a pseudo-high-dimensional KD tree: which is not actually high dimensional, but just needs to perform nearest neighbor searches in a space that combines the states (x,y,z, yaw) of all the robots - over and over again during the planning phase. The metric is simple enough, as it is just Euclidean distance, but I don't know if using KD trees for this will be computationally efficient. I'm looking for some suggestions on how to describe the configuration space for an efficient multi-robot RRT (I am thinking of a maximum of five robots) with a state dimension of 4.