172 lines
4.9 KiB
Plaintext
172 lines
4.9 KiB
Plaintext
// Topocentric Coordinates (U=east, V=north, W=up)
|
|
struct UVW
|
|
{
|
|
double dU,dV,dW;
|
|
};
|
|
|
|
// Kinematic Vector in m/s
|
|
struct Kin_Vect
|
|
{
|
|
double dVx, dVy, dVz;
|
|
};
|
|
|
|
// Geographic Coordinates in rad
|
|
struct LLA_Rad
|
|
{
|
|
double dLat, dLon, dAlt;
|
|
};
|
|
|
|
struct XYZ
|
|
{
|
|
double dX = 0,dY = 0,dZ = 0;
|
|
};
|
|
|
|
// Kinematic Parameters (dCrs, dClb in rad, dSpd in m/s)
|
|
struct Movement
|
|
{
|
|
double dCrs, dSpd, dClb;
|
|
};
|
|
|
|
|
|
|
|
/** Generate_Movement
|
|
*
|
|
* Generates the kinematic data based on the
|
|
* geocentric kinematic vector and the loaction
|
|
*/
|
|
Movement Generate_Movement(Kin_Vect Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
|
|
{
|
|
Movement Result;
|
|
|
|
// Transform geocentric vector to topocentric vector
|
|
|
|
|
|
UVW UVW_Coord;
|
|
|
|
UVW_Coord.dU = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLon) +
|
|
(Kin_Dat.dVy) * cos(LLA_rCoord.dLon);
|
|
|
|
UVW_Coord.dV = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) -
|
|
(Kin_Dat.dVy) * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
|
|
(Kin_Dat.dVz) * cos(LLA_rCoord.dLat);
|
|
|
|
UVW_Coord.dW = (Kin_Dat.dVx) * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
|
|
(Kin_Dat.dVy) * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
|
|
(Kin_Dat.dVz) * sin(LLA_rCoord.dLat);
|
|
|
|
|
|
// Transform topocentric vector to Mov_Dat
|
|
|
|
if (abs(UVW_Coord.dU) < dEPS and abs(UVW_Coord.dV) < dEPS and abs(UVW_Coord.dW) < dEPS)
|
|
{
|
|
Result.dSpd = 0;
|
|
|
|
Result.dClb = 0;
|
|
|
|
Result.dCrs = 0;
|
|
|
|
} else
|
|
{
|
|
Result.dSpd = pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2) + pow(UVW_Coord.dW,2), 0.5);
|
|
|
|
Result.dCrs = ShiftRadCrs(atan2( UVW_Coord.dU, UVW_Coord.dV ));
|
|
|
|
Result.dClb = ShiftRadClb(atan2( UVW_Coord.dW, pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2), 0.5) ));
|
|
}
|
|
|
|
|
|
return Result;
|
|
};
|
|
|
|
|
|
/** Generate_Kin_Vect
|
|
*
|
|
* Generates the geocentric kinematic vector based on the
|
|
* kinematic data and the location
|
|
*/
|
|
Kin_Vect Generate_Kin_Vect(Movement Mov_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
|
|
{
|
|
Kin_Vect Result;
|
|
|
|
//Transform Mov_Dat to topocentric vector
|
|
UVW UVW_Coord;
|
|
// Topocentric Coordinates (U=east, V=north, W=up)
|
|
|
|
UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
|
|
UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
|
|
UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);
|
|
|
|
//Transform topocentric vector to geocentric vector
|
|
|
|
Result.dVx = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
|
|
UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
|
|
UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);
|
|
|
|
Result.dVy = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
|
|
UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
|
|
UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);
|
|
|
|
Result.dVz = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
|
|
UVW_Coord.dW * sin(LLA_rCoord.dLat);
|
|
|
|
|
|
return Result;
|
|
};
|
|
|
|
|
|
/// Return new position after Timestep
|
|
inline XYZ GetNewPosKin(XYZ StartPos, Kin_Vect* Motion, Acc_Vect Acceleration, unsigned int Timestep, unsigned char DR_Algorithm = 5)
|
|
{
|
|
XYZ Result;
|
|
|
|
double dTimestep = static_cast<double>(Timestep);
|
|
|
|
if (DR_Algorithm == 4 or DR_Algorithm == 5)
|
|
{
|
|
Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0 + 0.5 * Acceleration.dAx * pow(dTimestep / 1000.0, 2);
|
|
Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0 + 0.5 * Acceleration.dAy * pow(dTimestep / 1000.0, 2);
|
|
Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0 + 0.5 * Acceleration.dAz * pow(dTimestep / 1000.0, 2);
|
|
|
|
Motion->dVx += Acceleration.dAx * dTimestep / 1000.0;
|
|
Motion->dVy += Acceleration.dAy * dTimestep / 1000.0;
|
|
Motion->dVz += Acceleration.dAz * dTimestep / 1000.0;
|
|
}
|
|
else
|
|
{
|
|
Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0;
|
|
Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0;
|
|
Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0;
|
|
};
|
|
|
|
return Result;
|
|
}
|
|
|
|
// Geographic Coordinates in rad
|
|
struct LLA_Rad
|
|
{
|
|
double dLat, dLon, dAlt;
|
|
};
|
|
|
|
|
|
/// Return new position after Timestep
|
|
inline LLA_Rad GetNewPos(LLA_Rad StartPos, Movement Motion, unsigned int Timestep)
|
|
{
|
|
LLA_Rad Result;
|
|
|
|
double Distance = Motion.dSpd * cos(Motion.dClb) * Timestep / 1000.0;
|
|
|
|
double DeltaAlt = Motion.dSpd * sin(Motion.dClb) * Timestep / 1000.0;
|
|
|
|
Result.dLat = asin( sin(StartPos.dLat) * cos( Distance / dR) + cos(StartPos.dLat) * sin( Distance / dR) * cos(Motion.dCrs));
|
|
|
|
Result.dLon = StartPos.dLon + atan2(sin(Motion.dCrs) * sin( Distance / dR) * cos(StartPos.dLat),
|
|
cos( Distance / dR) - sin(StartPos.dLat) * sin(Result.dLat));
|
|
|
|
Result.dAlt = StartPos.dAlt + DeltaAlt;
|
|
|
|
Result = ShiftLLA_Rad(Result);
|
|
|
|
return Result;
|
|
}
|
|
|