#include <math.h>
#define CONSTANT_MAG 4*3.1415926*100
#define PI 3.1415926
double Grav_value[4];
//double *GRAV_FORWORD(double *Grav_value, double P[][3],double x,double y,double z)
double *GRAV_FORWORD(double *Grav_value, double P[][3],double x,double y,double z)
{
//NO 's max number is 100
void trs_coordinate3(double a[][3],double b[],double c[]);
/* Move to global?*/
//double Grav_value[4];
int i;
double P_obj[3];
double New_Point[4][3];////新系下三角形角点New_Point[?][3]
double cos_alpha[2];//
double sin_alpha[2];//
double R[4],AZ[4],AX[4],AY[4];//3=?
//double density;
double New_X_x,New_X_y,New_X_z;//坐标系分量
double New_Y_x,New_Y_y,New_Y_z;
double New_Z_x,New_Z_y,New_Z_z;
double a1,b1,c1;//变换系数
double a2,b2,c2;
double a3,b3,c3;
double New_P_obj[3];//新观测点
double trs_Old_A[3][3];//变换到测量坐标系的系数
double trs_New_A[3][3];//变换到计算坐标系的系数
//double v1_length2;//dx1,dx2,dy1,dy2;///临时变量
double h1,h2,g1,g2;
//double Q1=0.0;//
//double Q2=0.0;//
double I=0.0;//I=Q1+Q2
// double Mag_value_New[3];//计算坐标系的磁场
//A_angle=A_angle*(PI/180);
//I_angle=I_angle*(PI/180);
P_obj[0]=x;
P_obj[1]=y;
P_obj[2]=z;
//坐标系变换de准备
//X
New_X_x=P[2][0]-P[0][0];
New_X_y=P[2][1]-P[0][1];
New_X_z=P[2][2]-P[0][2];
// x y z
a1=New_X_x/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
b1=New_X_y/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
c1=New_X_z/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
//Z
New_Z_x=New_X_y*(P[1][2]-P[0][2])-New_X_z*(P[1][1]-P[0][1]);
New_Z_y=New_X_z*(P[1][0]-P[0][0])-New_X_x*(P[1][2]-P[0][2]);
New_Z_z=New_X_x*(P[1][1]-P[0][1])-New_X_y*(P[1][0]-P[0][0]);
a3=New_Z_x/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
b3=New_Z_y/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
c3=New_Z_z/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
//Y
New_Y_x=New_Z_y*New_X_z-New_Z_z*New_X_y;
New_Y_y=New_Z_z*New_X_x-New_Z_x*New_X_z;
New_Y_z=New_Z_x*New_X_y-New_Z_y*New_X_x;
a2=New_Y_x/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
b2=New_Y_y/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
c2=New_Y_z/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
trs_New_A[0][0]=a1;////变换到计算坐标系的系数
trs_New_A[0][1]=b1;
trs_New_A[0][2]=c1;
trs_New_A[1][0]=a2;
trs_New_A[1][1]=b2;
trs_New_A[1][2]=c2;
trs_New_A[2][0]=a3;
trs_New_A[2][1]=b3;
trs_New_A[2][2]=c3;
trs_Old_A[0][0]=a1;//////变换到测量坐标系的系数
trs_Old_A[0][1]=a2;
trs_Old_A[0][2]=a3;
trs_Old_A[1][0]=b1;
trs_Old_A[1][1]=b2;
trs_Old_A[1][2]=b3;
trs_Old_A[2][0]=c1;
trs_Old_A[2][1]=c2;
trs_Old_A[2][2]=c3;
//坐标系变换准备结束
//将数据变换到计算的坐标系统中
//trs_coordinate(double a[],double b[],double c[])
for(i=0;i<3;i++)
trs_coordinate3(trs_New_A,&P[i][0],&New_Point[i][0]);//第一维为点号
New_Point[3][0]=New_Point[0][0];
New_Point[3][1]=New_Point[0][1];
New_Point[3][2]=New_Point[0][2];
trs_coordinate3(trs_New_A,P_obj,New_P_obj);
for(i=0;i<=2;i++)
{
R[i]=pow(New_Point[i][0]-New_P_obj[0],2)
+pow(New_Point[i][1]-New_P_obj[1],2)
+pow(New_Point[i][2]-New_P_obj[2],2);
R[i]=sqrt(R[i]);//baoliu
AX[i]=-(New_Point[i][0]-New_P_obj[0]);
AY[i]=-(New_Point[i][1]-New_P_obj[1]);
AZ[i]=-(New_Point[i][2]-New_P_obj[2]);
}
h1=(AX[0]*AY[1]-AX[1]*AY[0])/(AY[1]-AY[0]);
h2=(AX[2]*AY[1]-AX[1]*AY[0])/(AY[1]-AY[0]);
//cos_alpha[0]
g1=(AX[1]-AX[0])/(AY[1]-AY[0]);
g2=(AX[1]-AX[2])/(AY[1]-AY[0]);
cos_alpha[0]=cos(atan(g1));
cos_alpha[1]=cos(atan(g2));
sin_alpha[0]=sin(atan(g1));
sin_alpha[1]=sin(atan(g2));
I=h1*cos_alpha[0]*log((AX[0]*sin_alpha[0]+AY[0]*cos_alpha[0]+R[0])/(AX[1]*sin_alpha[0]+AY[1]*cos_alpha[0]+R[1]))
-h2*cos_alpha[1]*log((AX[2]*sin_alpha[1]+AY[0]*cos_alpha[1]+R[2])/(AX[1]*sin_alpha[1]+AY[1]*cos_alpha[1]+R[1]))
+AY[0]*log((AX[0]+R[0])/(AX[2]+R[2]))
+2*AZ[0]*atan((AY[0]*cos_alpha[0]+(1+sin_alpha[0])*(AX[0]+R[0]))/(AZ[0]*cos_alpha[0]))
-2*AZ[0]*atan((AY[1]*cos_alpha[0]+(1+sin_alpha[0])*(AX[1]+R[1]))/(AZ[0]*cos_alpha[0]))
-2*AZ[0]*atan((AY[0]*cos_alpha[1]+(1+sin_alpha[1])*(AX[2]+R[2]))/(AZ[0]*cos_alpha[1]))
+2*AZ[0]*atan((AY[1]*cos_alpha[1]+(1+sin_alpha[1])*(AX[1]+R[1]))/(AZ[0]*cos_alpha[1]));
///////////////////////////////////////////////
Grav_value[0]=I*a3;
Grav_value[1]=I*b3;
Grav_value[2]=I*c3;
//return I*c3;
return &Grav_value[0];
//return Grav_value
}
/////////////////////////////////////////////////////////////////////////
//double Mag_value[4];
double *MAG_FORWORD(double *Mag_value, double P[][3],int NO,double A_angle,double I_angle,
double J_MAG,double x,double y,double z)
{ //NO 's max number is 100
double qqq=0.0;
void trs_coordinate3(double a[][3],double b[],double c[]);
/* Move to glabal */
//double Mag_value[4];
int i;
double P_obj[3];
double New_Point[101][3];////新系下三角形角点New_Point[?][3]
double cos_alpha[100];//
double sin_alpha[100];//
double R[101],r[101],AZ[101],AX[101];//3=?
double density;
double New_X_x,New_X_y,New_X_z;//坐标系分量
double New_Y_x,New_Y_y,New_Y_z;
double New_Z_x,New_Z_y,New_Z_z;
double a1,b1,c1;//变换系数
double a2,b2,c2;
double a3,b3,c3;
double New_P_obj[3];//新观测点
double trs_Old_A[3][3];//变换到测量坐标系的系数
double trs_New_A[3][3];//变换到计算坐标系的系数
double v1_length2,v2_length2,dy1,dy2,dx1,dx2;///临时变量
double Mag_value_New[3];//计算坐标系的磁场
A_angle=A_angle*(PI/180);
I_angle=I_angle*(PI/180);
P_obj[0]=x;
P_obj[1]=y;
P_obj[2]=z;
//坐标系变换de准备
//X
New_X_x=P[NO-1][0]-P[0][0];
New_X_y=P[NO-1][1]-P[0][1];
New_X_z=P[NO-1][2]-P[0][2];
// x y z
a1=New_X_x/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
b1=New_X_y/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
c1=New_X_z/sqrt(New_X_x*New_X_x+New_X_y*New_X_y+New_X_z*New_X_z);
//Z
New_Z_x=New_X_y*(P[1][2]-P[0][2])-New_X_z*(P[1][1]-P[0][1]);
New_Z_y=New_X_z*(P[1][0]-P[0][0])-New_X_x*(P[1][2]-P[0][2]);
New_Z_z=New_X_x*(P[1][1]-P[0][1])-New_X_y*(P[1][0]-P[0][0]);
a3=New_Z_x/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
b3=New_Z_y/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
c3=New_Z_z/sqrt(New_Z_x*New_Z_x+New_Z_y*New_Z_y+New_Z_z*New_Z_z);
//Y
New_Y_x=New_Z_y*New_X_z-New_Z_z*New_X_y;
New_Y_y=New_Z_z*New_X_x-New_Z_x*New_X_z;
New_Y_z=New_Z_x*New_X_y-New_Z_y*New_X_x;
a2=New_Y_x/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
b2=New_Y_y/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
c2=New_Y_z/sqrt(New_Y_x*New_Y_x+New_Y_y*New_Y_y+New_Y_z*New_Y_z);
trs_New_A[0][0]=a1;////变换到计算坐标系的系数
trs_New_A[0][1]=b1;
trs_New_A[0][2]=c1;
trs_New_A[1][0]=a2;
trs_New_A[1][1]=b2;
trs_New_A[1][2]=c2;
trs_New_A[2][0]=a3;
trs_New_A[2][1]=b3;
trs_New_A[2][2]=c3;
trs_Old_A[0][0]=a1;//////变换到测量坐标系的系数
trs_Old_A[0][1]=a2;
trs_Old_A[0][2]=a3;
trs_Old_A[1][0]=b1;
trs_Old_A[1][1]=b2;
trs_Old_A[1][2]=b3;
trs_Old_A[2][0]=c1;
trs_Old_A[2][1]=c2;
trs_Old_A[2][2]=c3;
//坐标系变换准备结束
//将数据变换到计算的坐标系统中
//trs_coordinate(double a[],double b[],double c[])
for(i=0;i<NO;i++)
trs_coordinate3(trs_New_A,&P[i][0],&New_Point[i][0]);//第一维为点号
New_Point[NO][0]=New_Point[0][0];
New_Point[NO][1]=New_Point[0][1];
New_Point[NO][2]=New_Point[0][2];
trs_coordinate3(trs_New_A,P_obj,New_P_obj);
//密度
density=a3*cos(I_angle)*cos(A_angle)+b3*cos(I_angle)*sin(A_angle)+c3*sin(I_angle);
density=-density*J_MAG;
// printf("%lf ",density);
//密度求解结束
//角度
for(i=0;i<NO;i++)
{
cos_alpha[i]=(New_Point[i+1][0]-New_Point[i][0]);
sin_alpha[i]=(New_Point[i+1][1]-New_Point[i][1]);
v1_length2=pow((New_Point[i+1][0]-New_Point[i][0]),2)