public double Getdistance(_soPoint goal, soGeoPoint V) { double deltax = goal.x - V.x; double deltay = goal.y - V.y; return(Math.Sqrt(deltax * deltax + deltay * deltay));//得到距离 }
//将平面坐标转换为车体坐标系 public _soPoint tranfers(double x, double y, double azimuth) { double resultX, resultY; long finalX, finalY; double cosa = Math.Cos(azimuth * Math.PI / 180); double sina = Math.Sin(azimuth * Math.PI / 180); resultX = x * cosa - y * sina; resultY = x * sina + y * cosa; finalX = (long)(resultX * 50.0 + 1500); finalY = (long)(resultY * 50.0 + 1000); _soPoint temp = new _soPoint(); temp.x = finalX; temp.y = finalY; return(temp); }
/********* 根据车辆航向角将大地坐标差值转化成载体坐标系下的坐标 ************ * 输入:导航点大地坐标与车辆大地坐标的坐标差,航向角(度为单位) * 输出:在下面的载体坐标系的导航点的坐标(3000*3000) * 载体坐标定义:车辆左下为(0,0),右上为(60,60),车辆中心点(30,20) * **************************************************************************/ public _soPoint ComputeBodyRelativeCoorAccordintCoordiateDiffToUGVCenter(double x, double y, double azimuth) { double resultX, resultY; long finalX, finalY; double cosa = Math.Cos(azimuth * Math.PI / 180); double sina = Math.Sin(azimuth * Math.PI / 180); //坐标旋转(大地->载体)载体中心坐标(0,0) resultX = x * cosa - y * sina; resultY = x * sina + y * cosa;//坐标转换 //将坐标转换为载体中心坐标(30,20),并将(60,60)转换为(3000,3000) finalX = (long)(resultX * 50.0 + 1500); finalY = (long)(resultY * 50.0 + 1000); FlagPointOut60L60WFrame = false; if (finalX < 1) { finalX = 1; FlagPointOut60L60WFrame = true; } else if (finalX > 2999) { finalX = 2999; FlagPointOut60L60WFrame = true; } if (finalY < 1) { finalY = 1; FlagPointOut60L60WFrame = true; } else if (finalY > 2999) { finalY = 2999; FlagPointOut60L60WFrame = true; } _soPoint temp = new _soPoint(); temp.x = finalX; temp.y = finalY; return(temp); }
/************** 根据当前点以及航向角计算载体坐标系的导航点坐标 **************** * 输入:(1) 当前点CurrentPoint (2) 导航点NavigationPoints (3) 航向 * 输出:x[301],y[301] 共301个点,车中心点坐标(30*50,20*50),左下为(0,0),右上为(60*50,60*50); * *************************************************************/ public void ComputeNavigationPointCoordinateToBeSent(_soPoint CurrentPoint, RoadModel NavigationPoints, double azimuth) { long j = 0; start_index = 0; end_index = 0; FlagPointOut60L60WFrame = false; long index = 0; //int road_index = 0; if (NavigationPoints.WholeNumberOfNavigationPoints == 0) { return; } long sum = (NavigationPoints.end_search_index + NavigationPoints.WholeNumberOfNavigationPoints - NavigationPoints.start_search_index) % (NavigationPoints.WholeNumberOfNavigationPoints); for (long i = 0; i < sum; i++) { if (j < 1000)//数据定义为1000 { resultx[j] = NavigationPoints.x_new[(i + NavigationPoints.start_search_index) % (NavigationPoints.WholeNumberOfNavigationPoints)]; resulty[j] = NavigationPoints.y_new[(i + NavigationPoints.start_search_index) % (NavigationPoints.WholeNumberOfNavigationPoints)]; j++; } else { break; } } /********** 对当前点向车后方的点进行赋值 ***********/ //index-当前在result中的序号 index = (NavigationPoints.present_point_index - NavigationPoints.start_search_index + NavigationPoints.WholeNumberOfNavigationPoints) % (NavigationPoints.WholeNumberOfNavigationPoints); //对未出60*60矩形框的点进行转换 while ((FlagPointOut60L60WFrame == false) && index > -1 && index < 1000) { //根据车辆航向角将输入坐标相对车辆坐标的坐标差值(大地坐标)转化成载体坐标系下的坐标 temp_point = ComputeBodyRelativeCoorAccordintCoordiateDiffToUGVCenter(resultx[index] - CurrentPoint.x, resulty[index] - CurrentPoint.y, azimuth); x_temp[index] = Convert.ToInt32(temp_point.x); y_temp[index] = Convert.ToInt32(temp_point.y); index--; } if (index > 0) { start_index = index + 1; } else { start_index = 0; } /********** 对当前点向车前方的点进行赋值 ***********/ index = (NavigationPoints.present_point_index - NavigationPoints.start_search_index + NavigationPoints.WholeNumberOfNavigationPoints) % (NavigationPoints.WholeNumberOfNavigationPoints); FlagPointOut60L60WFrame = false; while ((!FlagPointOut60L60WFrame) && index < (NavigationPoints.end_search_index - NavigationPoints.start_search_index + NavigationPoints.WholeNumberOfNavigationPoints) % NavigationPoints.WholeNumberOfNavigationPoints && index < 1000) { temp_point = ComputeBodyRelativeCoorAccordintCoordiateDiffToUGVCenter(resultx[index] - CurrentPoint.x, resulty[index] - CurrentPoint.y, azimuth); x_temp[index] = Convert.ToInt32(temp_point.x); y_temp[index] = Convert.ToInt32(temp_point.y); index++; } end_index = index - 1; if ((end_index - start_index < 301) && (end_index - start_index >= 0)) { for (long i = start_index; i < end_index; i++) { if (i < 1000) { xNavigationPointCoordianteToBeSent[i - start_index] = x_temp[i]; yNavigationPointCoordianteToBeSent[i - start_index] = y_temp[i]; } } /********** 当点数小于300个时,用最后一个点值填充后续所有点 ***********/ for (long i = end_index; i < 301 + start_index; i++) { if (i < 1000) { xNavigationPointCoordianteToBeSent[i - start_index] = x_temp[end_index]; yNavigationPointCoordianteToBeSent[i - start_index] = y_temp[end_index]; } } } else { for (long i = start_index; i < start_index + 301; i++) { if (i < 1000) { xNavigationPointCoordianteToBeSent[i - start_index] = x_temp[i]; yNavigationPointCoordianteToBeSent[i - start_index] = y_temp[i]; } } } }