public void getMap()
{
total.intsection1.linksin.Clear();
total.intsection1.linksout.Clear();
total.intsection1.ID = 1;
total.intsection1.connectors.Clear();
var gb18030 = Encoding.GetEncoding("GB18030");
List <intersection> intersection1 = new List <intersection>();
fs = new FileStream(Application.StartupPath + "\\campus test.txt", FileMode.Open);
sr = new StreamReader(fs, gb18030, true);
string intersectionStr = "";
string X1str = "";
string X2str = "";
string linkin_id = "";
string linkout_id = "";
string connector_info = "";
string connect = "";
int linkin_num;
int linkout_num;
string lane_count = "";
string lane_id = "";
intersectionStr = sr.ReadLine();
while (intersectionStr != null)
{
intersection newIntersection = new intersection();
newIntersection.ID = Convert.ToInt32(intersectionStr.Split('|')[0]);
//MessageBox.Show("jiahchakoubianhao" + newIntersection.intersectionnum);
linkin_num = Convert.ToInt32(intersectionStr.Split('|')[1]);
//MessageBox.Show("jiahchakouname" + newIntersection.intersectionname);
linkout_num = Convert.ToInt32(intersectionStr.Split('|')[2]);
//MessageBox.Show("jiahchakoubianhao" + numOfRoads);
linkin_id = Convert.ToString(intersectionStr.Split('|')[3]);
linkout_id = Convert.ToString(intersectionStr.Split('|')[4]);
lane_count = Convert.ToString(intersectionStr.Split('|')[5]);
lane_id = Convert.ToString(intersectionStr.Split('|')[6]);
int[] linkin = new int[linkin_num];
int[] linkout = new int[linkout_num];
for (int i = 0; i < linkin_num; i++)
{
link link1 = new link();
link1.ID = Convert.ToInt32(linkin_id.Split(',')[i]);
total.intsection1.linksin.Add(link1);
}
for (int i = 0; i < linkout_num; i++)
{
link link1 = new link();
link1.ID = Convert.ToInt32(linkout_id.Split(',')[i]);
total.intsection1.linksout.Add(link1);
}
int a = 0;
for (int i = 0; i < linkin_num; i++)
{
linkin[i] = Convert.ToInt32(lane_count.Split(',')[i]);
for (int j = 0; j < linkin[i]; j++)
{
a += linkin[i];
lane lane1 = new lane();
lane1.ID = Convert.ToInt32(lane_id.Split(',')[a - 1]);
total.intsection1.linksin[i].lanes.Add(lane1);
}
}
int b = 0;
for (int i = 0; i < linkout_num; i++)
{
linkout[i] = Convert.ToInt32(lane_count.Split(',')[i + linkin_num]);
for (int j = 0; j < linkout[i]; j++)
{
b += linkout[i];
lane lane1 = new lane();
lane1.ID = Convert.ToInt32(lane_id.Split(',')[a + b - 1]);
total.intsection1.linksout[i].lanes.Add(lane1);
}
}
connector_info = Convert.ToString(intersectionStr.Split('|')[7]);
for (int i = 0; i < (linkin_num * (linkout_num - 1)); i++)
{
connect = Convert.ToString(connector_info.Split('/')[i]);
connector c1 = new connector();//connector
//MessageBox.Show("duhaole" + connect);
c1.ID = Convert.ToInt32(connect.Split(',')[0]);
int start;
int end;
start = Convert.ToInt32(connect.Split(',')[1]);
end = Convert.ToInt32(connect.Split(',')[2]);
if (start <= linkin_num)
{
c1.lanestart = total.intsection1.linksin[start - 1].lanes[0];
c1.laneend = total.intsection1.linksout[end - 1 - linkin_num].lanes[0];
}
c1.length = Convert.ToDouble(connect.Split(',')[3]);
c1.vmax = Convert.ToDouble(connect.Split(',')[4]) / 3.6;
c1.vmin = Convert.ToDouble(connect.Split(',')[5]) / 3.6;
c1.angle = Convert.ToDouble(connect.Split(',')[6]);
total.intsection1.connectors.Add(c1);
}
X1str = Convert.ToString(intersectionStr.Split('|')[8]);//冲突矩阵
for (int i = 0; i < 144; i++)
{
total.intsection1.X1[i] = Convert.ToInt32(X1str.Split(',')[i]);
}
X2str = Convert.ToString(intersectionStr.Split('|')[9]);//冲突距离矩阵
for (int i = 0; i < 144; i++)
{
total.intsection1.X2[i] = Convert.ToDouble(X2str.Split(',')[i]);
}
int a1 = 0;
for (int i = 0; i < linkin.Length; i++)
{
for (int j = 0; j < linkin[i]; j++)
{
a1 += linkin[i];//linkin都为1
string section = Convert.ToString(intersectionStr.Split('|')[9 + a1]);
int section_num = Convert.ToInt32(section.Split(',')[0]);
section section1 = new section();
for (int k = 0; k < section_num; k++)
{
section1.id = Convert.ToInt32(section.Split(',')[5 * k + 1]);
section1.start.lonti = Convert.ToDouble(section.Split(',')[5 * k + 2]);
section1.start.lati = Convert.ToDouble(section.Split(',')[5 * k + 3]);
section1.end.lonti = Convert.ToDouble(section.Split(',')[5 * k + 4]);
section1.end.lati = Convert.ToDouble(section.Split(',')[5 * k + 5]);
section1.length = Convert.ToDouble(section.Split(',')[5 * k + 6]);
section1.angle = Convert.ToDouble(section.Split(',')[5 * k + 7]);
section1.getlength();
total.intsection1.linksin[i].lanes[j].length += section1.length;
//section1.getangle();
section1.laneid = total.intsection1.linksin[i].lanes[j].ID;
section1.linkid = total.intsection1.linksin[i].ID;
}
total.intsection1.linksin[i].lanes[j].sections.Add(section1);
}
}
int b1 = 0;
for (int i = 0; i < linkout.Length; i++)
{
for (int j = 0; j < linkout[i]; j++)
{
b1 += linkout[i];
string section = Convert.ToString(intersectionStr.Split('|')[9 + a1 + b1]);
int section_num = Convert.ToInt32(section.Split(',')[0]);
section section1 = new section();
for (int k = 0; k < section_num; k++)
{
section1.id = Convert.ToInt32(section.Split(',')[5 * k + 1]);
section1.start.lonti = Convert.ToDouble(section.Split(',')[5 * k + 2]);
section1.start.lati = Convert.ToDouble(section.Split(',')[5 * k + 3]);
section1.end.lonti = Convert.ToDouble(section.Split(',')[5 * k + 4]);
section1.end.lati = Convert.ToDouble(section.Split(',')[5 * k + 5]);
section1.length = Convert.ToDouble(section.Split(',')[5 * k + 6]);
section1.angle = Convert.ToDouble(section.Split(',')[5 * k + 7]);
section1.getlength();
total.intsection1.linksout[i].lanes[j].length += section1.length;
//section1.getangle();
//直接读取角度
section1.laneid = total.intsection1.linksout[i].lanes[j].ID;
section1.linkid = total.intsection1.linksout[i].ID;
}
total.intsection1.linksout[i].lanes[j].sections.Add(section1);
}
}
intersectionStr = sr.ReadLine();
}
sr.Close();
fs.Close();
}
public void mapmatch()
{
shabi = 0;
shabi2 = 0;
shibie = 3;
List <section> candidataSections = new List <section>();
List <connector> candidatacon = new List <connector>();
for (int i = 0; i < total.intsection1.linksin.Count; i++)
{
for (int j = 0; j < total.intsection1.linksin[i].lanes.Count; j++)//1st step: direction linkin
{
for (int k = 0; k < total.intsection1.linksin[i].lanes[j].sections.Count; k++)
{
if (Math.Abs(status1.angle - total.intsection1.linksin[i].lanes[j].sections[k].angle) < 15)
{
candidataSections.Add(total.intsection1.linksin[i].lanes[j].sections[k]);
}
}
}
}
for (int i = 0; i < total.intsection1.linksout.Count; i++)
{
for (int j = 0; j < total.intsection1.linksout[i].lanes.Count; j++)//1st step: direction linkout
{
for (int k = 0; k < total.intsection1.linksout[i].lanes[j].sections.Count; k++)
{
if (Math.Abs(status1.angle - total.intsection1.linksout[i].lanes[j].sections[k].angle) < 15)
{
candidataSections.Add(total.intsection1.linksout[i].lanes[j].sections[k]);
}
}
}
}
if (path1.laneend != null && path1.lanestart != null)//?
{
if (path1.lanestart.sections.Count > 0 && path1.laneend.sections.Count > 0)
{
if (Math.Abs(status1.angle - path1.lanestart.sections[path1.lanestart.sections.Count - 1].angle) < 90 || status1.angle - path1.laneend.sections[0].angle < 90)
{
candidatacon.Add(path1.con);
}
}
}
if (candidataSections.Count + candidatacon.Count == 0)//没有符合的话
{
status1.lane1.ID = 0;
status1.linkid = 0;
status1.sectionid = 0;
status1.y = 0;
shibie = 3;
}
else
{
double minDiff = 20;
for (int i = 0; i < candidataSections.Count; i++)
{
if (candidataSections[i].geterror(status1.position) < 20 && candidataSections[i].geterror(status1.position) < minDiff)//距离匹配 误差比较满足要求
{
status1.lane1.ID = candidataSections[i].laneid;
for (int j = 0; j < total.intsection1.linksin.Count; j++)
{
for (int k = 0; k < total.intsection1.linksin[j].lanes.Count; k++)
{
if (status1.lane1.ID == total.intsection1.linksin[j].lanes[k].ID)
{
path1.lanestart = total.intsection1.linksin[j].lanes[k];
status1.lane1 = total.intsection1.linksin[j].lanes[k];
shibie = 1;//在进口道
}
}
}
if (shibie != 1)
{
for (int j = 0; j < total.intsection1.linksout.Count; j++)
{
for (int k = 0; k < total.intsection1.linksout[j].lanes.Count; k++)
{
if (status1.lane1.ID == total.intsection1.linksout[j].lanes[k].ID)
{
status1.lane1 = total.intsection1.linksout[j].lanes[k];
shabi = total.intsection1.linksout[j].lanes[k].ID;
shabi2 = status1.lane1.ID;
shibie = 2;//在出口道
}
}
}
}
status1.linkid = candidataSections[i].linkid;
status1.sectionid = candidataSections[i].id;
status1.y = 0;
minDiff = candidataSections[i].geterror(status1.position);
}
}
section section1 = new section();
if (path1.lanestart.sections.Count > 0 && path1.laneend.sections.Count > 0)
{
if (section1.getdistance(status1.position, path1.lanestart.sections[0].start) > path1.lanestart.length && section1.getdistance(status1.position, path1.laneend.sections[path1.laneend.sections.Count - 1].end) > path1.laneend.length)
{
if (path1.con.geterror(status1.position) < 10)
{
shibie = 0;//在connector
status1.conid = path1.con.ID;
}
}
}
}
}
