296 lines
9.5 KiB
C++
296 lines
9.5 KiB
C++
// TrackPoints.cpp : Defines the initialization routines for the DLL.
|
|
//
|
|
#include "TrackPoints.h"
|
|
#include "stdio.h"
|
|
#include "stdlib.h"
|
|
#include "qmath.h"
|
|
#include "memory.h"
|
|
#include "string.h"
|
|
using namespace std;
|
|
// extern "C"
|
|
// {
|
|
// __declspec (dllexport) int TrackPoints(double nDevi,double nAzim,double rlev,double &XE,double &YN,double &ZD,double &HOFF0,double &HOAZ0,double >D0,double &AZIM0);
|
|
// __declspec (dllexport) int initTrackPoints(double azca,double tvd,double dxe,double dyn,double rlev=0,double GLDDDEP0=30,double hoff=0);
|
|
// }
|
|
double AZA0=0 ; //主投影方位
|
|
double AZCA=0; //磁偏角 AZCA
|
|
//double DXS;//初始值
|
|
//double DYS;//初始值
|
|
double TVDS=0;//初始值
|
|
double DZDA=0;//全局变量
|
|
double FLSA=0; //FLSA=0.0为最小曲率法 FLSA=1.0为折线法
|
|
double PI=3.1415926f;
|
|
double RLEA=0.125; //采样间隔
|
|
double FLAG=1; //全局变量
|
|
double XE=0; //东西位移
|
|
double YN=0; //南北位移
|
|
double ZD=0; //垂直深度
|
|
|
|
float OLDDEVI=0;
|
|
float OLDAZIM=0;
|
|
|
|
double DEV1=0,AZIM1=0,DEV2=0,AZIM2=0;//全局变量
|
|
double HOFF=0,HOAZ=0;//叫水平位移,后者叫闭合方位
|
|
double rlev=0.125;
|
|
double oldnDevi=0;
|
|
double oldnAzim=0;
|
|
double oldnDevi1[1000];
|
|
double oldnAzim1[1000];
|
|
double GLDDDEP=30;
|
|
double DEP0=0;
|
|
int no=0;
|
|
int NON=1;
|
|
int initTrackPoints(double azca,double tvd,double dxe,double dyn,double rlev0,double GLDDDEP0,double hoff)
|
|
{
|
|
//====================================================================
|
|
AZA0=180 ; //主投影方位
|
|
AZCA=azca; //磁偏角 AZCA
|
|
TVDS=tvd;//初始值?????????
|
|
DZDA=0;//全局变量
|
|
FLSA=0; //FLSA=0.0为最小曲率法 FLSA=1.0为折线法
|
|
FLAG=0; //全局变量
|
|
XE=dyn;
|
|
YN=dxe;//井斜x,y坐标
|
|
DEV1=0,AZIM1=0,DEV2=0,AZIM2=0;//全局变量
|
|
HOFF=hoff,HOAZ=0;//叫水平位移,后者叫闭合方位
|
|
ZD=tvd;//-w.earthEle-w.dEle ; //??????????
|
|
OLDDEVI=0;
|
|
OLDAZIM=0;
|
|
oldnDevi=0;
|
|
oldnAzim=0;
|
|
memset(oldnDevi1,0,sizeof(double)*1000);
|
|
memset(oldnAzim1,0,sizeof(double)*1000);
|
|
GLDDDEP=GLDDDEP0;
|
|
DEP0=0;
|
|
no=0;
|
|
rlev=rlev0;
|
|
if(rlev) NON=GLDDDEP/rlev+0.5;
|
|
if(NON<1) NON=1;
|
|
return 1;
|
|
}
|
|
|
|
int TrackPoints(double nDevi,double nAzim,double RLEA,double &XE0,double &YN0,double &ZD0,double &HOFF0,double &HOAZ0,double >D0,double &AZIM0)
|
|
{
|
|
if(no<0) no=0;
|
|
if(nDevi!=-9999.0&&nDevi!=-999.25&&nDevi!=-99999.0)
|
|
{
|
|
oldnDevi=nDevi;
|
|
oldnAzim=nAzim;
|
|
}
|
|
double DEVI=oldnDevi;
|
|
double AZIM=oldnAzim;
|
|
if(RLEA==0) RLEA=rlev;
|
|
if(AZCA+nAzim<0) AZIM=AZCA+nAzim+360;
|
|
else AZIM=AZCA+nAzim;
|
|
double GTD=0;
|
|
|
|
if(FLAG==1)
|
|
{
|
|
ZD=ZD+(1-pow((DEVI-OLDDEVI)*PI/180,2)/24.0)*RLEA*cos((OLDDEVI+DEVI)*PI/360);
|
|
XE=XE+(1-(pow((DEVI-OLDDEVI)*PI/180,2)+
|
|
pow(((AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:
|
|
(AZIM-OLDAZIM>180?AZIM-OLDAZIM-360:AZIM-OLDAZIM)))*PI/180,2))/24)*RLEA
|
|
*sin((DEVI+OLDDEVI)/2*PI/180)*cos(
|
|
(AZIM-OLDAZIM>180?(AZIM+OLDAZIM-360)*PI/360:
|
|
(AZIM-OLDAZIM<-180?(AZIM+OLDAZIM+360)*PI/360:(AZIM+OLDAZIM)*PI/360)));
|
|
YN=YN+(1-(pow((DEVI-OLDDEVI)*PI/180,2)+pow((
|
|
(AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:
|
|
(AZIM-OLDAZIM>180,AZIM-OLDAZIM-360,AZIM-OLDAZIM)))*PI/180,2))/24)*RLEA*sin((DEVI+OLDDEVI)/2*PI/180)*sin(
|
|
(AZIM-OLDAZIM>180?(AZIM+OLDAZIM-360)*PI/360:
|
|
(AZIM-OLDAZIM<-180?(AZIM+OLDAZIM+360)*PI/360:(AZIM+OLDAZIM)*PI/360)));
|
|
HOFF=sqrt(XE*XE+YN*YN);
|
|
|
|
float val1=(XE>0)?((YN>0)?0:360):0;
|
|
float val2=(XE<0)?180:0;
|
|
|
|
HOAZ=atan(YN/XE)*180/PI+val1+val2;
|
|
if(DEP0<GLDDDEP) {
|
|
GTD=0;
|
|
}
|
|
else {
|
|
GTD=sqrt(pow((DEVI-oldnDevi1[0])/GLDDDEP,2)+
|
|
pow((AZIM-oldnAzim1[0]>180?AZIM-oldnAzim1[0]-360:
|
|
AZIM-oldnAzim1[0]<-180?AZIM-oldnAzim1[0]+360:AZIM-oldnAzim1[0])/GLDDDEP,2)*
|
|
pow(sin((DEVI+oldnDevi1[0])/2*PI/180),2))*GLDDDEP;//30
|
|
}
|
|
}
|
|
else {
|
|
HOFF=sqrt(XE*XE+YN*YN)+HOFF;
|
|
float val1=(XE>0)?((YN>0)?0:360):0;
|
|
float val2=(XE<0)?180:0;
|
|
float X=abs(XE);
|
|
float Y=abs(YN) ;
|
|
if((XE>0.0) && (YN>0.0)) HOAZ=atan2(X,Y) ;
|
|
if((XE>0.0) && (YN<0.0)) HOAZ=PI-atan2(X,Y);
|
|
if((XE<0.0) && (YN<0.0)) HOAZ=PI+atan2(X,Y);
|
|
if((XE<0.0) && (YN>0.0)) HOAZ=2*PI-atan2(X,Y) ;
|
|
if((XE==0.0) && (YN>0.0)) HOAZ=0.;
|
|
if((YN==0.0) && (XE>0.0)) HOAZ=0.5*PI;
|
|
if((XE==0.0) && (YN<0.0)) HOAZ=PI ;
|
|
if((YN==0.0) && (XE<0.0)) HOAZ=1.5*PI ;
|
|
HOAZ=(HOAZ*360.)/(2*PI) ;
|
|
GTD=0;
|
|
}
|
|
FLAG=1;
|
|
OLDDEVI=DEVI;
|
|
OLDAZIM=AZIM;
|
|
DEP0+=RLEA;
|
|
if(DEP0>GLDDDEP) DEP0=GLDDDEP;
|
|
if(rlev) {
|
|
if(no>=NON) {
|
|
no=NON-1;
|
|
memmove(&oldnDevi1[0],&oldnDevi1[1],no*sizeof(double));
|
|
memmove(&oldnAzim1[0],&oldnAzim1[1],no*sizeof(double));
|
|
}
|
|
}
|
|
oldnDevi1[no]=DEVI;
|
|
oldnAzim1[no]=AZIM;
|
|
no++;
|
|
/*
|
|
double oldnDevi=0;
|
|
double oldnAzim=0;
|
|
////////////////////////////////////////////////////////////////////////////
|
|
if(nDevi!=-9999.0)
|
|
{
|
|
oldnDevi=nDevi;
|
|
oldnAzim=nAzim;
|
|
}
|
|
|
|
|
|
double DEVI=oldnDevi;
|
|
double AZIM=oldnAzim;
|
|
double RLES,DX,DY,TVD,DDB,DDV,DDE,DDA,X,Y;
|
|
|
|
DEVI=(DEVI/180.)*PI;
|
|
AZIM= fmod(AZIM+AZCA+360.0,360.0); //AZCA=???
|
|
RLES=RLEA;
|
|
AZIM=(AZIM/360.)*(2*PI); //AZA0=???
|
|
if(RLEA==0.1524)
|
|
{
|
|
if((AZA0>=90.0) && (AZA0<180.0)) RLES=0.15241f;
|
|
if((AZA0>=180.0) && (AZA0<270.0)) RLES=0.152422f;
|
|
if((AZA0>=270.0) && (AZA0<=360.0)) RLES=0.1524333f;
|
|
if((AZA0>=0.0) && (AZA0 <15.0)) RLES=0.152388f;
|
|
if((AZA0>=15.0) && (AZA0 <90.0)) RLES=0.15242f;
|
|
}
|
|
if(RLEA==0.0762)
|
|
{
|
|
if((AZA0>=90.0) && (AZA0<140.0)) RLES=0.07623f;
|
|
if((AZA0>=140.0) && (AZA0<180.0)) RLES=0.0762332f;
|
|
if((AZA0>=180.0) && (AZA0<270.0)) RLES=0.0762331f;
|
|
if((AZA0>=270.0) && (AZA0<=360.0)) RLES=0.0762332f;
|
|
if((AZA0>=0.0) && (AZA0<15.0)) RLES=0.076188f;
|
|
if((AZA0>=15.0) && (AZA0<90.0)) RLES=0.07623325f;
|
|
}
|
|
|
|
if(FLAG==1.0) // FLAG =????
|
|
goto label_22;
|
|
FLAG=1.0;
|
|
DEV1=DEVI;
|
|
AZIM1=AZIM;
|
|
goto label_11;
|
|
label_22 :
|
|
DEV2=DEVI;
|
|
AZIM2=AZIM;
|
|
if(FLSA==0.0)
|
|
{
|
|
if((DEV2==DEV1) && (AZIM2==AZIM1)) goto label_100;
|
|
if((DEV2==DEV1) && (AZIM2!=AZIM1)) goto label_300;
|
|
if((DEV2!=DEV1) && (AZIM2==AZIM1)) goto label_200;
|
|
if ((AZIM1<=1.5*PI) && (AZIM1>=0.5*PI)) goto label_500;
|
|
if((AZIM2<=1.5*PI) && (AZIM2>=0.5*PI)) goto label_500;
|
|
if((AZIM1>1.5*PI) && (AZIM2>1.5*PI)) goto label_500;
|
|
if((AZIM1<0.5*PI) && (AZIM2<0.5*PI)) goto label_500;
|
|
if((AZIM1<0.5*PI) && (AZIM2>1.5*PI)) goto label_400;
|
|
DDA=(DEV2-DEV1)*(AZIM2+2*PI-AZIM1);
|
|
DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
|
|
DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
|
|
DX=(RLES*DDV)/DDA;
|
|
DY=(RLES*DDE)/DDA;
|
|
goto label_510;
|
|
label_400 : DDB=(DEV2-DEV1)*(AZIM2-2*PI-AZIM1);
|
|
DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
|
|
DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
|
|
DX=(RLES*DDV)/DDB;
|
|
DY=(RLES*DDE)/DDB;
|
|
goto label_510;
|
|
label_500 : DDB=(DEV2-DEV1)*(AZIM2-AZIM1);
|
|
DDV=(cos(DEV1)-cos(DEV2))*(cos(AZIM1)-cos(AZIM2));
|
|
DDE=(cos(DEV1)-cos(DEV2))*(sin(AZIM2)-sin(AZIM1));
|
|
DX=(RLES*DDV)/DDB;
|
|
DY=(RLES*DDE)/DDB;
|
|
label_510 : TVD=(RLES*(sin(DEV2)-sin(DEV1)))/(DEV2-DEV1);
|
|
goto label_666;
|
|
label_100 : DX=RLES*sin(DEV2)*sin(AZIM2);
|
|
DY=RLES*sin(DEV2)*cos(AZIM2);
|
|
TVD=RLES*cos(DEV2);
|
|
goto label_666;
|
|
label_200 : DX=(RLES*(cos(DEV1)-cos(DEV2))*sin(AZIM2))/(DEV2-DEV1);
|
|
DY=(RLES*(cos(DEV1)-cos(DEV2))*cos(AZIM2))/(DEV2-DEV1);
|
|
TVD=(RLES*(sin(DEV2)-sin(DEV1)))/(DEV2-DEV1);
|
|
goto label_666;
|
|
label_300 : if ((AZIM1<=1.5*PI) && (AZIM1>=0.5*PI)) goto label_600;
|
|
if((AZIM2<=1.5*PI) && (AZIM2>=0.5*PI)) goto label_600;
|
|
if((AZIM1>1.5*PI) && (AZIM2>1.5*PI)) goto label_600;
|
|
if((AZIM1<0.5*PI) && (AZIM2<0.5*PI)) goto label_600;
|
|
if((AZIM1<0.5*PI) && (AZIM2>1.5*PI)) goto label_550;
|
|
DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(2*PI+AZIM2-AZIM1);
|
|
DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(2*PI+AZIM2-AZIM1);
|
|
goto label_650;
|
|
label_550 : DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(AZIM2-AZIM1-2*PI);
|
|
DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(AZIM2-AZIM1-2*PI);
|
|
goto label_650;
|
|
label_600 : DX=(RLES*(cos(AZIM1)-cos(AZIM2))*sin(DEV2))/(AZIM2-AZIM1);
|
|
DY=(RLES*(sin(AZIM2)-sin(AZIM1))*sin(DEV2))/(AZIM2-AZIM1);
|
|
label_650 : TVD=RLES*cos(DEV2);
|
|
label_666 :
|
|
XE=XE+DX;
|
|
YN=YN+DY;
|
|
DZDA=DZDA+TVD; //DZDA=?
|
|
ZD=TVDS+DZDA; //TVDS=?
|
|
}
|
|
if(FLSA==1.0)
|
|
{
|
|
DX=RLES*sin(DEV2)*sin(AZIM2);
|
|
DY=RLES*sin(DEV2)*cos(AZIM2);
|
|
TVD=RLES*cos(DEV2);
|
|
XE=XE+DX;
|
|
YN=YN+DY;
|
|
DZDA=DZDA+TVD;
|
|
ZD=TVDS+DZDA;
|
|
}
|
|
DEV1=DEV2;
|
|
AZIM1=AZIM2;
|
|
|
|
label_11 : ;
|
|
HOFF=sqrt(XE*XE+YN*YN);
|
|
X=abs(XE);
|
|
Y=abs(YN) ;
|
|
if((XE>0.0) && (YN>0.0)) HOAZ=atan2(X,Y) ;
|
|
if((XE>0.0) && (YN<0.0)) HOAZ=PI-atan2(X,Y);
|
|
if((XE<0.0) && (YN<0.0)) HOAZ=PI+atan2(X,Y);
|
|
if((XE<0.0) && (YN>0.0)) HOAZ=2*PI-atan2(X,Y) ;
|
|
if((XE==0.0) && (YN>0.0)) HOAZ=0.;
|
|
if((YN==0.0) && (XE>0.0)) HOAZ=0.5*PI;
|
|
if((XE==0.0) && (YN<0.0)) HOAZ=PI ;
|
|
if((YN==0.0) && (XE<0.0)) HOAZ=1.5*PI ;
|
|
HOAZ=(HOAZ*360.)/(2*PI) ;
|
|
float GTD=0;
|
|
GTD=sqrt(pow((DEVI-OLDDEVI)/RLEA,2)+
|
|
pow((AZIM-OLDAZIM>180?AZIM-OLDAZIM-360:
|
|
AZIM-OLDAZIM<-180?AZIM-OLDAZIM+360:AZIM-OLDAZIM)/RLEA,2)*
|
|
pow(sin((DEVI+OLDDEVI)/2*PI/180),2))*RLEA;//30
|
|
|
|
OLDDEVI=DEVI;
|
|
OLDAZIM=AZIM;
|
|
*/
|
|
XE0=YN;
|
|
YN0=XE;
|
|
ZD0=ZD;
|
|
HOAZ0=HOAZ;
|
|
HOFF0=HOFF;
|
|
GTD0=GTD;
|
|
AZIM0=AZIM;
|
|
return 1;
|
|
}
|