public double getdistance(GPSpoint p, GPSpoint h)//计算GPS点之间的距离 弧线
{
double radLat1 = rad(p.lati);
double radLat2 = rad(h.lati);
double a = radLat1 - radLat2;
double b = rad(p.lonti) - rad(h.lonti);
double s = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) +
Math.Cos(radLat1) * Math.Cos(radLat2) * Math.Pow(Math.Sin(b / 2), 2)));
s = s * EARTH_RADIUS;
s = Math.Round(s * 10000) / 10000;
return(s);
}
public double getdistance(GPSpoint p, GPSpoint h)
{
double s = Math.Sqrt((Math.Pow(p.lati - h.lati, 2.0) + Math.Pow(p.lonti - h.lonti, 2.0))) * 10.0; //相对距离放大十倍
return(s);
}
public double geterror(GPSpoint e)
{
double err = getdistance(e, lanestart.sections[lanestart.sections.Count - 1].end) + getdistance(e, laneend.sections[0].start) - getdistance(lanestart.sections[lanestart.sections.Count - 1].end, laneend.sections[0].start);
return(err);
}
public double geterror(GPSpoint e)
{
double err = getdistance(e, start) + getdistance(e, end) - getdistance(start, end);
return(err);
}