PointMass.hpp

Go to the documentation of this file.

// Vulcan 3-DOF Point Mass Dynamics
// Pure stateless functions for point mass trajectory computation
#pragma once
 
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/janus.hpp>
 
namespace vulcan::dynamics {
 
// =============================================================================
// 3-DOF Point Mass Dynamics (Inertial Frame)
// =============================================================================

template <typename Scalar>
Vec3<Scalar> point_mass_acceleration(const Vec3<Scalar> &force,
                                     const Scalar &mass) {
    return force / mass;
}

template <typename Scalar> Scalar speed(const Vec3<Scalar> &velocity) {
    return janus::norm(velocity);
}

template <typename Scalar>
Vec3<Scalar> velocity_direction(const Vec3<Scalar> &velocity) {
    Scalar v_mag = janus::norm(velocity);
    return velocity / (v_mag + Scalar(1e-12)); // Regularized for zero velocity
}
 
// =============================================================================
// 3-DOF Point Mass Dynamics (ECEF Frame)
// =============================================================================

template <typename Scalar>
Vec3<Scalar> point_mass_acceleration_ecef(const Vec3<Scalar> &position,
                                          const Vec3<Scalar> &velocity,
                                          const Vec3<Scalar> &force,
                                          const Scalar &mass,
                                          const Vec3<Scalar> &omega_earth) {
    // Force-based acceleration
    Vec3<Scalar> a_force = force / mass;
 
    // Coriolis: -2 * ω × v
    Vec3<Scalar> coriolis = Scalar(-2) * janus::cross(omega_earth, velocity);
 
    // Centrifugal: -ω × (ω × r)
    Vec3<Scalar> centrifugal =
        -janus::cross(omega_earth, janus::cross(omega_earth, position));
 
    return a_force + coriolis + centrifugal;
}
 
// =============================================================================
// Specific Force Components
// =============================================================================

template <typename Scalar>
Vec3<Scalar> specific_force(const Vec3<Scalar> &acceleration) {
    return acceleration; // They're the same thing
}

template <typename Scalar>
Vec3<Scalar> g_load(const Vec3<Scalar> &acceleration,
                    const Scalar &g0 = Scalar(9.80665)) {
    return acceleration / g0;
}

template <typename Scalar>
Scalar g_load_magnitude(const Vec3<Scalar> &acceleration,
                        const Scalar &g0 = Scalar(9.80665)) {
    return janus::norm(acceleration) / g0;
}
 
// =============================================================================
// Flight Path Angles
// =============================================================================

template <typename Scalar>
Scalar flight_path_angle(const Vec3<Scalar> &velocity) {
    Scalar v_mag = janus::norm(velocity);
    // Z-down: negative vz is upward, positive gamma
    return janus::asin(-velocity(2) / (v_mag + Scalar(1e-12)));
}

template <typename Scalar> Scalar heading_angle(const Vec3<Scalar> &velocity) {
    return janus::atan2(velocity(1), velocity(0));
}

template <typename Scalar>
Vec3<Scalar> velocity_from_angles(const Scalar &speed, const Scalar &gamma,
                                  const Scalar &chi) {
    Scalar cos_gamma = janus::cos(gamma);
    Scalar sin_gamma = janus::sin(gamma);
    Scalar cos_chi = janus::cos(chi);
    Scalar sin_chi = janus::sin(chi);
 
    return Vec3<Scalar>{
        speed * cos_gamma * cos_chi, // North (vx)
        speed * cos_gamma * sin_chi, // East (vy)
        -speed * sin_gamma           // Down (vz), negative for positive gamma
    };
}
 
// =============================================================================
// Energy & Orbital Quantities
// =============================================================================

template <typename Scalar>
Scalar specific_kinetic_energy(const Vec3<Scalar> &velocity) {
    return Scalar(0.5) * janus::dot(velocity, velocity);
}

template <typename Scalar>
Scalar specific_potential_energy(const Vec3<Scalar> &position,
                                 const Scalar &mu) {
    return -mu / janus::norm(position);
}

template <typename Scalar>
Scalar specific_energy(const Vec3<Scalar> &position,
                       const Vec3<Scalar> &velocity, const Scalar &mu) {
    return specific_kinetic_energy(velocity) +
           specific_potential_energy(position, mu);
}
 
} // namespace vulcan::dynamics