挖泥船舶定位程序

来源：互联网发布：java实现发邮件带附件编辑：程序博客网时间：2024/04/29 01:04
// LonLat2.c : 定义控制台应用程序的入口点。//#include <stdio.h>#include <math.h>/***********************************************************************************函数：CalOffset5PtsFromGPS*功能：以施工船舶GPS为原点，计算船中心和四个锚位的相对位置（偏移量），*以GPS为中心，船首平行方向为y轴正方向，正横为x轴正方向*作者：chunhui zhou*日期：2017 Feb***********************************************************************************/int CalOffset5PtsFromGPS(double A, double B,double C, double D,double L, double W,    double x[5], double y[5])//第0个点为船中心点，剩下4个点为锚点。{    double x0, y0;    if(A <=0 || B <=0 || C <=0 || D <=0 || L <=0 || W <=0 )        return -1;    //船中心坐标相对GPS的偏移量，以GPS为中心，船首平行方向为y轴正方向，正横为x轴正方向。    x0 = x[0] = (D-C)/2;    y0 = y[0] = (A-B)/2;    x[1] =x0+W;    y[1] =y0+L;    x[2] =x0+W;    y[2] =y0-L;    x[3] =x0-W;    y[3] =y0-L;    x[4] =x0-W;    y[4] =y0+L;    return 0;}/******************************************************************************************函数：CalRotate5PtsFromGPS*功能：以施工船舶GPS为原点，计算船中心和四个锚位的旋转后的位置（偏移量），Heading为船首向*以GPS为中心，坐标系旋转了一个角度Heading；假设坐标系不变，相当于各点旋转了(－Heading)*作者：chunhui zhou*日期：2017 Feb*******************************************************************************************/int CalRotate5PtsFromGPS(double Heading, double x[5], double y[5])//第0个点为船中心点，剩下4个点为锚点。{    int i;    double tempx, tempy;    double a = Heading * 3.1415926 /180.0;//转为弧度    for(i = 0; i < 5; i++)    {        tempx=x[i]*cos(-a)-y[i]*sin(-a);        tempy=x[i]*sin(-a)+y[i]*cos(-a);        x[i] = tempx;        y[i] = tempy;    }    return 0;}/******************************************************************************************函数：CalLatLon5PtsFromGPS*功能：已知施工船舶GPS经纬度坐标，计算船中心和四个锚位的偏移量（单位为米）*求船中心和四个锚位的经纬度坐标，供显示虚拟航标用（包括GPS共可得到6个经纬度坐标）。*作者：chunhui zhou*日期：2017 Feb*******************************************************************************************/int CalLatLon5PtsFromGPS(double LatGPS, double LonGPS, double x[5], double y[5],    //第0个点为船中心点，剩下4个点为锚点。       double Lat[5], double Lon[5])//注意纬度在前{    double offsetLon, offsetLat, Dep;    int i;    for(i = 0; i < 5; i++)    {        offsetLat = y[i]/1852.0/60.0;        Dep = x[i]/1852.0/60.0;        offsetLon = Dep/cos(LatGPS*3.1415926/180);        Lon[i] = LonGPS + offsetLon;        Lat[i] = LatGPS + offsetLat;    }    return 0;}int main(int argc, char* argv[]){    double x[5], y[5], Lon[5], Lat[5];    int i;    double A, B, C, D, L, W;    double Heading = 90.0;    double LatGPS = 30.0, LonGPS = 120.0;    A=100, B = 30, C = 5, D = 10;    L = 200, W=100;    CalOffset5PtsFromGPS(A, B, C, D, L, W, x, y);    printf("\n偏移量为：\n");    for(i = 0; i < 5; i++)        printf("%.5f, %.5f\n", x[i], y[i]);    CalRotate5PtsFromGPS(Heading, x, y);    printf("\n旋转后%3.1f度航向角后的偏移量为：\n", Heading);    for(i = 0; i < 5; i++)        printf("%.5f, %.5f\n", x[i], y[i]);    CalLatLon5PtsFromGPS(LatGPS,LonGPS, x, y, Lat, Lon);    printf("\nGPS经纬度为：%3.5f %3.5f\n", LatGPS, LonGPS);    printf("其它经纬度为：\n");    for(i = 0; i < 5; i++)        printf("%.5f, %.5f\n", Lat[i], Lon[i]);    return 0;}
0 0