delete duplication

Author: kenjihiranabe

drone_physics/body_physics.cpp

@@ -23,81 +23,6 @@ namespace hako::drone_physics {
  *  Maths section (3D frame transformations between body and ground)
  */
 
-/*
- * For generic vectors. all vector types can be available including
- * velocity, acceleration, angular ones, but NOT for angles/angular rates(EULERS).
- */
-VectorType ground_vector_from_body(
-    const VectorType& body,
-    const EulerType& angle)
-{
-    using std::sin; using std::cos;
-    const auto
-        c_phi   = cos(angle.phi),   s_phi = sin(angle.phi),
-        c_theta = cos(angle.theta), s_theta = sin(angle.theta),
-        c_psi   = cos(angle.psi),   s_psi = sin(angle.psi);
-    
-    const auto [x, y, z] = body;
-
-    /*
-     * eq.(1.71),(1.124) in Nonami's book.
-     * See also https://mtkbirdman.com/flight-dynamics-body-axes-system
-     * for the transformation equations.
-     */
-    /*****************************************************************/  
-    double
-    x_e =   (c_theta * c_psi)                           * x
-            + (s_phi * s_theta * c_psi - c_phi * s_psi) * y
-            + (c_phi * s_theta * c_psi + s_phi * s_psi) * z;
-    double
-    y_e =   (c_theta * s_psi)                           * x
-            + (s_phi * s_theta * s_psi + c_phi * c_psi) * y
-            + (c_phi * s_theta * s_psi - s_phi * c_psi) * z;
-    double
-    z_e =   (- s_theta)                                 * x
-            + (s_phi * c_theta)                         * y
-            + (c_phi * c_theta)                         * z;
-    /*****************************************************************/
-
-    return {x_e, y_e, z_e};
-}
-
-/* for generic vectors. use the meaningful aliases below  */
-VectorType body_vector_from_ground(
-    const VectorType& ground,
-    const EulerType& angle)
-{
-    using std::sin; using std::cos;
-    const auto
-        c_phi   = cos(angle.phi),   s_phi = sin(angle.phi),
-        c_theta = cos(angle.theta), s_theta = sin(angle.theta),
-        c_psi   = cos(angle.psi),   s_psi = sin(angle.psi);
-    const auto [x_e, y_e, z_e] = ground;
-    /*
-     * See eq.(1.69), inverse of (1.124) in Nonami's book.
-     * See also https://mtkbirdman.com/flight-dynamics-body-axes-system
-     * for the transformation equations.
-     */
-    /*****************************************************************/  
-    double
-    x =   (c_theta * c_psi)                         * x_e
-        + (c_theta * s_psi)                         * y_e
-        - (s_theta)                                 * z_e;
-
-    double
-    y =   (s_phi * s_theta * c_psi - c_phi * s_psi) * x_e
-        + (s_phi * s_theta * s_psi + c_phi * c_psi) * y_e
-        + (s_phi * c_theta)                         * z_e;
-
-    double
-    z = (c_phi * s_theta * c_psi + s_phi * s_psi)   * x_e
-        + (c_phi * s_theta * s_psi - s_phi * c_psi) * y_e
-        + (c_phi * c_theta)                         * z_e;
-    /*****************************************************************/  
-
-    return {x, y, z};
-}
-
 /* Tranlsform angular rate in body frame to ground frame eq.(1.109)*/
 EulerRateType euler_rate_from_body_angular_velocity(
     const AngularVelocityType& body,