#include "Arith_VehicleCoordModule.h"
#include <cmath>



// 标定参数：安装误差
double g_YawBias = 0;
double g_PitchBias = 0;
double g_RollBias = 0;
double g_XBias = 0;
double g_YBias = 0;
double g_ZBias = 0;


void setBiasPara(double YawBias, double PitchBias, double RollBias, double deltaX, double deltaY, double deltaZ)
{
	g_YawBias = YawBias;
	g_PitchBias = PitchBias;
	g_RollBias = RollBias;
	g_XBias = deltaX;
	g_YBias = deltaY;
	g_ZBias = deltaZ;
}

void getBiasPara(double * YawBias, double * PitchBias, double * RollBias, double * deltaX, double * deltaY, double * deltaZ)
{
	*YawBias = g_YawBias;
	*PitchBias =g_PitchBias;
	*RollBias = g_RollBias;
	*deltaX = g_XBias;
	*deltaY=  g_YBias;
	*deltaZ = g_ZBias;
}


// 像方到物方转换
Pole getPointMirrorServoPoleFromImageXY(int Col,int Row,int nWidth,int nHeight,
										float fServoA,float fServoB,float fLen,float pxSizeOfum)
{
	// by wcw04046 @ 2021/12/04
	// 注意:在通常成像系统中，伺服读数基本等价于伺服系下角度（需要考虑正割补偿）
	// 在指向镜成像系统中，伺服读数与伺服系下角度完全不等价 fServoAz fServoPt 与目标在伺服系位置非线性 
	// 伺服坐标系角度
	Pole ServoPole;

	// 像素焦距
	float f_0 = fLen/pxSizeOfum * 1000;

	// 等效像方入射矢量
	PointXYZ CamVec;
	CamVec.X = f_0;
	CamVec.Y = -nHeight/2 + Row;
	CamVec.Z = (-nWidth/2 + Col);

	// 反射镜能将正前方(x轴)的目标反射到成像系统光轴(y轴)上时，角编码器为零位

	// 平面镜1零位的法向量
	PointXYZ N_01;
	N_01.X = 1/sqrt(2.0);
	N_01.Y = 1/sqrt(2.0);
	N_01.Z = 0;

	double Sx[9] = {1,0,0,
	0,cos(RADIAN(fServoA)),-sin(RADIAN(fServoA)),
	0,sin(RADIAN(fServoA)),cos(RADIAN(fServoA))};

	double Sy[9] = {cos(RADIAN(fServoB)),0,-sin(RADIAN(fServoB)),
	0,1,0,
	sin(RADIAN(fServoB)),0,cos(RADIAN(fServoB))};

	PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
	// 镜面反射矩阵
	//	M = eye(3)-(2*N*N');
	double M1[9] = {0};
	RMirror(N_1,M1);


	// 平面镜2零位的法向量
	PointXYZ N_02;
	N_02.X = -1/sqrt(2.0);
	N_02.Y = -1/sqrt(2.0);
	N_02.Z = 0;
	PointXYZ N_2 = RtPoint(Sx,N_02);
	double M2[9] = {0};
	RMirror(N_2,M2);

	// 平面镜3零位的法向量
	PointXYZ N_03;
	N_03.X = 1/sqrt(2.0);
	N_03.Y = -1/sqrt(2.0);
	N_03.Z = 0;
	PointXYZ N_3 = RtPoint(Sx,N_03);
	double M3[9] = {0};
	RMirror(N_3,M3);

	// 平面镜4零位的法向量
	PointXYZ N_04;
	N_04.X = -1/sqrt(2.0);
	N_04.Y = 1/sqrt(2.0);
	N_04.Z = 0;
	PointXYZ N_4 = RtPoint(Sx,N_04);
	double M4[9] = {0};
	RMirror(N_4,M4);

	// 等效物方出射矢量
	PointXYZ worldPosVec = RtPoint(M1,RtPoint(M2,RtPoint(M3,RtPoint(M4,CamVec))));
	PointXYZ worldPosVec_1 = NormPoint(worldPosVec);

    ServoPole.alpha = ANGLE(asin(worldPosVec_1.Y));
	ServoPole.beta = ANGLE(atan2(worldPosVec_1.Z,worldPosVec_1.X));
	ServoPole.distance = 1000;

	return ServoPole;

}

// 物方到像方转换
void getPointMirrorImageXYFromServoPole(int* Col,int* Row,int nWidth,int nHeight,float WAlaph,float WBeta,
										float fServoA,float fServoB,float fLen,float pxSizeOfum)
{
	//物方
	int dep = 1000;
	PointXYZ WorldVec;
	WorldVec.Y = -dep * sin(RADIAN(WAlaph));
	WorldVec.X = -dep * cos(RADIAN(WAlaph)) * cos(RADIAN(WBeta));
	WorldVec.Z = -dep * cos(RADIAN(WAlaph)) * sin(RADIAN(WBeta));

	// 内参矩阵
	float f_0 = fLen/pxSizeOfum * 1000;

	double K[9] = 
	{f_0,0,nWidth/2,
	0,f_0,nHeight/2,
	0,0,1};

	// 平面镜1零位的法向量
	PointXYZ N_01;
	N_01.X = 1/sqrt(2.0);
	N_01.Y = 1/sqrt(2.0);
	N_01.Z = 0;

	double Sx[9] = {1,0,0,
		0,cos(RADIAN(fServoB)),-sin(RADIAN(fServoB)),
		0,sin(RADIAN(fServoB)),cos(RADIAN(fServoB))};

	double Sy[9] = {cos(RADIAN(fServoA)),0,-sin(RADIAN(fServoA)),
		0,1,0,
		sin(RADIAN(fServoA)),0,cos(RADIAN(fServoA))};

	PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
	// 镜面反射矩阵
	//	M = eye(3)-(2*N*N');
	double M1[9] = {0};
	RMirror(N_1,M1);


	// 平面镜2零位的法向量
	PointXYZ N_02;
	N_02.X = -1/sqrt(2.0);
	N_02.Y = -1/sqrt(2.0);
	N_02.Z = 0;
	PointXYZ N_2 = RtPoint(Sx,N_02);
	double M2[9] = {0};
	RMirror(N_2,M2);

	// 平面镜3零位的法向量
	PointXYZ N_03;
	N_03.X = 1/sqrt(2.0);
	N_03.Y = -1/sqrt(2.0);
	N_03.Z = 0;
	PointXYZ N_3 = RtPoint(Sx,N_03);
	double M3[9] = {0};
	RMirror(N_3,M3);

	// 平面镜4零位的法向量
	PointXYZ N_04;
	N_04.X = -1/sqrt(2.0);
	N_04.Y = 1/sqrt(2.0);
	N_04.Z = 0;
	PointXYZ N_4 = RtPoint(Sx,N_04);
	double M4[9] = {0};
	RMirror(N_4,M4);

	PointXYZ camPosVec = RtPoint(M4,RtPoint(M3,RtPoint(M2,RtPoint(M1,WorldVec))));

	double m_adjust[9] = {0,0,1,0,1,0,1,0,0};

	PointXYZ pixPos = RtPoint(K,RtPoint(m_adjust,camPosVec));

	*Col = pixPos.X / float(pixPos.Z);
	*Row = pixPos.Y / float(pixPos.Z);



}



Pole getCamZaxisNUERota(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta)
{
	// 定义相机坐标系Z轴向量，长度不影响
	PointXYZ pTest = {0};pTest.Z = 10000;

	//计算旋转矩阵
	double Mat[9] = {0};
	getCarNUEXYZFromEOCamXYZ_Mat(mYaw,mPitch,mRoll,servoAlpha,servoBeta,Mat);

	// 计算大地系向量
	PointXYZ targetCarNUEXYZ = RtPoint(Mat,pTest);

	// 计算大地系转动量
	Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ);


	return targetCarNUEPole;
}

// 信息上报函数
Pole getCarNUEPoleFromEOCamPole(Pole targetCamPole, double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta)
{
	//// 方位角限制在0-360
	if (targetCamPole.beta > 360)
	{
		targetCamPole.beta -= 360;
	}

	if(targetCamPole.distance <= 100)
	{
		targetCamPole.distance = 10000;
	}
	//限制俯仰角度
// 	targetCamPole.alpha = MIN(targetCamPole.alpha, 80);
// 	targetCamPole.alpha = MAX(targetCamPole.alpha, -5);

// 1:一般旋转矩阵方法
	// 光电镜面极坐标 转 光电镜面直角坐标
	PointXYZ targetCamXYZ = getXYZFromPole(targetCamPole);

	// 光电镜面直角坐标 转 光电伺服直角坐标
	PointXYZ targetServoXYZ = getEOServoXYZFromEOCamXYZ(targetCamXYZ, servoAlpha, servoBeta);

// 2:正割补偿方法
	// 光电镜面极坐标 转 光电伺服极坐标
// 	Pole targetEOServoPOLE;
// 	targetEOServoPOLE.alpha = alaph + targetCamPole.alpha;
// 	targetEOServoPOLE.beta = beta + targetCamPole.beta/cos(RADIAN(alaph));
// 	targetEOServoPOLE.distance = targetCamPole.distance;
// 	// 光电伺服极坐标 转 光电伺服直角坐标
// 	targetServoXYZ = getXYZFromPole(targetEOServoPOLE);


//指向镜系统的成像模型
#ifdef POINT_MIRROR_SYS
	int nWidth = 640;
	int nHeight = 512;
	int Row = SERVO_CalcObjCoordinateX((FLOAT32)targetCamPole.beta, nWidth,
		0, g_SERVO_stInput.fResolAz);
	int Col = SERVO_CalcObjCoordinateY((FLOAT32)targetCamPole.alpha, nHeight,
		0, g_SERVO_stInput.fResolPt);


	// test
	//servoBeta = 20;
	//servoAlpha = 5;
	//Col = 320;
	//Row = 256;
	//	Pole targetServoPole1 = getPointMirrorServoPoleFromImageXY( Col, Row, nWidth, nHeight,
	//		servoBeta, servoAlpha,80,15);
	//	targetServoXYZ = getXYZFromPole(targetServoPole1);
	//
	//	Col = 0;
	//	Row = 0;
	//
	//	getPointMirrorImageXYFromServoPole(&Col,&Row,nWidth,nHeight,targetServoPole.alpha,targetServoPole.beta,
	//		servoBeta, servoAlpha,80,15);


	Pole targetServoPole = getPointMirrorServoPoleFromImageXY( Row, Col, nWidth, nHeight,
			servoBeta, servoAlpha,80,15);
		targetServoXYZ = getXYZFromPole(targetServoPole);

#endif




	// 光电伺服直角坐标 转 光电基准直角坐标
	PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, g_YawBias, g_PitchBias, g_RollBias);

	// 光电基准直角坐标 转 平台直角坐标
	PointXYZ targetCarXYZ = getCarXYZFromEOBaseXYZ(targetEOBaseXYZ, g_XBias, g_YBias, g_ZBias);

	// 平台直角坐标 转 平台地理直角坐标
	PointXYZ targetCarNUEXYZ = getCarNUEXYZFromCarXYZ(targetCarXYZ, mYaw, mPitch, mRoll);

	// 平台地理直角坐标 转 平台地理极坐标
	Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ);

#if 0
	//double Mat[9] = {0};
	//mYaw = 0;mPitch = 80;mRoll =5.1;
	//servoAlpha = 0;servoBeta=0;
	PointXYZ pTest = {0};pTest.Z = 10000;
	getCarNUEXYZFromEOCamXYZ_Mat(mYaw,mPitch,mRoll,servoAlpha,servoBeta,Mat);
	PointXYZ targetCarNUEXYZ2 = RtPoint(Mat,pTest);
	Pole targetCarNUEPole2 = getPoleFromXYZ(targetCarNUEXYZ2);
#endif


	// 上报目标平台地理极坐标
	return targetCarNUEPole;
}

PointXYZ getEOServoXYZFromEOCamXYZ(PointXYZ targetCamXYZ,double alaph,double beta)
{
	// 先绕Z轴俯仰角旋转-alaph，后绕Y轴方位角旋转-beta
	double oppRz[9];
	double oppRy[9];
	double oppRyz[9];

	// 计算旋转矩阵
	Rz(-alaph, oppRz);
	Ry(-beta, oppRy);
	MultiMat33(oppRy,oppRz, oppRyz);

	// 坐标转换
	return RtPoint(oppRyz, targetCamXYZ);
}

PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, double eYaw, double ePitch, double eRoll)
{
	// 先绕x轴横滚旋转 -eRoll，再绕z轴俯仰旋转 -ePitch，再绕y轴方位旋转-eYaw

	double oppRx[9];
	double oppRy[9];
	double oppRz[9];
	double oppRyzx[9];

	// 计算旋转矩阵
	Rx(-eRoll, oppRx);
	Rz(-ePitch, oppRz);
	Ry(-eYaw, oppRy);
	
	MultiMat333(oppRy,oppRz, oppRx, oppRyzx);

	// 坐标转换
	return RtPoint(oppRyzx, targetServoXYZ);

}

PointXYZ getCarXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double deltaX, double deltaY, double deltaZ)
{
	//平台坐标系下目标坐标
	PointXYZ targetCarXYZ;


	targetCarXYZ.X = targetEOBaseXYZ.X + deltaX;
	targetCarXYZ.Y = targetEOBaseXYZ.Y + deltaY;
	targetCarXYZ.Z = targetEOBaseXYZ.Z + deltaZ;

	return targetCarXYZ;
}

PointXYZ getCarNUEXYZFromCarXYZ(PointXYZ targetCarXYZ, double mYaw, double mPitch, double mRoll)
{
	// 先绕x轴横滚旋转 -mRoll，再绕z轴俯仰旋转 -mPitch，再绕y轴方位旋转-mYaw

	double oppRx[9];
	double oppRy[9];
	double oppRz[9];
	double oppRyzx[9];

	// 计算旋转矩阵
	Rx(-mRoll, oppRx);
	Rz(-mPitch, oppRz);
	Ry(-mYaw, oppRy);

	MultiMat333(oppRy,oppRz, oppRx, oppRyzx);

	// 坐标转换
	return RtPoint(oppRyzx, targetCarXYZ);

}



// 接受导引函数
Pole getEOServoPoleFromCarNUEPole(Pole targetCarNUEPole, Pole* targetEOCamPole,double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta)
{
	// 地理极坐标 转 地理直角坐标
	PointXYZ targetCarNUEXYZ = getXYZFromPole(targetCarNUEPole);

	// 地理直角坐标 转 平台直角坐标
	PointXYZ targetCarXYZ = getCarXYZFromCarNUEXYZ(targetCarNUEXYZ, mYaw, mPitch, mRoll);

	// 平台直角 转 光电基准系直角
	PointXYZ targetEOBaseXYZ = getEOBaseXYZFromCarXYZ(targetCarXYZ, g_XBias, g_YBias, g_ZBias);

	// 光电基准系直角 转 光电伺服直角
	PointXYZ targetEOServoXYZ = getEOServoXYZFromEOBaseXYZ(targetEOBaseXYZ, g_YawBias, g_PitchBias, g_RollBias);

	// 光电伺服直角 转 光电伺服极坐标
	Pole targetEOServoPole = getPoleFromXYZ(targetEOServoXYZ);

 	// 光电伺服直角 转 光电镜面直角坐标
 	PointXYZ targetEOCamXYZ = getEOCamXYZFromEOServoXYZ(targetEOServoXYZ, servoAlpha, servoBeta);
 
 	// 光电镜面直角系 转 光电镜面极坐标
 	*targetEOCamPole = getPoleFromXYZ(targetEOCamXYZ);


	//指向镜系统的成像模型
#ifdef POINT_MIRROR_SYS
	int nWidth = 640;
	int nHeight = 512;

	int Col = 0;
	int Row = 0;

	getPointMirrorImageXYFromServoPole(&Col,&Row,nWidth,nHeight,targetEOServoPole.alpha,targetEOServoPole.beta,servoAlpha,servoBeta,80,15);
	// 计算目标的方位角、俯仰角
	targetEOCamPole->beta = SERVO_CalcObjAzimuth(Col,
		nWidth,	0, g_SERVO_stInput.fResolAz);
	targetEOCamPole->alpha = SERVO_CalcObjPitching(Row,
		nHeight,0, g_SERVO_stInput.fResolPt);

#endif

	return targetEOServoPole;
}

PointXYZ getCarXYZFromCarNUEXYZ(PointXYZ targetCarNUEXYZ, double mYaw, double mPitch, double mRoll)
{
	// 先绕y轴方位旋转mYaw -> 再绕z轴俯仰旋转 mPitch -> 绕x轴横滚旋转 mRoll
	double oppRx[9];
	double oppRy[9];
	double oppRz[9];
	double oppRxzy[9];

	// 计算旋转矩阵
	Ry(mYaw, oppRy);
	Rz(mPitch, oppRz);
	Rx(mRoll, oppRx);

	MultiMat333(oppRx, oppRz, oppRy, oppRxzy);

	// 坐标转换
	return RtPoint(oppRxzy, targetCarNUEXYZ);

}

PointXYZ getEOBaseXYZFromCarXYZ(PointXYZ targetCarXYZ, double deltaX, double deltaY, double deltaZ)
{
	//平台坐标系下目标坐标
	PointXYZ targetEOBaseXYZ;

	targetEOBaseXYZ.X = targetCarXYZ.X - deltaX;
	targetEOBaseXYZ.Y = targetCarXYZ.Y - deltaY;
	targetEOBaseXYZ.Z = targetCarXYZ.Z - deltaZ;

	return targetEOBaseXYZ;
}

PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double eYaw, double ePitch, double eRoll)
{
	// 先绕y轴方位旋转eYaw,再绕z轴俯仰旋转 ePitch,再绕x轴横滚旋转 eRoll
	double oppRx[9];
	double oppRy[9];
	double oppRz[9];
	double oppRxzy[9];

	// 计算旋转矩阵
	Rx(eRoll, oppRx);
	Rz(ePitch, oppRz);
	Ry(eYaw, oppRy);

	MultiMat333(oppRx, oppRz, oppRy, oppRxzy);

	// 坐标转换
	return RtPoint(oppRxzy, targetEOBaseXYZ);

}

PointXYZ getEOCamXYZFromEOServoXYZ(PointXYZ targetEOServoXYZ, double alaph, double beta)
{
	// 先绕Y轴方位角旋转beta,后绕Z轴俯仰角旋转alaph
	double oppRz[9];
	double oppRy[9];
	double oppRzy[9];

	// 计算旋转矩阵
	Rz(alaph, oppRz);
	Ry(beta, oppRy);
	MultiMat33(oppRz, oppRy, oppRzy);

	// 坐标转换
	return RtPoint(oppRzy, targetEOServoXYZ);

}

Pole getPoleFromXYZ(PointXYZ targetXYZ)
{
	double x = targetXYZ.X;
	double y = targetXYZ.Y;
	double z = targetXYZ.Z;

	Pole result;
	result.distance = sqrt(x*x + y*y + z*z);
	result.alpha = ANGLE(asin(y / result.distance));

	//if (x < 0)
	//{
	//	result.alpha = -180-result.alpha;
	//}


	result.beta = (ANGLE(atan2(z , x)));
// 	if (x < 0 && z >= 0) // 90 - 180
// 	{
// 		result.beta = 180 - result.beta;
// 	}
// 	else if (x < 0 && z < 0) // 180 - 270
// 	{
// 		result.beta = result.beta + 180;
// 	}
// 	else if (x >= 0 && z < 0) // 270 - 360
// 	{
// 		result.beta = 360 - result.beta;
// 	}

	return result;

}

PointXYZ getXYZFromPole(Pole targetPole)
{
	// 距离未知时，异原点坐标系之间不能转换
	// 但由于同车坐标系的原点差一般远小于目标距离，故距离较大时误差较小，误差量级可以用线偏差/距离获得的弧度进行估计
	// 如线偏差1米，目标3000米，则转换误差为(1/1000-1/3000)约为0.04°
	if (targetPole.distance < 100)
	{
		targetPole.distance = 1000;
	}

	PointXYZ result;
	double aa, bb, cc;
	//double ac = sqrt(targetPole.distance*targetPole.distance-bb*bb);    
	bb = sin(RADIAN(targetPole.alpha)) * targetPole.distance;
	double ac = sqrt(targetPole.distance*targetPole.distance - bb*bb);
	aa = cos(RADIAN(targetPole.beta)) * ac;
	cc = sin(RADIAN(targetPole.beta)) * ac;

	result.X = aa;
	result.Y = bb;
	result.Z = cc;

	return result;

}

//大地坐标转地球空间直角坐标
PointXYZ getXYZFromBLH(PointBLH BLH)
{
	double B = BLH.B;
	double L = BLH.L;
	double H = BLH.H;

	double W = sqrt(1 - ee * pow(sin(RADIAN(B)), 2));
	double N = a / W;          // 椭球的卯酉圈曲率

	// 将地理坐标变换到空间直角坐标
	double X = (N + H) * cos(RADIAN(B)) * cos(RADIAN(L));
	double Y = (N + H) * cos(RADIAN(B)) * sin(RADIAN(L));
	double Z = (N * (1 - ee) + H) * sin(RADIAN(B));

	PointXYZ pointXYZ;
	pointXYZ.X = X;
	pointXYZ.Y = Y;
	pointXYZ.Z = Z;

	return pointXYZ;
}

//地球空间直角坐标-->大地坐标
PointBLH getBLHFromXYZ(PointXYZ pointXYZ)
{
	double X = pointXYZ.X;
	double Y = pointXYZ.Y;
	double Z = pointXYZ.Z;

	double U = atan(Z * a / (sqrt(X * X + Y * Y) * b));
	double B0 = atan((Z + b * epep * pow(sin(U), 3)) / (sqrt(X*X + Y*Y) - a*ee*pow(cos(U), 3)));
	double N = a / sqrt(1 - ee * pow(sin(B0), 2));
	//double L = atan(Y /X);
	double L = atan2(Y, X); // 修改为全球范围计算          by wcw04046 @ 2021/02/18

	double H = sqrt(X*X + Y*Y + pow(Z + N*ee*sin(B0), 2)) - N;
	double B = atan(Z / sqrt(X*X + Y*Y) / (1 - ee*N / (N + H)));

	PointBLH pointBLH;
	pointBLH.B = ANGLE(B);
	pointBLH.H = H;
// 	// 此处仅考虑东经
// 	pointBLH.L = ANGLE(L) > 0 ? ANGLE(L) : ANGLE(L) + 180;

	// 修改为全球范围计算          by wcw04046 @ 2021/02/18
	pointBLH.L = ANGLE(L);

	return pointBLH;
}

//*********************************************************************************************
//  由NUE直角坐标变换到CGCS直角坐标
//  targetPointXYZ：NUE下的目标坐标
//  selfPointXYZ：测站在CGCS下的直角坐标
//  返回CGCS下的目标坐标
//*********************************************************************************************
PointXYZ getCGCSXYZFromNUEXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
{
	// 将测站所在位置的CGCS直角坐标变换到地理坐标
	PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);

	/* 由NUE直角坐标变换到CGCS直角坐标*/
	double A[3];
	double G[3];
	double OppRx[9];
	double OppRy[9];
	double OppRz[9];
	double OppRzx[9];
	double OppRzxy[9];

	// 1、计算旋转矩阵
	Rx(-selfPointBLH.B, OppRx);
	Ry(-90, OppRy);
	Rz(90 - selfPointBLH.L, OppRz);

	MultiMat(OppRz, OppRx, OppRzx, 3, 3, 3);
	MultiMat(OppRzx, OppRy, OppRzxy, 3, 3, 3);

	// 2、将NUE直角坐标变换到CGCS直角坐标
	A[0] = targetPointXYZ.X;
	A[1] = targetPointXYZ.Y;
	A[2] = targetPointXYZ.Z;
	MultiMat(OppRzxy, A, G, 3, 3, 1);

	PointXYZ result;
	result.X = G[0] + selfPointXYZ.X;
	result.Y = G[1] + selfPointXYZ.Y;
	result.Z = G[2] + selfPointXYZ.Z;
	return result;
}

//*********************************************************************************************
// 由CGCS直角坐标变换到NUE直角坐标
// targetPointXYZ：CGCS下的目标坐标
// selfPointXYZ：测站在CGCS下的直角坐标
// 返回NUE下的目标坐标
//*********************************************************************************************
PointXYZ getNUEXYZFromCGCSXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
{
	// 将测站所在位置的CGCS直角坐标变换到地理坐标
	PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);

	// 计算中间值
	double dx = targetPointXYZ.X - selfPointXYZ.X;
	double dy = targetPointXYZ.Y - selfPointXYZ.Y;
	double dz = targetPointXYZ.Z - selfPointXYZ.Z;

	/* 由CGCS直角坐标变换到NUE直角坐标 */
	double A[3];
	double G[3];
	double OppRx[9];
	double OppRy[9];
	double OppRz[9];
	double OppRyx[9];
	double OppRyxz[9];

	// 1、计算旋转矩阵
	Rx(selfPointBLH.B, OppRx);
	Ry(90, OppRy);
	Rz(-90 + selfPointBLH.L, OppRz);

	MultiMat(OppRy, OppRx, OppRyx, 3, 3, 3);
	MultiMat(OppRyx, OppRz, OppRyxz, 3, 3, 3);

	// 2、将CGCS直角坐标变换到NUE直角坐标
	A[0] = dx;
	A[1] = dy;
	A[2] = dz;
	MultiMat(OppRyxz, A, G, 3, 3, 1);

	PointXYZ result;
	result.X = G[0];
	result.Y = G[1];
	result.Z = G[2];
	return result;
}



// 绕X轴旋转矩阵
/* 1    0      0
0    cos    sin
0   -sin    cos
*/
void Rx(double omega, double *Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = 1;
	Opp[1] = 0;
	Opp[2] = 0;
	Opp[3] = 0;
	Opp[4] = coso;
	Opp[5] = sino;
	Opp[6] = 0;
	Opp[7] = -sino;
	Opp[8] = coso;
}

// 绕Y轴旋转矩阵
/* cos    0    sin
0     1     0
-sin  0    cos
*/
void Ry(double omega, double *Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = coso;
	Opp[1] = 0;
	Opp[2] = sino;
	Opp[3] = 0;
	Opp[4] = 1;
	Opp[5] = 0;
	Opp[6] = -sino;
	Opp[7] = 0;
	Opp[8] = coso;
}


// 绕Z轴旋转矩阵 俯仰角
/* cos    sin    0
-sin cos  0
0   0     1
*/
void Rz(double omega, double *Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = coso;
	Opp[1] = sino;
	Opp[2] = 0;
	Opp[3] = -sino;
	Opp[4] = coso;
	Opp[5] = 0;
	Opp[6] = 0;
	Opp[7] = 0;
	Opp[8] = 1;
}

// 镜面反射矩阵计算
void RMirror(PointXYZ normalVec, double *Opp)
{
	//	M = eye(3)-(2*N*N');
	Opp[0] = 1-2*normalVec.X*normalVec.X;
	Opp[1] = -2*normalVec.X*normalVec.Y;
	Opp[2] = -2*normalVec.X*normalVec.Z;
	Opp[3] = -2*normalVec.X*normalVec.Y;
	Opp[4] = 1-2*normalVec.Y*normalVec.Y;
	Opp[5] = -2*normalVec.Y*normalVec.Z;
	Opp[6] = -2*normalVec.X*normalVec.Z;
	Opp[7] = -2*normalVec.Y*normalVec.Z;
	Opp[8] = 1-2*normalVec.Z*normalVec.Z;

}
//*********************************************************************************************
//  矩阵乘法，矩阵A*矩阵B
//*********************************************************************************************
void  MultiMat(double *A, double *B, double *AB, int m, int n, int p)
{
	// A为m*n ; B为n*p  ;输出AB为m*p
	int	i, j, k;
	double Multisum;

	for (i = 0; i < m; i++) //输出矩阵的m行
	{
		for (j = 0; j < p; j++) //输出矩阵的p列
		{
			Multisum = 0;
			for (k = 0; k < n; k++)
				Multisum += A[i*n + k] * B[k*p + j];

			AB[i*p + j] = Multisum;
		}
	}
}

//*********************************************************************************************
//  3阶矩阵乘法，矩阵A*矩阵B
//*********************************************************************************************
void MultiMat33(double * A, double * B, double * AB)
{
	MultiMat(A, B, AB, 3, 3, 3);
}

//*********************************************************************************************
//  3阶矩阵乘法，矩阵A*矩阵B*矩阵C
//*********************************************************************************************
void MultiMat333(double * A, double * B, double * C, double * ABC)
{
	double BC[9];
	MultiMat(B, C, BC, 3, 3, 3);
	MultiMat(A, BC, ABC, 3, 3, 3);
}

// 3维空间旋转矩阵执行坐标转换
PointXYZ RtPoint(double * M, PointXYZ Point)
{
	// M * [X,Y,Z]'
	double A[3];
	double G[3];
	A[0] = Point.X;
	A[1] = Point.Y;
	A[2] = Point.Z;
	MultiMat(M, A, G, 3, 3, 1);

	PointXYZ result;
	result.X = G[0];
	result.Y = G[1];
	result.Z = G[2];
	return result;

}

// 归一化
PointXYZ NormPoint(PointXYZ Point)
{
	double Len = sqrt(Point.X * Point.X + Point.Y * Point.Y + Point.Z * Point.Z);
    Point.X /= Len;
	Point.Y /= Len;
	Point.Z /= Len;
	return Point;
}
void getCarNUEXYZFromEOCamXYZ_Mat(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta,double* Mat)
{
	double oppRz[9];
	double oppRy[9];
	double oppRx[9];

	// 1.光电镜面直角坐标 转 光电伺服直角坐标

	// 先绕Z轴俯仰角旋转-alaph，后绕Y轴方位角旋转-beta
	double oppRyz_servo[9];
	// 计算旋转矩阵
	Rz(-servoAlpha, oppRz);
	Ry(-servoBeta, oppRy);
	MultiMat33(oppRy,oppRz, oppRyz_servo);

	// 2.光电伺服直角坐标 转 光电基准直角坐标

	// 先绕x轴横滚旋转 -eRoll，再绕z轴俯仰旋转 -ePitch，再绕y轴方位旋转-eYaw
	double oppRyzx_base[9];
	// 计算旋转矩阵
	Rx(-g_RollBias, oppRx);
	Rz(-g_PitchBias, oppRz);
	Ry(-g_YawBias, oppRy);
	MultiMat333(oppRy,oppRz, oppRx, oppRyzx_base);

	// 3.光电基准直角坐标 转 平台直角坐标
	// 不考虑平移关系
	
	// 4.平台直角坐标 转 平台地理直角坐标

	double oppRyzx_GEO[9];
	// 计算旋转矩阵
	Rx(-mRoll, oppRx);
	Rz(-mPitch, oppRz);
	Ry(-mYaw, oppRy);
	MultiMat333(oppRy,oppRz, oppRx, oppRyzx_GEO);


	// 连续旋转矩阵计算,注意顺序
	MultiMat333(oppRyzx_GEO,oppRyzx_base,oppRyz_servo,Mat);
}