fsb_body.h

 
#ifndef FSB_BODY_H
#define FSB_BODY_H
 
#include <array>
#include "fsb_configuration.h"
#include "fsb_quaternion.h"
#include "fsb_motion.h"
#include "fsb_types.h"
 
namespace fsb
{
 
struct Inertia
{
    Real ixx = 0.0;
    Real iyy = 0.0;
    Real izz = 0.0;
    Real ixy = 0.0;
    Real ixz = 0.0;
    Real iyz = 0.0;
};
 
struct MassProps
{
    Real mass = 0.0;
    Vec3 com = {};
    Inertia inertia = {};
};
 
struct PrincipalInertia
{
    Quaternion rot = {};
    Inertia inertia = {};
};
 
struct BodyCartesianPva
{
    std::array<CartesianPva, MaxSize::kBodies> body = {};
};
 
struct Body
{
    MotionVector origin_offset = {};
    MassProps mass_props = {};
    PrincipalInertia principal_inertia = {};
    size_t joint_index = 0;
    bool is_leaf = true;
};
 
Inertia body_rotate_inertia(const Quaternion& rot, const Inertia& inertia);
 
MassProps body_transform_mass_props(const Transform& transf, const MassProps& mass_props);
 
Inertia body_parallel_axis_inertia(Real mass, const Vec3& com, const Inertia& inertia);
 
MassProps body_combine(
    const MassProps& mass_props_a, const Transform& transf_ab, const MassProps& mass_props_b);
 
bool body_inertia_principal_axis(const Inertia& inertia, PrincipalInertia& principal_inertia);
 
bool body_validate_inertia_is_pd(const Inertia& inertia);
 
Vec3 inertia_multiply_vector(const Inertia& inertia, const Vec3& vec);
 
Vec3 inertia_cross_multiply_vector(const Inertia& inertia, const Vec3& vel);
 
} // namespace fsb
 
#endif