112 lines
3.2 KiB
C
112 lines
3.2 KiB
C
#ifndef LINUXCNC_WEB_WASM_SIM_POSEMATH_H
|
|
#define LINUXCNC_WEB_WASM_SIM_POSEMATH_H
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
#include <stdint.h>
|
|
|
|
enum sim_posemath_result {
|
|
SIM_POSEMATH_OK = 0,
|
|
SIM_POSEMATH_ERR_ARGUMENT = -400,
|
|
SIM_POSEMATH_ERR_NONFINITE = -401,
|
|
SIM_POSEMATH_ERR_DEGENERATE = -402,
|
|
SIM_POSEMATH_ERR_RANGE = -403,
|
|
SIM_POSEMATH_ERR_MOTION_TYPE = -404,
|
|
SIM_POSEMATH_ERR_STATE = -405,
|
|
SIM_POSEMATH_ERR_CAPACITY = -406,
|
|
};
|
|
|
|
/* Values match LinuxCNC's tc_motion_type_t. */
|
|
enum sim_posemath_motion_type {
|
|
SIM_POSEMATH_LINEAR = 1,
|
|
SIM_POSEMATH_CIRCULAR = 2,
|
|
SIM_POSEMATH_RIGID_TAP = 3,
|
|
SIM_POSEMATH_SPHERICAL = 4,
|
|
};
|
|
|
|
typedef struct sim_posemath_cartesian {
|
|
double x;
|
|
double y;
|
|
double z;
|
|
} sim_posemath_cartesian;
|
|
|
|
typedef struct sim_posemath_pose {
|
|
sim_posemath_cartesian tran;
|
|
double a;
|
|
double b;
|
|
double c;
|
|
double u;
|
|
double v;
|
|
double w;
|
|
} sim_posemath_pose;
|
|
|
|
typedef struct sim_posemath_segment {
|
|
sim_posemath_pose start;
|
|
sim_posemath_pose end;
|
|
sim_posemath_cartesian center;
|
|
sim_posemath_cartesian normal;
|
|
double velocity;
|
|
double acceleration;
|
|
int motion_type;
|
|
int turn;
|
|
} sim_posemath_segment;
|
|
|
|
typedef struct sim_tp_state {
|
|
sim_posemath_pose position;
|
|
double velocity;
|
|
double progress;
|
|
uint32_t tick;
|
|
uint32_t queue_depth;
|
|
uint32_t active_id;
|
|
int motion_type;
|
|
int status;
|
|
} sim_tp_state;
|
|
|
|
#define SIM_TP_QUEUE_CAPACITY 32u
|
|
|
|
typedef struct sim_tp {
|
|
sim_posemath_segment queue[SIM_TP_QUEUE_CAPACITY];
|
|
uint32_t ids[SIM_TP_QUEUE_CAPACITY];
|
|
uint32_t head;
|
|
uint32_t count;
|
|
uint32_t next_id;
|
|
uint32_t active_id;
|
|
uint32_t tick;
|
|
double cycle_time;
|
|
double velocity_limit;
|
|
double acceleration_limit;
|
|
double progress;
|
|
double current_velocity;
|
|
sim_posemath_pose position;
|
|
int motion_type;
|
|
int status;
|
|
} sim_tp;
|
|
|
|
int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output);
|
|
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output);
|
|
int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
|
|
double fraction, sim_posemath_cartesian *output);
|
|
int sim_posemath_is_finite(const sim_posemath_cartesian *value);
|
|
int sim_posemath_pose_is_finite(const sim_posemath_pose *value);
|
|
int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
|
|
double fraction, sim_posemath_pose *output);
|
|
int sim_posemath_segment_validate(const sim_posemath_segment *segment);
|
|
int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
|
|
sim_posemath_pose *output);
|
|
uint64_t sim_posemath_hash_state(const sim_tp_state *state);
|
|
|
|
int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit);
|
|
int sim_tp_reset(sim_tp *tp);
|
|
int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out);
|
|
int sim_tp_step(sim_tp *tp);
|
|
int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state);
|
|
uint64_t sim_tp_state_hash(const sim_tp *tp);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif
|