/// <summary>
/// 计算两GPS点距离，返回以米为单位
/// </summary>
/// <param name="longitude1"></param>
/// <param name="latitude1"></param>
/// <param name="longitude2"></param>
/// <param name="latitude2"></param>
/// <returns></returns>
public static double getUTMDistance(double longitude1, double latitude1, double longitude2, double latitude2)
{
double len = 0;
double xx1, yy1, xx2, yy2;
//int z1, z2;
//CoordinateConverter.UTMXYToLatLon(517670.66484, 4032205.24460, out xx, out yy, 50, false);
CoordinateConverter.LatLonToUTMXY(latitude1, longitude1, out xx1, out yy1);
CoordinateConverter.LatLonToUTMXY(latitude2, longitude2, out xx2, out yy2);
len = Math.Sqrt((xx1 - xx2) * (xx1 - xx2) + (yy1 - yy2) * (yy1 - yy2));
return(len);
}
////获取特定点的邻居数列
public static List <point> get_Neighbour(point thePoint)
{
double distance;
List <point> neighbour = new List <point>();
foreach (point points in setOfPoints)
{
//distance = dist(thePoint, points);
distance = CoordinateConverter.getUTMDistance(thePoint.longitude, thePoint.latitude, points.longitude, points.latitude);
if (distance <= Eps)
{
neighbour.Add(points);
}
}
neighbour.Remove(thePoint);
return(neighbour);
}
/// <summary>
/// 根据经纬度，计算半径radius距离内的所有单位工程，按距离排序输出
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="radius"></param>
/// <param name="firmList"></param>
/// <returns></returns>
public Dictionary <CRailwayDWProj, double> getNearDWProj(double x, double y, double radius, List <CRailwayDWProj> dwProjectList)
{
Dictionary <CRailwayDWProj, double> dict = new Dictionary <CRailwayDWProj, double>(dwProjectList.Count);
double len;
//getNearPos(x, y, out mm, out xx, out yy, out zz, out dir);
for (int i = 0; i < dwProjectList.Count; i++)
{
len = CoordinateConverter.getUTMDistance(dwProjectList[i].mLongitude_Mid, dwProjectList[i].mLatitude_Mid, x, y);
if (len < radius)
{
dict.Add(dwProjectList[i], len);
}
}
var dictSort = from d in dict orderby d.Value ascending select d;
return(dict);
}
/// <summary>
///
/// </summary>
/// <param name="x">中心点经度</param>
/// <param name="y">中心点纬度</param>
/// <param name="h">中心点高度</param>
/// <param name="r">半径，以米为单位</param>
/// <param name="angle">角度，圆周360度</param>
/// <param name="count">点的个数</param>
/// <returns>圆周点集(经纬度，高度)</returns>
public static double[] GPSPie(double x, double y, double h, double r, int angle, out int count)
{
int step = 5;
count = angle / step + 1 + (angle % step > 0 ? 1 : 0);
if (count == 1)
{
count = 0;
return(null);
}
//double[] ps = new double[] { -l / 2, w / 2, h, l / 2, w / 2, h, l / 2, -w / 2, h, -l / 2, -w / 2, h };
double[] p = new double[count * 3];
double[] pd = new double[count * 3];
double a = 0;
double astep = Math.PI / 180 * step;
double ux, uy;
int i = 0;
CoordinateConverter.LatLonToUTMXY(y, x, out ux, out uy);
// 计算圆弧点utm坐标
for (i = 0; i < 3 * (count - 1); a += astep, i += 3)
{
p[i] = r * Math.Cos(a) + ux;
p[i + 1] = r * Math.Sin(a) + uy;
pd[i + 2] = h;
}
//if (i == count){
// i --;
p[i] = r * Math.Cos(angle * Math.PI / 180) + ux;
p[i + 1] = r * Math.Sin(angle * Math.PI / 180) + uy;
pd[i + 2] = h;
//}
for (i = 0; i < count * 3; i += 3)
{
CoordinateConverter.UTMXYToLatLon(p[i], p[i + 1], out pd[i + 1], out pd[i], false);
}
return(pd);
}
/// <summary>
/// 根据里程（DK123+45），计算半径radius距离内的所有单位，按距离排序输出
/// </summary>
/// <param name="dkCode"></param>
/// <param name="radius"></param>
/// <param name="firmList"></param>
/// <returns></returns>
public Dictionary <CRailwayFirm, double> getNearFirms(string dkCode, double radius, List <CRailwayFirm> firmList)
{
//double[] lenList = new double[firmList.Count];
Dictionary <CRailwayFirm, double> dict = new Dictionary <CRailwayFirm, double>(firmList.Count);
double xx, yy, zz, dir, len;
//double xx1,yy1,zz1,dir1;
getPosbyMeter(dkCode, out xx, out yy, out zz, out dir);
for (int i = 0; i < firmList.Count; i++)
{
len = CoordinateConverter.getUTMDistance(firmList[i].CenterLongitude, firmList[i].CenterLatitude, xx, yy);
if (len < radius)
{
dict.Add(firmList[i], len);
}
}
var dictSort = from d in dict orderby d.Value ascending select d;
return(dict);
}
/// <summary>
/// 计算偏移一定角度和距离之后的经纬度
/// </summary>
/// <param name="startDKM"></param>
/// <param name="endDKM"></param>
/// <param name="stepm"></param>
/// <param name="angleOff"></param>
/// <param name="disOff"></param>
/// <param name="latout"></param>
/// <param name="lonout"></param>
/// <param name="z"></param>
/// <returns></returns>
public int getOffsetLine(string startDKM, string endDKM, double stepm, double angleOff, double disOff, out double[] latout, out double[] lonout, out double[] z)
{
lonout = latout = z = null;
double[] x = null;
double[] y = null;
//double[] z = null;
double[] d = null;
int pointNum = getSubLineByDKCode(startDKM, endDKM, stepm, out x, out y, out z, out d);
if (pointNum > 0)
{
latout = new double[pointNum];
lonout = new double[pointNum];
for (int i = 0; i < pointNum; i++)
{
CoordinateConverter.LatLonOffest(y[i], x[i], d[i], angleOff, disOff, out latout[i], out lonout[i]); //lat, lon
}
}
return(pointNum);
}
public string mDKCode; // "DK" "DIK"
// public double[]
public CRailwayLine(string dkcode, double fromM, double toM, bool isA, int num, double[] m, double[] x, double[] y, double[] z)
{
mStart = fromM;
mEnd = toM;
if (mStart < mEnd)
{
mIsReverse = false;
}
else
{
mIsReverse = true;
}
mDKCode = dkcode;
mIsAuxiliary = isA;
//mFromID = fromID;
//mToID = toID;
mLength = m[num - 1] - m[0];
mPointNum = (int)((mLength - 0.05) / 10) + 2; //FIXME 误差
meter = new double[mPointNum];
meter[0] = m[0];
meter[mPointNum - 1] = m[num - 1];
//for (int j = 0; j < num - 1; j++)
// if (m[j] >= m[j+ 1])
// Console.WriteLine("mileage error " + m[j] + "\t" + m[j+1]);
for (int i = 1; i < mPointNum - 1; i++)
{
meter[i] = meter[i - 1] + 10;
}
longitude = CubicSpline.Compute(num, m, x, meter);
latitude = CubicSpline.Compute(num, m, y, meter);
altitude = CubicSpline.Compute(num, m, z, meter);
CoordinateConverter.LatLonToYawList(mPointNum, latitude, longitude, out utmX, out utmY, out heading);
}
/// <summary>
/// 根据经纬度，计算半径radius距离内的所有工点，按距离排序输出
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="radius"></param>
/// <param name="firmList"></param>
/// <returns></returns>
public Dictionary <CRailwayProject, double> getNearProject(double x, double y, double radius, List <CRailwayProject> projectList)
{
Dictionary <CRailwayProject, double> dict = new Dictionary <CRailwayProject, double>(projectList.Count);
double len, len_start, len_mid, len_end;
//getNearPos(x, y, out mm, out xx, out yy, out zz, out dir);
for (int i = 0; i < projectList.Count; i++)
{
len_start = CoordinateConverter.getUTMDistance(projectList[i].mLongitude_Start, projectList[i].mLatitude_Start, x, y);
len_mid = CoordinateConverter.getUTMDistance(projectList[i].CenterLongitude, projectList[i].CenterLatitude, x, y);
len_end = CoordinateConverter.getUTMDistance(projectList[i].mLongitude_End, projectList[i].mLatitude_End, x, y);
len = getMinLen(len_start, len_mid, len_end);
if (len < radius)
{
dict.Add(projectList[i], len);
}
}
var dictSort = from d in dict orderby d.Value ascending select d;
return(dict);
}
// 117.11726427，36.67414252
public static void MarsToEarth(double[] marsLong, double[] marsLat, double[] earthLong, double[] earthLat,
double[] newMarsLong, double[] newMarsLat, out double[] newEarthLong, out double[] newEarthLat)
{
int ptNum = marsLong.Length;
double[,] X = MatrixTool.Init(2 * ptNum, 1);
double[,] Y = MatrixTool.Init(2 * ptNum, 1);
int pN = newMarsLong.Length;
double[] XX = new double[pN * 2];
double[] YY = new double[pN * 2];
newEarthLong = new double[pN];
newEarthLat = new double[pN];
//double[] YYRes = new double[pN * 2];
for (int i = 0; i < ptNum; i++)
{
CoordinateConverter.LatLonToUTMXY(marsLat[i], marsLong[i], out X[2 * i + 0, 0], out X[2 * i + 1, 0]);
CoordinateConverter.LatLonToUTMXY(earthLat[i], earthLong[i], out Y[2 * i + 0, 0], out Y[2 * i + 1, 0]);
CoordinateConverter.LatLonToUTMXY(newMarsLat[i], newMarsLong[i], out XX[2 * i + 0], out XX[2 * i + 1]);
}
CoordTrans4Param ct = new CoordTrans4Param();
ct.CalculateTrans4Param(X, Y);
for (int i = 0; i < pN; i++)
{
ct.TransCoord(XX[i * 2 + 0], XX[i * 2 + 1], out YY[i * 2 + 0], out YY[i * 2 + 1]);
CoordinateConverter.UTMXYToLatLon(YY[i * 2 + 0], YY[i * 2 + 1], out newEarthLat[i], out newEarthLong[i]);
//Console.WriteLine("准确的 x {0}, y {1}, 计算的 x {2} , y {3} ", YYRead[i * 2 + 0], YYRead[i * 2 + 1], YY84[i * 2 + 0], YY84[i * 2 + 1]);
//Console.WriteLine("准确的 x {0}, y {1}, 计算的 x {2} , y {3} ", YYRead[i * 2 + 0], YYRead[i * 2 + 1], YY[i * 2 + 0] / sx + cx2, YY[i * 2 + 1] / sy + cy2);
}
//return YYRes;
}
/// <summary>
/// 旧版，没有dkcode2
/// </summary>
/// <param name="idx"></param>
/// <param name="dkcode"></param>
/// <param name="fromM"></param>
/// <param name="toM"></param>
/// <param name="isA"></param>
/// <param name="isR"></param>
/// <param name="isD"></param>
/// <param name="num"></param>
/// <param name="m"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <param name="stepm"></param>
/// <param name="otherDKCode"></param>
public CRailwayLine(int idx, string dkcode, double fromM, double toM, bool isA, bool isR, bool isD, int num, double[] m, double[] x, double[] y, double[] z, double stepm = 10, string otherDKCode = "")
{
mIndex = idx;
mStart = fromM;
mEnd = toM;
if (mStart < mEnd)
{
mIsReverse = false;
}
else
{
mIsReverse = true;
}
mDKCode = dkcode;
mIsAuxiliary = isA;
mIsRight = isR;
mIsDouble = isD;
if (mIsDouble)
{
mOffset = 2.5; // 根据济青铁路轨道间距5米测算
}
else
{
mOffset = 5.4; // 线杆偏移位置
}
mStepm = Math.Max(1, Math.Abs(stepm)); //
//mFromID = fromID;
//mToID = toID;
// 利用stepM重采样,0513,FIXED，采样非10米时，mPointNum计算错误
mLength = m[num - 1] - m[0];
if (mLength < 2 * mStepm)
{
mPointNum = 2;
}
else
{
mPointNum = (int)((mLength - 2 * stepm) / stepm) + 3; //最后一段一般大于10米，避免最后一段过短，
}
meter = new double[mPointNum];
meter[0] = m[0];
meter[mPointNum - 1] = m[num - 1];
//for (int j = 0; j < num - 1; j++)
// if (m[j] >= m[j+ 1])
// Console.WriteLine("mileage error " + m[j] + "\t" + m[j+1]);
for (int i = 1; i < mPointNum - 1; i++)
{
meter[i] = meter[i - 1] + mStepm;
}
longitude = CubicSpline.Compute(num, m, x, meter);
latitude = CubicSpline.Compute(num, m, y, meter);
altitude = CubicSpline.Compute(num, m, z, meter);
//for (int i = 0; i < mPointNum - 1; i += (int)(1000/mStepm))
// mBBoxList.Add(new RectangleF((float)(longitude[i] - 0.01),(float)(latitude[i] -0.01),0.02f,0.02f));
//if (mPointNum % 100 > 20)
// mBBoxList.Add(new RectangleF((float)(longitude[mPointNum -1] - 0.01), (float)(latitude[mPointNum -1] - 0.01), 0.02f, 0.02f));
// RectangleF rec = mBBoxList[0];
//CoordinateConverter.LatLonToUTMXYList(mPointNum, latitude, longitude, out utmX, out utmY);
mOtherDKCode = otherDKCode;
CoordinateConverter.LatLonToOffsetYawList(latitude, longitude, 90, mOffset, out mOffsetX, out mOffsetY, out heading);
//CoordinateConverter.LatLonToUTMXYList(mPointNum, latitudeMars, longitudeMars, out utmXMars, out utmYMars);
//CoordinateConverter.LatLonOffest(longitude,latitude,heading,90,mOffset,out mOffsetX,out mOffsetY );
#if DEBUG
validateHeading();
#endif
}
public double getUTMDistance(double x, double y)
{
return(CoordinateConverter.getUTMDistance(mX, mY, x, y));
}
//// 给定里程，计算经纬度朝向坐标
//public CRWPosition getPosbyMeter(double m)
//{
// CRWPosition pos = new CRWPosition();
// int index = 0;
// //x = y = z = 0; dir = 0;
// if (m < start)
// {
// pos.longitude = longitude[0];
// pos.latitude = latitude[0];
// pos.altitude = altitude[0];
// if (heading != null)
// pos.heading = heading[0];
// return pos;
// }
// if (m >= end)
// {
// pos.longitude = longitude[mPointNum - 1];
// pos.latitude = latitude[mPointNum - 1];
// pos.altitude = altitude[mPointNum - 1];
// if (heading != null)
// pos.heading = heading[mPointNum - 1];
// return pos;
// }
// int nm = (int)m;
// if (nm % 10 == 9)
// {
// index = nm / 10 + 1;
// pos.longitude = longitude[index];
// pos.latitude = latitude[index];
// pos.altitude = altitude[index];
// if (heading != null)
// pos.heading = heading[index];
// }
// else
// {
// index = nm / 10;
// if (nm % 10 == 0)
// {
// pos.longitude = longitude[index];
// pos.latitude = latitude[index];
// pos.altitude = altitude[index];
// if (heading != null)
// pos.heading = heading[index];
// }
// else
// {
// double t = (m - nm) / 10;
// pos.longitude = longitude[index] * (1 - t) + longitude[index + 1] * t;
// pos.latitude = latitude[index] * (1 - t) + latitude[index + 1] * t;
// pos.altitude = altitude[index] * (1 - t) + altitude[index + 1] * t;
// if (heading != null)
// pos.heading = heading[index] * (1 - t) + heading[index + 1] * t;
// }
// }
// return pos;
//}
/// <summary>
/// 给定经纬度，输出里程与距离
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="mileage"></param>
/// <param name="distance"></param>
public void getMeterbyPos(double x, double y, out double mileage, out double distance)
{
int errorNum = 0;
double ux, uy;
CoordinateConverter.LatLonToUTMXY(y, x, out ux, out uy);
distance = 0;
mileage = 0;
try
{
mileage = 0;
int count = mPointNum;
double mindist = (utmX[0] - ux) * (utmX[0] - ux) +
(utmY[0] - uy) * (utmY[0] - uy);
double dist = 10;
int step = (int)Math.Sqrt(count) + 1;
//int i;
int index = 0;
for (int i = step; i < count; i += step)
{
dist = (utmX[i] - ux) * (utmX[i] - ux) + (utmY[i] - uy) * (utmY[i] - uy);
if (dist < mindist)
{
mindist = dist;
index = i;
}
errorNum = i;
}
int index2 = index;
int j = index - step;
if (j < 0)
{
j = 0;
}
for (; j < index + step && j < count; j++)
{
dist = (utmX[j] - ux) * (utmX[j] - ux) + (utmY[j] - uy) * (utmY[j] - uy);
if (dist < mindist)
{
mindist = dist;
index2 = j;
}
errorNum = -j;
}
if (index2 > 0 && index2 < count - 1)
{
double d1, d2;
d1 = Math.Sqrt(Math.Pow(utmX[index2 - 1] - ux, 2) + Math.Pow(utmY[index2 - 1] - uy, 2));
d2 = Math.Sqrt(Math.Pow(utmX[index2 + 1] - ux, 2) + Math.Pow(utmY[index2 + 1] - uy, 2));
mileage = meter[index2 - 1] + (meter[index2 + 1] - meter[index2 - 1]) * d1 / (d1 + d2);
}
else if (index2 == 0)
{
double d1, d2;
d1 = Math.Sqrt(Math.Pow(utmX[0] - ux, 2) + Math.Pow(utmY[0] - uy, 2));
d2 = Math.Sqrt(Math.Pow(utmX[1] - ux, 2) + Math.Pow(utmY[1] - uy, 2));
mileage = (meter[1] - meter[0]) * d1 / (d1 + d2);
}
else
{
double d1, d2;
d1 = Math.Sqrt(Math.Pow(utmX[count - 2] - ux, 2) + Math.Pow(utmY[count - 2] - uy, 2));
d2 = Math.Sqrt(Math.Pow(utmX[count - 1] - ux, 2) + Math.Pow(utmY[count - 1] - uy, 2));
mileage = meter[count - 2] + (meter[count - 1] - meter[count - 2]) * d1 / (d1 + d2);
}
double mx, my, mz, md;
getPosbyLocalMeter(mileage, out mx, out my, out mz, out md);
if (mIsReverse)
{
mileage = mStart - mileage;
}
else
{
mileage = mStart + mileage;
}
distance = CoordinateConverter.getUTMDistance(mx, my, x, y);
//return mileage;
}
catch (Exception e)
{
Console.WriteLine("error num" + errorNum);
//return -1;
}
}
