fsb_body.h

 
#ifndef FSB_BODY_H
#define FSB_BODY_H
 
#include <array>
#include <cstddef>
#include "fsb_configuration.h"
#include "fsb_quaternion.h"
#include "fsb_motion.h"
#include "fsb_types.h"
 
namespace fsb
{
 
struct Inertia
{
    real_t ixx;
    real_t iyy;
    real_t izz;
    real_t ixy;
    real_t ixz;
    real_t iyz;
};
 
struct MassProps
{
    real_t mass;
    Vec3 com;
    Inertia inertia;
};
 
struct PrincipalInertia
{
    Quaternion rot;
    Inertia inertia;
};
 
struct BodyCartesianPva
{
    std::array<CartesianPva, MaxSize::bodies> body;
};
 
struct Body
{
    MotionVector origin_offset;
    MassProps mass_props;
    PrincipalInertia principal_inertia;
    size_t joint_index;
    bool is_leaf;
};
 
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_t 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