RigidBody.hpp

Go to the documentation of this file.

// Vulcan Rigid Body Dynamics
// 6DOF equations of motion for trajectory optimization and simulation
#pragma once
 
#include <vulcan/dynamics/RigidBodyTypes.hpp>
#include <vulcan/rotations/RotationKinematics.hpp>
 
#include <janus/math/Linalg.hpp>
#include <janus/math/Quaternion.hpp>
 
namespace vulcan::dynamics {
 
// =============================================================================
// Translational Dynamics
// =============================================================================

template <typename Scalar>
Vec3<Scalar> translational_dynamics(const Vec3<Scalar> &velocity_body,
                                    const Vec3<Scalar> &omega_body,
                                    const Vec3<Scalar> &force_body,
                                    const Scalar &mass) {
    // v_dot = F/m - ω × v
    Vec3<Scalar> transport = janus::cross(omega_body, velocity_body);
    return force_body / mass - transport;
}
 
// =============================================================================
// Rotational Dynamics (Euler's Equations)
// =============================================================================

template <typename Scalar>
Vec3<Scalar> rotational_dynamics(const Vec3<Scalar> &omega_body,
                                 const Vec3<Scalar> &moment_body,
                                 const Mat3<Scalar> &inertia) {
    // I * ω_dot = M - ω × (I * ω)
    Vec3<Scalar> H = inertia * omega_body; // Angular momentum
    Vec3<Scalar> gyroscopic = janus::cross(omega_body, H);
    Vec3<Scalar> rhs = moment_body - gyroscopic;
 
    // Solve I * ω_dot = rhs using janus::solve for symbolic compatibility
    return janus::solve(inertia, rhs);
}
 
// =============================================================================
// Frame Transformations
// =============================================================================

template <typename Scalar>
Vec3<Scalar>
velocity_to_reference_frame(const Vec3<Scalar> &velocity_body,
                            const janus::Quaternion<Scalar> &attitude) {
    return attitude.rotate(velocity_body);
}

template <typename Scalar>
Vec3<Scalar> velocity_to_body_frame(const Vec3<Scalar> &velocity_ref,
                                    const janus::Quaternion<Scalar> &attitude) {
    return attitude.conjugate().rotate(velocity_ref);
}
 
// =============================================================================
// Full 6DOF Derivatives
// =============================================================================

template <typename Scalar>
RigidBodyDerivatives<Scalar> compute_6dof_derivatives(
    const RigidBodyState<Scalar> &state, const Vec3<Scalar> &force_body,
    const Vec3<Scalar> &moment_body, const MassProperties<Scalar> &mass_props) {
    RigidBodyDerivatives<Scalar> derivs;
 
    // Position rate = velocity in reference frame
    derivs.position_dot =
        velocity_to_reference_frame(state.velocity_body, state.attitude);
 
    // Velocity rate = translational dynamics
    derivs.velocity_dot = translational_dynamics(
        state.velocity_body, state.omega_body, force_body, mass_props.mass);
 
    // Attitude rate = quaternion kinematics
    derivs.attitude_dot =
        quaternion_rate_from_omega(state.attitude, state.omega_body);
 
    // Angular velocity rate = rotational dynamics
    derivs.omega_dot =
        rotational_dynamics(state.omega_body, moment_body, mass_props.inertia);
 
    return derivs;
}
 
// =============================================================================
// ECEF Dynamics with Earth Rotation Effects
// =============================================================================

template <typename Scalar>
Vec3<Scalar> translational_dynamics_ecef(const Vec3<Scalar> &position,
                                         const Vec3<Scalar> &velocity_ecef,
                                         const Vec3<Scalar> &force_ecef,
                                         const Scalar &mass,
                                         const Vec3<Scalar> &omega_earth) {
    // Coriolis: 2 * (ω_earth × v)
    Vec3<Scalar> coriolis =
        Scalar(2) * janus::cross(omega_earth, velocity_ecef);
 
    // Centrifugal: ω × (ω × r)
    Vec3<Scalar> centrifugal =
        janus::cross(omega_earth, janus::cross(omega_earth, position));
 
    // Total acceleration: F/m - Coriolis - Centrifugal
    return force_ecef / mass - coriolis - centrifugal;
}
 
} // namespace vulcan::dynamics