dijstra的代码实现，c和c++实现。可以作为路径规划使用。