ADD: added Movement calculation and a Tracklist class

data/movement.txt.txt

@@ -0,0 +1,171 @@
+// 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;
+}
+