#include "Arith_CoordModule.h"

//#include "Arith_SysVar.h"
#include <cmath>
#include "Arith_Utils.h"



// 从惯性系（建航坐标系）到  像坐标系
POINT32F getImagePosFromStablePole(Pole stablePole, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err)
{
	POINT32F imgPos = { 0 };

	// 从稳定性到设备系
	Pole devicePole = getDevicePoleFromStablePole(stablePole, att, err);

	// 从设备系到像方
	imgPos = getImagePosFromDevicePole(devicePole, cam, servoInfo);

	return imgPos;

}

// 从像坐标系 到 惯性系（建航坐标系）
Pole getStablePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err)
{
	Pole StablePole = { 0 };

	// 从像方到设备系
	Pole devicePole = getDevicePoleFromImagePos(imagePos, cam, servoInfo);

	// 从设备系到惯性系
	StablePole = getStablePoleFromDevicePole(devicePole, att, err);

	return StablePole;
}



BBOOL bTargetInScanFov(ANGLE32F targetPos, Pole scanNueRange1, Pole scanNueRange2)
{
	BBOOL AzInScan = TRUE;
	BBOOL PtInScan = TRUE;
	// 俯仰扫描区间有效
	if (scanNueRange1.alpha != 0 && scanNueRange2.alpha != 0)
	{
		if (targetPos.fPt < scanNueRange1.alpha || targetPos.fPt > scanNueRange2.alpha)
		{
			PtInScan = FALSE;
		}

	}

	// 方位扫描区间有效
	if (scanNueRange1.beta != 0 && scanNueRange2.beta != 0)
	{
		if (targetPos.fAz < scanNueRange1.beta || targetPos.fAz > scanNueRange2.beta)
		{
			AzInScan = FALSE;
		}
	}

	return (AzInScan && PtInScan);

}

Pole getDevicePoleFromStablePole(Pole stablePole, EulerRPY att, DeviceSetupError err)
{
	Pole targetCarNUEPole = stablePole;

	// 地理极坐标 转 地理直角坐标
	PointXYZ targetCarNUEXYZ = getXYZFromPole(targetCarNUEPole);

	// 地理直角坐标 转 平台直角坐标
	PointXYZ targetCarXYZ = getCarXYZFromCarNUEXYZ(targetCarNUEXYZ, att.fYaw, att.fPitch, att.fRoll);

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

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

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

	return targetEOServoPole;

}

Pole getStablePoleFromDevicePole(Pole devicePole, EulerRPY att, DeviceSetupError err)
{
	// 设备系转伺服直角坐标
	PointXYZ targetServoXYZ = getXYZFromPole(devicePole);

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

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

	// 平台直角坐标 转 平台地理直角坐标
	PointXYZ targetCarNUEXYZ = getCarNUEXYZFromCarXYZ(targetCarXYZ, att.fYaw, att.fPitch, att.fRoll);

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

	return targetCarNUEPole;

}



// 像方转设备系
Pole getDevicePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo)
{
	Pole targetDevicePole = { 0 };

	if(cam.bImageRataSys)
	{
		double refmat[9] = { 0 };
		PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat);

		targetDevicePole = PMS_ProjectPixelToWorld((int)imagePos.x, (int)imagePos.y, refmat, 
			cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth / 2, cam.dy + cam.ImageHeight / 2);
	}
	else
	{
		// 像方转相机坐标系
		Pole targetCamPole = { 0 };
		float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000));
		SINT32 cx = cam.dx + cam.ImageWidth / 2;
		SINT32 cy = cam.dy + cam.ImageHeight / 2;
		targetCamPole.beta = (imagePos.x - cx) * fAglRes;
		targetCamPole.alpha = (cy - imagePos.y) * fAglRes;

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

		// 光电镜面直角坐标 转 光电伺服直角坐标
		PointXYZ targetServoXYZ = getEOServoXYZFromEOCamXYZ(targetCamXYZ, servoInfo.fPt, servoInfo.fAz);

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

		targetDevicePole = getPoleFromXYZ(targetServoXYZ);
	}


	return targetDevicePole;
}


// 设备系转像方坐标
POINT32F getImagePosFromDevicePole(Pole devicePole, CameraParam cam, ServoParam servoInfo)
{
	POINT32F imgpos = { 0 };

	// 常规光电成像模型
	if(cam.bImageRataSys)
	{
		double refmat[9] = { 0 };
		double refmat_inv[9] = { 0 };
		PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat);
		//invMat3(refmat, refmat_inv);
		TransposeMat(refmat, 3, 3, refmat_inv);//等价

		PointXYZ imgPosition = 
			PMS_ProjectWorldToPixel((float)devicePole.beta, (float)devicePole.alpha, refmat_inv, 
				cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth/2, cam.dy + cam.ImageHeight/2);

		imgpos.x = (float)imgPosition.X;
		imgpos.y = (float)imgPosition.Y;

	}
	else
	{
		// 设备极坐标转直角坐标
		PointXYZ targetEOServoXYZ = getXYZFromPole(devicePole);

		// 光电伺服直角 转 光电镜面直角坐标(相机系)
		PointXYZ targetEOCamXYZ = getEOCamXYZFromEOServoXYZ(targetEOServoXYZ, servoInfo.fPt, servoInfo.fAz);

		//  光电镜面极坐标
		Pole targetEOCamPole = getPoleFromXYZ(targetEOCamXYZ);

		float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000.0f));
		SINT32 cx = cam.dx + cam.ImageWidth / 2;
		SINT32 cy = cam.dy + cam.ImageHeight / 2;
		imgpos.x = FLOAT32(targetEOCamPole.beta / fAglRes + cx);
		imgpos.y = FLOAT32(cy - targetEOCamPole.alpha / fAglRes);
	}

	return imgpos;
}





void PMS_MultiRefMat(float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, double* mat)
{
	// 反射镜1
	// fa -- 方位角输出-外框
	// fb -- 俯仰角输出-内框
	PointXYZ N1_0;
	N1_0.X = 1 / sqrt(2.0);
	N1_0.Y = 1 / sqrt(2.0);
	N1_0.Z = 0;

	// n1绕z轴旋转fb（向上为正）
	double Sz[9] = { cosd(fMainAPt),sind(fMainAPt),0,
	-sind(fMainAPt),cosd(fMainAPt),0,
	0,0,1 };

	// 再绕y轴旋转fa（向右为正）
	double Sy[9] = { cosd(fMainAz),0,-sind(fMainAz),
		0,1,0,
		sind(fMainAz),0,cosd(fMainAz) };

	// 反射镜1法向量  %% 前两个法向量在XOY面里
	//N1_F = Sy *  Sz * N1_0;
	PointXYZ N1_F = RtPoint(Sy, RtPoint(Sz, N1_0));

	// 反射镜1反射矩阵
	//R1 = eye(3) - (2 * N1_F * N1_F');
	double R1[9] = { 0 };
	RMirror(N1_F, R1);


	// 反射镜23，没有动  %%注意：第3、4面镜子已经在XOZ面里了，注意空间关系
	PointXYZ N2_0;
	N2_0.X = -1 / sqrt(2.0);
	N2_0.Y = -1 / sqrt(2.0);
	N2_0.Z = 0;
	double R2[9] = { 0 };
	RMirror(N2_0, R2);

	PointXYZ N3_0;
	N3_0.X = 1 / sqrt(2.0);
	N3_0.Y = 0;
	N3_0.Z = 1 / sqrt(2.0);
	double R3[9] = { 0 };
	RMirror(N3_0, R3);

	// 反射镜4是快反
	// Qa -- 方位角输出-外框,向右偏转为正
	// Qb -- 俯仰角输出-内框，抬头为正
	PointXYZ N4_0;
	N4_0.X = 1 / sqrt(2.0);
	N4_0.Y = 0;
	N4_0.Z = -1 / sqrt(2.0);

	// 外框扫描轴为y轴，内框扫描轴L0为x=z 即(1,0,1)'
	double Sy2[9] = { cosd(fFSMAz),0,-sind(fFSMAz),
		0,1,0,
		sind(fFSMAz),0,cosd(fFSMAz) };

	// 内框旋转向量,注意左手系，会引入负号
	PointXYZ L0;
	L0.X = 1;
	L0.Y = 0;
	L0.Z = 1;
	double SL2[9] = { 0 };
	rotationVectorToMatrix(L0, -fFSMPt, SL2);

	//快反镜法向量，经过内框和外框旋转
	// N4_F = Sy2 *  SL2 * N4_0;
	PointXYZ N4_F = RtPoint(Sy2, RtPoint(SL2, N4_0));

	// 快反的反射矩阵
	double R4[9] = { 0 };
	RMirror(N4_F, R4);

	// 整个光路的反射模型,从像方到物方
	double tmp[9] = { 0 };
	MultiMat333(R2, R3, R4, tmp);
	MultiMat33(R1, tmp, mat);


}

// 像方到物方的映射,转到方位俯仰角
Pole PMS_ProjectPixelToWorld(int x, int y, float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, float f_px, int cx, int cy)
{
	// 计算反射矩阵
 	double mat[9] = { 0 };
 	PMS_MultiRefMat(fMainAz, fMainAPt, fFSMAz, fFSMPt, mat);
 	return PMS_ProjectPixelToWorld(x, y, mat, f_px, cx, cy);
}

Pole PMS_ProjectPixelToWorld(int x, int y, double* mat, float f_px, int cx, int cy)
{
	Pole worldPole = { 0 };
	// 内参信息
	// const int nWidth = 860;
	// const int nHeight = 715;
	// const float f = 350;
	// const int piexlSize = 7.5;
	// float fpiexl = f/piexlSize * 1000; //像素焦距

	// 从靶面出射的矢量
	PointXYZ PO;
	PO.X = -f_px;
	PO.Y = -(y - cy);
	PO.Z = -(x - cx);

	//%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里
	PO.Y = -PO.Y;

	// 出射矢量
	PointXYZ OQ;
	OQ = RtPoint(mat, PO);



	// 物方角度
	double len = sqrt(PO.X * PO.X + PO.Y * PO.Y + PO.Z * PO.Z);


	worldPole.alpha = ANGLE(asin(-OQ.Y / len));
	worldPole.beta = ANGLE(atan2(OQ.Z, OQ.X));

	return worldPole;
}

// PointXYZ PMS_ProjectPixelToWorldVec(int x, int y, double* mat, float f_px, int cx, int cy)
// {
// 	Pole worldPole = { 0 };
// 	// 内参信息
// 	// const int nWidth = 860;
// 	// const int nHeight = 715;
// 	// const float f = 350;
// 	// const int piexlSize = 7.5;
// 	// float fpiexl = f/piexlSize * 1000; //像素焦距

// 	// 从靶面出射的矢量
// 	PointXYZ PO;
// 	PO.X = -f_px;
// 	PO.Y = -(y - cy);
// 	PO.Z = -(x - cx);

// 	//%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里
// 	PO.Y = -PO.Y;

// 	// 出射矢量
// 	PointXYZ OQ;
// 	OQ = RtPoint(mat, PO);

// 	return OQ;

// }
//物方到像方的映射,转到像素位置
PointXYZ PMS_ProjectWorldToPixel(float fAz, float fPt, double* refmat, float f_px, int cx, int cy)
{
	PointXYZ pxPos = { 0 };

	// 物方直角系出射矢量
	PointXYZ OQ = { 0 };
	float Dep = 1000;//单位向量
	OQ.Y = -Dep * sind(fPt);
	OQ.X = Dep * cosd(fPt) * cosd(fAz);
	OQ.Z = Dep * cosd(fPt) * sind(fAz);


	// 投影,得到靶面出射矢量
	PointXYZ PO = RtPoint(refmat, OQ);

	PointXYZ OP = { 0 };//射向靶面
	OP.X = -PO.X;
	OP.Y = -PO.Y;
	OP.Z = -PO.Z;


	// 内参矩阵
	pxPos.X = f_px * OP.Z / OP.X + cx;
	pxPos.Y = -f_px * OP.Y / OP.X + cy;


	return pxPos;
}

// PointXYZ PMS_ProjectWorldVecToPixel(PointXYZ worldVec, double* refmat, float f_px, int cx, int cy)
// {
// 	PointXYZ pxPos = { 0 };
// 	// 投影,得到靶面出射矢量
// 	PointXYZ PO = RtPoint(refmat, worldVec);

// 	PointXYZ OP = { 0 };//射向靶面
// 	OP.X = -PO.X;
// 	OP.Y = -PO.Y;
// 	OP.Z = -PO.Z;


// 	// 内参矩阵
// 	pxPos.X = f_px * OP.Z / OP.X + cx;
// 	pxPos.Y = -f_px * OP.Y / OP.X + cy;


// 	return pxPos;
// }


// // S726像方到物方转换
// 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;

// }

// // S726物方到像方转换
// 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);



// }




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);

}

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°

	// 吊舱无源定位允许最低高度10米
	if (targetPole.distance < 10)
	{
		targetPole.distance = 10000;
	}

	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;
}