CSUtils::ECEFPoint CSUtils::enu_to_ecef(const CSUtils::ENUPoint& _point, const CSUtils::GCSPoint& _origin)
{
double phi = _origin.latitude;
double lamda = _origin.longitude;
double h = _origin.altitude;
Eigen::Vector3d lccs_center_point;
lccs_center_point(0) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * cos(lamda);
lccs_center_point(1) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * sin(lamda);
lccs_center_point(2) = b * sin(atan(b * tan(phi) / a)) + h * sin(phi);
Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
C_1(0, 2) = -1.0; C_1(1, 1) = 1.0; C_1(2, 0) = 1.0;
Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
C_2(0, 0) = 1.0;
C_2(1, 1) = cos(phi); C_2(1, 2) = -sin(phi);
C_2(2, 1) = sin(phi); C_2(2, 2) = cos(phi);
Eigen::Matrix3d C_3 = Eigen::Matrix3d::Zero();
C_3(0, 0) = sin(lamda); C_3(0, 1) = cos(lamda);
C_3(1, 0) = -cos(lamda); C_3(1, 1) = sin(lamda);
C_3(2, 2) = 1.0;
Eigen::Matrix3d C = C_3 * C_2 * C_1;
Eigen::Vector3d ret = lccs_center_point + C * Eigen::Vector3d(_point.x, _point.y, _point.z);
return CSUtils::ECEFPoint(ret(0), ret(1), ret(2));
}
void eval(void)
{ int tmp ;
int tmp___0 ;
int tmp___1 ;
int tmp___2 ;
{
{
while (1) {
while_3_continue: /* CIL Label */
;
{
tmp___2 = exists_runnable_thread();
}
if (tmp___2) {
} else {
goto while_3_break;
}
if ((int )P_1_st == 0) {
{
tmp = __VERIFIER_nondet_int();
}
if (tmp) {
{
P_1_st = 1;
P_1();
}
} else {
}
} else {
}
if ((int )C_1_st == 0) {
{
tmp___1 = __VERIFIER_nondet_int();
}
if (tmp___1) {
{
C_1_st = 1;
C_1();
}
} else {
}
} else {
}
}
while_3_break: /* CIL Label */
;
}
return;
}
}
CSUtils::ENUPoint CSUtils::ccsa_to_enu(const CCSAPoint& _point, const Angle& _eps, const Angle& _beta)
{
double eps = degree_to_rad(_eps);
double beta = degree_to_rad(_beta);
Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
C_1(0, 0) = cos(beta); C_1(0, 2) = -sin(beta);
C_1(1, 1) = 1.0;
C_1(2, 0) = sin(beta); C_1(2, 2) = cos(beta);
Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
C_2(0, 0) = cos(eps); C_2(0, 1) = sin(eps);
C_2(1, 0) = -sin(eps); C_2(1, 1) = cos(eps);
C_2(2, 2) = 1.0;
Eigen::Vector3d ret = (C_2 * C_1).transpose() * Eigen::Vector3d(_point.x, _point.y, _point.z);
return CSUtils::ENUPoint(ret(0), ret(1), ret(2));
}
