/// <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); } }