ZJU-FAST-Lab / Swarm-Formation C++ 575 87 Formation Flight in Dense Environments swarm motion-planning multi-robot formation-flight