/* * not enought precision with float - use double */ void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) { /* convert used floats to double */ struct DoubleMat33 ltp_of_ecef_d; ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0]; ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1]; ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2]; ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3]; ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4]; ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5]; ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6]; ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7]; ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8]; struct EnuCoor_f enu_d; enu_d.x = (double) enu->x; enu_d.y = (double) enu->y; enu_d.z = (double) enu->z; /* compute in double */ struct EcefCoor_d ecef_d; MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d); /* convert result back to float and add it*/ ecef->x = (float) ecef_d.x + def->ecef.x; ecef->y = (float) ecef_d.y + def->ecef.y; ecef->z = (float) ecef_d.z + def->ecef.z; }
void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) { MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu); }