/*********版权所有（C）2020，武汉高德红外股份有限公司***************************************
* 文件名称： Arith_VehicleCoordModule.h
* 文件标识： 车载坐标转换模块标准化
* 内容摘要： 图像坐标系 <-> 平台地理坐标系，基于北-天-东 轴序
* 其它说明： 
* 当前版本：2.0
* 创建作者：04046wcw
* 创建日期：2020/12/03
* 包含关系：
* *********************************************************************************
* 以前版本：1.1
* 作    者：陈毅
* 完成日期：2014年9月23日
*****************************************************************************************/
#ifndef Arith_VehicleCoordModule_h__
#define Arith_VehicleCoordModule_h__


/*
// 坐标系示意图
y    x（车体前进方向）
▕
▕  ╱
▕ ╱
▕-——————————z

*/


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


// 椭球体定义
const double a = 6378137;
const double f = 1 / 298.257222101;
const double b = ((1 - f)*a);
const double ee = (1 - (1 - f)*(1 - f));
const double epep = (a*a / b / b - 1);


#ifndef _ARITH_GLOBAL_
#define ANGLE(x) (x / 3.141592653589793 * 180)
#define RADIAN(x) (x / 180 * 3.141592653589793)
#define	ABS(x)    ((x) < 0 ? -(x) : (x))                  //取x的绝对值
#define	MIN(a, b)               ((a) < (b) ? (a) : (b))                 //取a、b两者中的最小值
#define	MAX(a, b)               ((a) > (b) ? (a) : (b))                 //取a、b两者中的最大值
#endif

#ifndef _POINTXYZ_
#define _POINTXYZ_
// 空间直角坐标系,也可以看做向量
typedef struct tgPointXYZ {
	double X;
	double Y;
	double Z;
}PointXYZ;//单位：米
#endif


#ifndef _POLE_
#define _POLE_
//极坐标系
typedef struct tgPointPole {
	double alpha;    // 俯仰角
	double beta;     // 方位角
	double distance; // 极径
}Pole;//单位：°
#endif

#ifndef _RPY_
#define _RPY_
//RPY姿态角
typedef struct tgEulerRPY {
	double fRoll;//横滚角
	double fPitch;//俯仰角
	double fYaw;//方位角
}EulerRPY;//单位：°
#endif


#ifndef _POINTBLH_
#define _POINTBLH_
typedef struct tgPointBLH
{
	double B;        // 纬度
	double L;        // 经度
	double H;        // 高程
}PointBLH;           // 地理坐标系  //单位：°
#endif


void Coord_Test();


// 设置标定参数
void setBiasPara(double YawBias, double PitchBias, double RollBias,	double deltaX, double deltaY, double deltaZ);

// 获取当前标定参数
void getBiasPara(double *YawBias, double *PitchBias, double *RollBias, double *deltaX, double *deltaY, double *deltaZ);



// S726二维指向镜版本          by wcw04046 @ 2021/12/03
Pole getPointMirrorServoPoleFromImageXY(int Col,int Row,int nWidth,int nHeight,
										float fServoA,float fServoB,float fLen,float pxSizeOfum);


void getPointMirrorImageXYFromServoPole(int* Col,int* Row,int nWidth,int nHeight,float WAlaph,float WBeta,
										float fServoA,float fServoB,float fLen,float pxSizeOfum);



// IR370A-H补充接口

//光电镜面直角坐标 转 平台地理直角坐标 的 旋转矩阵。如果考虑平移需引入齐次坐标，不方便使用，这里简化处理
void getCarNUEXYZFromEOCamXYZ_Mat(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta,double* Mat);

// 相机坐标Z轴正方向(平行于图像X方向)在大地系下的偏转角度
Pole getCamZaxisNUERota(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta);

///////////////////////////////////////目标上报//////////////////////////////////////////////////////////
// 光电镜面极坐标 转 平台地理极坐标
Pole getCarNUEPoleFromEOCamPole(Pole targetCamPole,	double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta);

// 光电镜面直角坐标 转 光电伺服直角坐标
PointXYZ getEOServoXYZFromEOCamXYZ(PointXYZ targetCamXYZ,double alaph, double beta);

// 光电伺服直角坐标 转 光电基准直角坐标
PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, double eYaw, double ePitch, double eRoll);

// 光电基准直角坐标 转 平台直角坐标
PointXYZ getCarXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double deltaX, double deltaY, double deltaZ);

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



///////////////////////////////////////目标导引//////////////////////////////////////////////////////////
Pole getEOServoPoleFromCarNUEPole(Pole targetCarNUEPole, Pole* targetEOCamPole,double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta);

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

// 平台直角坐标 转 光电基准直角坐标
PointXYZ getEOBaseXYZFromCarXYZ(PointXYZ targetCarXYZ, double deltaX, double deltaY, double deltaZ);
 
// 光电基准直角坐标 转 光电伺服直角坐标 
PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double eYaw, double ePitch, double eRoll);

// 光电伺服直角坐标 转 光电镜面直角坐标
PointXYZ getEOCamXYZFromEOServoXYZ(PointXYZ targetEOServoXYZ, double alaph, double beta);



/////////////////////////////////////////大地坐标相关///////////////////////////////////////////////////////

//大地坐标转地球空间直角坐标
PointXYZ getXYZFromBLH(PointBLH BLH);

//地球空间直角坐标-->大地坐标
PointBLH getBLHFromXYZ(PointXYZ pointXYZ);

//由NUE直角坐标变换到CGCS直角坐标
PointXYZ getCGCSXYZFromNUEXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ);

//由CGCS直角坐标变换到NUE直角坐标
PointXYZ getNUEXYZFromCGCSXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ);


////////////////////////////////////////////////////////////////////////////////

// 直角坐标转极坐标（右手系）
Pole getPoleFromXYZ(PointXYZ targetXYZ);

// 极坐标转直角坐标（右手系）
PointXYZ getXYZFromPole(Pole targetPole);




// 矩阵乘法
void  MultiMat(double *A, double *B, double *AB, int m, int n, int p);
// M1(3*3) * M2(3*3)
void  MultiMat33(double *A, double *B, double *AB);
// M1(3*3) * M2(3*3) * M3(3*3)
void  MultiMat333(double *A, double *B, double *C, double *ABC);

// 坐标(矢量)旋转: M1(3*3) * M2(3,1)
PointXYZ  RtPoint(double *M, PointXYZ Point);

// 坐标(矢量)归一化
PointXYZ NormPoint(PointXYZ Point);


// 绕X轴旋转矩阵
void Rx(double omega, double *Opp);

// 绕Y轴旋转矩阵
void Ry(double omega, double *Opp);

// 绕Z轴旋转矩阵
void Rz(double omega, double *Opp);

// 镜面反射矩阵
void RMirror(PointXYZ normalVec, double *Opp);




#endif // Arith_VehicleCoordModule_h__