双像空间前方交会的源码

double a11,a12,a13,b11,b12,b13,c11,c12,c13;
 double L11,L12,L13,L1x,L14,L15,L16,L1y;
 
 //系数阵
 a11=cos(m_Phi_L1)*cos(m_Kappa_L1)-sin(m_Phi_L1)*sin(m_Omega_L1)*sin(m_Kappa_L1);
 a12=-cos(m_Phi_L1)*sin(m_Kappa_L1)-sin(m_Phi_L1)*sin(m_Omega_L1)*cos(m_Kappa_L1);
 a13=-sin(m_Phi_L1)*cos(m_Omega_L1);
 b11=cos(m_Omega_L1)*sin(m_Kappa_L1);
 b12=cos(m_Omega_L1)*cos(m_Kappa_L1);
 b13=-sin(m_Omega_L1);
 c11=sin(m_Phi_L1)*cos(m_Kappa_L1)+cos(m_Phi_L1)*sin(m_Omega_L1)*sin(m_Kappa_L1);
 c12=-sin(m_Phi_L1)*sin(m_Kappa_L1)+cos(m_Phi_L1)*sin(m_Omega_L1)*cos(m_Kappa_L1);
 c13=cos(m_Phi_L1)*cos(m_Omega_L1);

    L11=_m_F_L*a11+(_m_x_L-m_X0_L)*a13;
 L12=_m_F_L*b11+(_m_x_L-m_X0_L)*b13;
 L13=_m_F_L*c11+(_m_x_L-m_X0_L)*c13;
    L1x=_m_F_L*a11*m_Xs_L+_m_F_L*b11*m_Ys_L+_m_F_L*c11*m_Zs_L+(_m_x_L-m_X0_L)*a13*m_Xs_L+
        (_m_x_L-m_X0_L)*b13*m_Ys_L+(_m_x_L-m_X0_L)*c13*m_Zs_L;
    L14=_m_F_L*a12+(_m_y_L-m_Y0_L)*a13;
 L15=_m_F_L*b12+(_m_y_L-m_Y0_L)*b13;
 L16=_m_F_L*c12+(_m_y_L-m_Y0_L)*c13;
    L1y=_m_F_L*a12*m_Xs_L+_m_F_L*b12*m_Ys_L+_m_F_L*c12*m_Zs_L+(_m_y_L-m_Y0_L)*a13*m_Xs_L+
        (_m_y_L-m_Y0_L)*b13*m_Ys_L+(_m_y_L-m_Y0_L)*c13*m_Zs_L;
 
 double a21,a22,a23,b21,b22,b23,c21,c22,c23;
    double L21,L22,L23,L2x,L24,L25,L26,L2y;
    
 a21=cos(m_Phi_R1)*cos(m_Kappa_R1)-sin(m_Phi_R1)*sin(m_Omega_R1)*sin(m_Kappa_R1);
 a22=-cos(m_Phi_R1)*sin(m_Kappa_R1)-sin(m_Phi_R1)*sin(m_Omega_R1)*cos(m_Kappa_R1);