/// <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);
}
/// <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);
}
// 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;
}
public double getUTMDistance(double x, double y)
{
return(CoordinateConverter.getUTMDistance(mX, mY, x, y));
}
/// <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
}
/// <summary>
/// 求点与多条线段的最短距离，如果投影在多条线段内，返回t,否则f
/// </summary>
/// <param name="x">点</param>
/// <param name="y">点</param>
/// <param name="xa">多线段</param>
/// <param name="ya"></param>
/// <param name="ma"></param>
/// <param name="fromid">多线段起点下标</param>
/// <param name="toid">终点下标</param>
/// <param name="m">最近点</param>
/// <param name="dis">距离</param>
/// <returns></returns>
private static bool getDistance(double x, double y, double[] xa, double[] ya, double[] ma, int fromid, int toid, out double m, out double dis)
{
bool found = false;
m = dis = 0;
double lx = xa[fromid];
double ly = ya[fromid];
double rx = xa[toid];
double ry = ya[toid];
if ((x - lx) * (rx - lx) + (y - ly) * (ry - ly) < 0)
{
m = ma[fromid];
dis = CoordinateConverter.getUTMDistance(x, y, lx, ly);
}
else if ((x - rx) * (lx - rx) + (y - ry) * (ly - ry) < 0)
{
m = ma[toid];
dis = CoordinateConverter.getUTMDistance(x, y, rx, ry);
}
else
{
found = true;
int minid = fromid;
dis = CoordinateConverter.getUTMDistance(x, y, lx, ly);
for (int i = fromid + 1; i <= toid; i++)
{
double di = CoordinateConverter.getUTMDistance(x, y, xa[i], ya[i]);
if (di < dis)
{
dis = di;
minid = i;
}
}
int lid = minid - 1;
if (lid < fromid)
{
lid = fromid;
}
int rid = minid + 1;
if (rid > toid)
{
rid = toid;
}
double d1 = CoordinateConverter.getUTMDistance(x, y, xa[lid], ya[lid]);
double d2 = CoordinateConverter.getUTMDistance(x, y, xa[rid], ya[rid]);
double ddd = d1 / (d1 + d2);
//if (d1 > d2)
//{
// ddd = disInterpolate(x, y, xa[lid], ya[lid], xa[rid], ya[rid]);
//}
//else
//{
// ddd = 1 - disInterpolate(x,y,xa[rid],ya[rid],xa[lid],ya[lid]);
//}
m = ma[lid] + (ma[rid] - ma[lid]) * ddd;
double xm = xa[lid] + (xa[rid] - xa[lid]) * ddd;
double ym = ya[lid] + (ya[rid] - ya[lid]) * ddd;
dis = CoordinateConverter.getUTMDistance(x, y, xm, ym);
}
return(found);
}
private void button1_Click(object sender, EventArgs e)
{
double x1, y1, x2, y2;
x1 = 119;
x2 = 120;
y1 = y2 = 35;
Console.WriteLine("x:" + CoordinateConverter.getUTMDistance(x1, y1, x2, y2));
Console.WriteLine("y:" + CoordinateConverter.getUTMDistance(x1, y1, x1, y2 + 1));
string fileName = @"D:\sevenParamsTest.xlsx";
//if (chainList.Count > 0)
// chainList.Clear();
//LoadDataTableFromExcel(fileName, "Sheet1");
int ptNum = 10;
double[,] X = MatrixTool.Init(3 * ptNum, 1);
double[,] Y = MatrixTool.Init(3 * ptNum, 1);
double[,] Y84 = MatrixTool.Init(3 * ptNum, 1);
int pN = 10;
double[] XX = new double[pN * 3];
double[] YYRead = new double[pN * 3];
double[] YY = new double[pN * 3];
double[] YY84 = new double[pN * 3];
int i = 0;
int j = 0;
int k = 0;
foreach (DataRow dr in ds.Tables["Sheet1"].Rows)
{
if (i % 20 == 0 && j < ptNum)
{
X[j * 3 + 1, 0] = Convert.ToDouble(dr["North"]);
X[j * 3 + 0, 0] = Convert.ToDouble(dr["East"]);
X[j * 3 + 2, 0] = Convert.ToDouble(dr["Altitude"]);
Y84[j * 3 + 0, 0] = Convert.ToDouble(dr["Longitude"]);
Y84[j * 3 + 1, 0] = Convert.ToDouble(dr["Latitude"]);
Y84[j * 3 + 2, 0] = Convert.ToDouble(dr["Altitude"]);
j++;
}
if (i % 20 == 10 && k < ptNum)
{
XX[k * 3 + 1] = Convert.ToDouble(dr["North"]);
XX[k * 3 + 0] = Convert.ToDouble(dr["East"]);
XX[k * 3 + 2] = Convert.ToDouble(dr["Altitude"]);
YYRead[k * 3 + 0] = Convert.ToDouble(dr["Longitude"]);
YYRead[k * 3 + 1] = Convert.ToDouble(dr["Latitude"]);
YYRead[k * 3 + 2] = Convert.ToDouble(dr["Altitude"]);
k++;
}
i++;
if (j == ptNum && k == ptNum)
{
break;
}
}
for (i = 0; i < ptNum; i++)
{
CoordinateConverter.LatLonToUTMXY(Y84[3 * i + 1, 0], Y84[3 * i + 0, 0], out Y[3 * i + 0, 0], out Y[3 * i + 1, 0]);
}
CoordTrans7Param ct = new CoordTrans7Param();
ct.CalculateTrans7Param(X, Y);
for (i = 0; i < pN; i++)
{
ct.TransCoord(XX[i * 3 + 0], XX[i * 3 + 1], XX[i * 3 + 2], out YY[i * 3 + 0], out YY[i * 3 + 1], out YY[i * 3 + 2]);
CoordinateConverter.UTMXYToLatLon(YY[i * 3 + 0], YY[i * 3 + 1], out YY84[i * 3 + 1], out YY84[i * 3 + 0]);
Console.WriteLine("准确的 x {0}, y {1}, z{2}, 计算的 x {3} , y {4} , z {5}", YYRead[i * 3 + 0], YYRead[i * 3 + 1], YYRead[i * 3 + 2], YY84[i * 3 + 0], YY84[i * 3 + 1], YY[i * 3 + 2]);
}
}
private void button2_Click1(object sender, EventArgs e)
{
double x1, y1, x2, y2;
x1 = 119;
x2 = 120;
y1 = y2 = 35;
Console.WriteLine("x:" + CoordinateConverter.getUTMDistance(x1, y1, x2, y2));
Console.WriteLine("y:" + CoordinateConverter.getUTMDistance(x1, y1, x1, y2 + 1));
string fileName = @"D:\sevenParamsTest.xlsx";
//if (chainList.Count > 0)
// chainList.Clear();
//LoadDataTableFromExcel(fileName, "Sheet1");
int ptNum = 5;
double[,] X = MatrixTool.Init(2 * ptNum, 1);
double[,] Y = MatrixTool.Init(2 * ptNum, 1);
int pN = 10;
double[] XX = new double[pN * 2];
double[] YYRead = new double[pN * 2];
double[] YY = new double[pN * 2];
int i = 0;
int j = 0;
int k = 0;
foreach (DataRow dr in ds.Tables["Sheet1"].Rows)
{
if (i % 40 == 0 && j < ptNum)
{
X[j * 2 + 0, 0] = Convert.ToDouble(dr["North"]);
X[j * 2 + 1, 0] = Convert.ToDouble(dr["East"]);
Y[j * 2 + 0, 0] = Convert.ToDouble(dr["Longitude"]);
Y[j * 2 + 1, 0] = Convert.ToDouble(dr["Latitude"]);
j++;
}
if (i % 20 == 10 && k < pN)
{
XX[k * 2 + 0] = Convert.ToDouble(dr["North"]);
XX[k * 2 + 1] = Convert.ToDouble(dr["East"]);
YYRead[k * 2 + 0] = Convert.ToDouble(dr["Longitude"]);
YYRead[k * 2 + 1] = Convert.ToDouble(dr["Latitude"]);
k++;
}
i++;
if (j == ptNum && k == pN)
{
break;
}
}
double cx1, cy1, cx2, cy2;
cx1 = cy1 = cx2 = cy2 = 0;
double sx = 91310;
double sy = 111000;
for (i = 0; i < pN; i++)
{
cx1 += XX[2 * i];
cy1 += XX[2 * i + 1];
cx2 += YYRead[2 * i];
cy2 += YYRead[2 * i + 1];
}
cx1 /= pN;
cy1 /= pN;
cx2 /= pN;
cy2 /= pN;
for (i = 0; i < ptNum; i++)
{
X[2 * i, 0] = X[2 * i, 0] - cx1;
X[2 * i + 1, 0] = X[2 * i + 1, 0] - cy1;
Y[2 * i, 0] = (Y[2 * i, 0] - cx2) * sx;
Y[2 * i + 1, 0] = (Y[2 * i + 1, 0] - cy2) * sy;
}
for (i = 0; i < pN; i++)
{
XX[2 * i] = XX[2 * i] - cx1;
XX[2 * i + 1] = XX[2 * i + 1] - cy1;
}
CoordTrans4Param ct = new CoordTrans4Param();
ct.CalculateTrans4Param(X, Y);
for (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]);
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);
}
}
