//使用迪杰斯特拉算法计算图中两点间的最短路径
public static float Djstl(TrailerGraph g, int s, int e)
{
float shortLength = 0;
int[] label = new int[g.numVertex]; //标记是否找过
for (int i = 0; i < label.Length; i++)
{
label[i] = -1;
}
label[0] = 1;
Console.Write(s.ToString() + " ");
while (s != e)
{
int start = s;
float low = 10000;
int k = s;
for (int j = 0; j < g.numVertex; j++)
{
if (label[j] == -1 && low > g.adjMatrix[s, j].weight && g.adjMatrix[s, j].weight != 0)
{
low = g.adjMatrix[s, j].weight;
k = j;
}
}
label[s] = 1;
s = k;
int end = s;
shortLength += g.adjMatrix[start, end].weight;
}
return(shortLength);
}
// Use this for initialization
void Awake()
{
//初始化各种模型的生成参数
initCreatePara();
//创建轨道
CreateTrailer(GlobalVaribles.trailer_Para);
//创建轨道对应的有向图
GlobalVaribles.trailerGraph = TrailerGraph.CreateTrailer(
GlobalVaribles.trailer_Para, GlobalVaribles.edges, GlobalVaribles.vertexes);
//创建小车
CreateCars();
}
//生成一个轨道图的对象
public static TrailerGraph CreateTrailer(GlobalVaribles.Trailer_Para trailer_Para, List <Edge> edges, List <Vertex> vertexes)
{
//生成轨道对应的图
TrailerGraph trailerGraph = new TrailerGraph(vertexes.Count);
trailerGraph.numVertex = vertexes.Count;
//给点赋值（坐标）
for (int i = 0; i < vertexes.Count; i++)
{
trailerGraph.vertexes[i] = new Vertex(vertexes[i].index, vertexes[i].data, vertexes[i].vertexPos);
}
//给边赋值（边的两点序号、边的种类（直还是曲）、边长
for (int i = 0; i < vertexes.Count; i++)
{
trailerGraph.AddEdge(i, i, LineType.Straight, 0, new Vector3(0, 0, 0));
}
for (int i = 0; i < edges.Count; i++)
{
trailerGraph.AddEdge(edges[i]);
}
trailerGraph.curveRadius = (trailer_Para.inCurveRadius + trailer_Para.outCurveRadius) / 2;
trailerGraph.distMap = new float[trailerGraph.numVertex, trailerGraph.numVertex];
trailerGraph.path = new int[trailerGraph.numVertex, trailerGraph.numVertex];
trailerGraph.pathMap = new PathData[trailerGraph.numVertex, trailerGraph.numVertex];
for (int i = 0; i < trailerGraph.numVertex; i++)//必须这样初始化
{
for (int j = 0; j < trailerGraph.numVertex; j++)
{
trailerGraph.pathMap[i, j] = new PathData();
}
}
//根据图来初始化最短路径
trailerGraph.floyd();
trailerGraph.getAllPathList();
return(trailerGraph);
}
static void Main(string[] args)
{
//初始化轨道
TrailerGraph trailer = new TrailerGraph().createTrailer();
//初始化小车(按照车的前后顺序)
float carSpeed = 1f;
List <Car> cars = new List <Car>();
Car car1 = new Car(new Position(0, 15, 0), new EdgeData(), CarState.Empty, carSpeed);
Car car2 = new Car(new Position(-5, 10, 0), new EdgeData(), CarState.Empty, carSpeed);
cars.Add(car1);
cars.Add(car2);
//添加任务队列
List <Task> taskList = new List <Task>();
Task task1 = new Task(new Position(5, 5, 0), new EdgeData(2, 3, LineType.Straight, 10),
new Position(-5, 5, 0), new EdgeData(6, 1, LineType.Straight, 10));
Task task2 = new Task(new Position(5, -5, 0), new EdgeData(3, 4, LineType.Straight, 10),
new Position(-5, -5, 0), new EdgeData(5, 6, LineType.Straight, 10));
taskList.Add(task1);
taskList.Add(task2);
//得到所有的匹配组合matchList
List <Dictionary <Car, Task> > matchList = new List <Dictionary <Car, Task> >();
getMatchList(ref matchList, cars, taskList, 0, cars.Count);
#region
//计算不同排列下的任务实现总时间，每次只优化任务队列的前2个任务
//如果小车已经在运送了
//只实现一帧的计算
//分为任务和小车
//先得到当前的任务队列(先假设只处理前2个任务)
//假设同时只有2辆车（车有位置等信息）
//计算不同排列下的任务实现总时间
//假设只能顺时针运动
//难点：计算路径长度
//还要考虑最短路径（虽然只有一个运动方向）、、、
//得出最优方案
//根据最优方案调整小车的运动
#endregion
//计算每个组合的总时间
//并求出其中最短时间的组合
float[] times = new float[cars.Count];
int index = 0;
float minTime = 10000000f; //最短时间
Dictionary <Car, Task> minMatch = matchList[0]; //最短时间对应的匹配
foreach (Dictionary <Car, Task> match in matchList)
{
times[index] = trailer.calcuMultiTaskTime(match);
if (times[index] < minTime)
{
minTime = times[index];
minMatch = match;
}
index++;
}
//给各个小车分发任务
DistributeTasks(minMatch);
Console.ReadKey();
}