fsb_motion.h

 
#ifndef FSB_MOTION_H
#define FSB_MOTION_H
 
#include "fsb_quaternion.h"
#include "fsb_types.h"
 
namespace fsb
{
 
struct Transform
{
    Quaternion rotation;
    Vec3 translation;
};
 
struct MotionVector
{
    Vec3 angular;
    Vec3 linear;
};
 
struct CartesianPva
{
    Transform pose;
    MotionVector velocity;
    MotionVector acceleration;
};
 
Vec3 vector_add(const Vec3& v_a, const Vec3& v_b);
 
Vec3 vector_subtract(const Vec3& v_a, const Vec3& v_b);
 
Vec3 vector_scale(real_t scalar, const Vec3& vec);
 
Vec3 vector_multiply_elem(const Vec3& v_a, const Vec3& v_b);
 
Vec3 vector_cross(const Vec3& v_a, const Vec3& v_b);
 
real_t vector_dot(const Vec3& v_a, const Vec3& v_b);
 
real_t vector_norm(const Vec3& vec);
 
Transform transform_identity();
 
Transform transform_inverse(const Transform& transf);
 
Transform coord_transform(const Transform& transf_a, const Transform& transf_b);
 
Transform coord_transform_inverse(const Transform& transf_a, const Transform& transf_b);
 
Vec3 coord_transform_position(const Transform& transf, const Vec3& pos);
 
Transform coord_transform_apply_error(const Transform& transf_initial, const MotionVector& offset);
 
MotionVector
coord_transform_get_error(const Transform& transf_initial, const Transform& transf_final);
 
Transform
coord_transform_apply_body_offset(const Transform& transf_initial, const MotionVector& offset);
 
MotionVector
coord_transform_get_body_offset(const Transform& transf_initial, const Transform& transf_final);
 
MotionVector motion_transform_velocity(
    const Transform& parent_transf, const MotionVector& parent_velocity,
    const Transform& motion_transf, const MotionVector& motion_velocity);
 
MotionVector motion_transform_acceleration(
    const Transform& parent_transf, const MotionVector& parent_velocity,
    const MotionVector& parent_acceleration, const Transform& motion_transf,
    const MotionVector& motion_velocity, const MotionVector& motion_acceleration);
 
MotionVector
motion_transform_space_to_body(const Transform& pose, const MotionVector& space_motion);
 
MotionVector
motion_transform_body_to_space(const Transform& pose, const MotionVector& body_motion);
 
MotionVector motion_transform_velocity_position(
    const Transform& parent_transf, const MotionVector& parent_velocity,
    const Vec3& child_position);
 
MotionVector motion_transform_acceleration_position(
    const Transform& parent_transf, const MotionVector& parent_velocity,
    const MotionVector& parent_acceleration, const Vec3& child_position);
 
} // namespace fsb
 
#endif