684 lines
45 KiB
C
684 lines
45 KiB
C
#include "sim_interpreter.h"
|
|
|
|
#include <ctype.h>
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
/* Keep the public 648-byte object ABI stable; mutable parameter/diagnostic
|
|
* state is held in a bounded side table owned by the single Worker writer. */
|
|
#define META_SLOTS 32u
|
|
#define NAMED_PARAMETER_CAPACITY 16u
|
|
#define NAMED_PARAMETER_LENGTH 32u
|
|
#define PENDING_ASSIGNMENTS 32u
|
|
|
|
typedef struct named_parameter {
|
|
char name[NAMED_PARAMETER_LENGTH];
|
|
double value;
|
|
int used;
|
|
} named_parameter;
|
|
|
|
typedef struct interpreter_meta {
|
|
const sim_interpreter *owner;
|
|
double parameters[SIM_INTERPRETER_PARAMETER_MAX + 1u];
|
|
named_parameter named[NAMED_PARAMETER_CAPACITY];
|
|
double coordinate_offsets[9][9];
|
|
int coordinate_system;
|
|
int cutter_comp;
|
|
int cycle_mode;
|
|
int feed_mode;
|
|
int coordinate_l20;
|
|
int coolant_mode;
|
|
int selected_tool;
|
|
double tool_length;
|
|
sim_posemath_segment last_motion;
|
|
int has_motion;
|
|
sim_posemath_pose last_output_pose;
|
|
int has_output_pose;
|
|
uint32_t error_column;
|
|
int32_t error_code;
|
|
} interpreter_meta;
|
|
|
|
static interpreter_meta metadata[META_SLOTS];
|
|
|
|
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
|
|
uint64_t hash = UINT64_C(1469598103934665603);
|
|
size_t index;
|
|
for (index = 0; index < length; ++index) {
|
|
hash ^= (uint64_t)bytes[index];
|
|
hash *= UINT64_C(1099511628211);
|
|
}
|
|
return hash;
|
|
}
|
|
|
|
static interpreter_meta *meta_for(const sim_interpreter *interpreter, int create) {
|
|
interpreter_meta *free_slot = NULL;
|
|
size_t index;
|
|
if (interpreter == NULL) return NULL;
|
|
for (index = 0; index < META_SLOTS; ++index) {
|
|
if (metadata[index].owner == interpreter) return &metadata[index];
|
|
if (free_slot == NULL && metadata[index].owner == NULL) free_slot = &metadata[index];
|
|
}
|
|
if (!create || free_slot == NULL) return NULL;
|
|
memset(free_slot, 0, sizeof(*free_slot));
|
|
free_slot->owner = interpreter;
|
|
return free_slot;
|
|
}
|
|
|
|
static void meta_reset(interpreter_meta *meta, const sim_interpreter *owner) {
|
|
memset(meta, 0, sizeof(*meta));
|
|
meta->owner = owner;
|
|
}
|
|
|
|
static int finite_state(const sim_interpreter_state *state) {
|
|
const double *values;
|
|
size_t index;
|
|
if (state == NULL) return 0;
|
|
values = &state->x;
|
|
for (index = 0; index < 11; ++index) if (!isfinite(values[index])) return 0;
|
|
return 1;
|
|
}
|
|
|
|
static int code_is(double value, int integer_code) {
|
|
return isfinite(value) && fabs(value - (double)integer_code) <= 1e-12;
|
|
}
|
|
|
|
typedef struct { uint32_t magic, size; sim_interpreter interpreter; interpreter_meta meta; } interpreter_checkpoint;
|
|
uint32_t sim_interpreter_checkpoint_size(void) { return sizeof(interpreter_checkpoint); }
|
|
int32_t sim_interpreter_checkpoint_export(const sim_interpreter *interpreter, uint8_t *output, uint32_t capacity) {
|
|
const interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta == NULL || output == NULL || capacity != sizeof(interpreter_checkpoint)) return SIM_INTERP_ERR_ARGUMENT;
|
|
interpreter_checkpoint *saved = (interpreter_checkpoint *)output;
|
|
memset(saved, 0, sizeof(*saved));
|
|
saved->magic = UINT32_C(0x49504331); saved->size = sizeof(*saved);
|
|
saved->interpreter = *interpreter; saved->meta = *meta; saved->meta.owner = NULL;
|
|
return SIM_INTERP_OK;
|
|
}
|
|
int32_t sim_interpreter_checkpoint_restore(sim_interpreter *interpreter, const uint8_t *input, uint32_t length) {
|
|
if (interpreter == NULL || input == NULL || length != sizeof(interpreter_checkpoint)) return SIM_INTERP_ERR_ARGUMENT;
|
|
const interpreter_checkpoint *saved = (const interpreter_checkpoint *)input;
|
|
if (saved->magic != UINT32_C(0x49504331) || saved->size != sizeof(*saved) || saved->meta.owner != NULL || !finite_state(&saved->interpreter.state) || saved->interpreter.canonical_length >= SIM_INTERPRETER_CANONICAL_CAPACITY || saved->meta.coordinate_system < 0 || saved->meta.coordinate_system > 9) return SIM_INTERP_ERR_ARGUMENT;
|
|
for (unsigned i = 0; i < NAMED_PARAMETER_CAPACITY; ++i) if (saved->meta.named[i].used && memchr(saved->meta.named[i].name, '\0', NAMED_PARAMETER_LENGTH) == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
interpreter_meta *meta = meta_for(interpreter, 1);
|
|
if (meta == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
*interpreter = saved->interpreter; *meta = saved->meta; meta->owner = interpreter;
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
static int code_is_value(double value, double expected) {
|
|
return isfinite(value) && fabs(value - expected) <= 1e-12;
|
|
}
|
|
|
|
static int axis_index(char letter) {
|
|
switch (letter) {
|
|
case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
|
|
case 'A': return 3; case 'B': return 4; case 'C': return 5;
|
|
case 'U': return 6; case 'V': return 7; case 'W': return 8;
|
|
default: return -1;
|
|
}
|
|
}
|
|
|
|
static double *axis_pointer(sim_interpreter_state *state, int index) { return &state->x + index; }
|
|
|
|
static void machine_pose_for_state(const interpreter_meta *meta, const sim_interpreter_state *state, int machine_coordinates, sim_posemath_pose *pose) {
|
|
// The interpreter state already stores machine coordinates. Offsets are
|
|
// applied exactly once when accepting an absolute coordinate word.
|
|
(void)meta; (void)machine_coordinates;
|
|
pose->tran.x = state->x; pose->tran.y = state->y; pose->tran.z = state->z;
|
|
pose->a = state->a; pose->b = state->b; pose->c = state->c;
|
|
pose->u = state->u; pose->v = state->v; pose->w = state->w;
|
|
}
|
|
|
|
static void set_error(sim_interpreter *interpreter, interpreter_meta *meta, int code, uint32_t column) {
|
|
if (interpreter != NULL) interpreter->state.status = code;
|
|
if (meta != NULL) { meta->error_code = code; meta->error_column = column; }
|
|
}
|
|
|
|
static int parse_decimal(const char **cursor, const char *end, double *output) {
|
|
const char *start = *cursor;
|
|
int sign = 1, digits = 0, exponent_sign = 1, exponent = 0;
|
|
long double value = 0.0L, scale = 1.0L;
|
|
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
|
|
if (**cursor == '-') sign = -1;
|
|
++*cursor;
|
|
}
|
|
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
|
value = value * 10.0L + (long double)(**cursor - '0'); ++*cursor; digits = 1;
|
|
}
|
|
if (*cursor < end && **cursor == '.') {
|
|
++*cursor;
|
|
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
|
value = value * 10.0L + (long double)(**cursor - '0'); scale *= 10.0L; ++*cursor; digits = 1;
|
|
}
|
|
}
|
|
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
|
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
|
|
++*cursor;
|
|
if (*cursor < end && (**cursor == '+' || **cursor == '-')) { if (**cursor == '-') exponent_sign = -1; ++*cursor; }
|
|
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
|
while (*cursor < end && isdigit((unsigned char)**cursor)) { if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0'); ++*cursor; }
|
|
}
|
|
value = (long double)sign * value / scale;
|
|
if (exponent != 0) value *= powl(10.0L, (long double)(exponent_sign * exponent));
|
|
*output = (double)value;
|
|
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
|
|
typedef struct expression_parser {
|
|
interpreter_meta *meta;
|
|
const char *cursor;
|
|
const char *end;
|
|
int pending_index[PENDING_ASSIGNMENTS];
|
|
double pending_value[PENDING_ASSIGNMENTS];
|
|
size_t pending_count;
|
|
} expression_parser;
|
|
|
|
static void skip_space(expression_parser *parser) {
|
|
while (parser->cursor < parser->end && isspace((unsigned char)*parser->cursor)) ++parser->cursor;
|
|
}
|
|
|
|
static int identifier(expression_parser *parser, char *buffer, size_t capacity) {
|
|
size_t length = 0;
|
|
skip_space(parser);
|
|
while (parser->cursor < parser->end && isalpha((unsigned char)*parser->cursor)) {
|
|
if (length + 1u < capacity) buffer[length++] = (char)toupper((unsigned char)*parser->cursor);
|
|
++parser->cursor;
|
|
}
|
|
buffer[length] = '\0';
|
|
return length > 0 ? SIM_INTERP_OK : SIM_INTERP_ERR_SYNTAX;
|
|
}
|
|
|
|
static uint32_t named_hash(const char *name) { return (uint32_t)(fnv1a64((const unsigned char *)name, strlen(name)) & UINT32_C(0x7fffffff)); }
|
|
|
|
static int parameter_lookup(const expression_parser *parser, uint32_t index, double *value) {
|
|
size_t item;
|
|
if (index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX || value == NULL) return SIM_INTERP_ERR_PARAMETER;
|
|
for (item = parser->pending_count; item > 0; --item) {
|
|
if ((uint32_t)parser->pending_index[item - 1u] == index) { *value = parser->pending_value[item - 1u]; return SIM_INTERP_OK; }
|
|
}
|
|
*value = parser->meta->parameters[index];
|
|
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
|
|
static int named_lookup(expression_parser *parser, const char *name, double *value, int create) {
|
|
size_t index;
|
|
if (name == NULL || name[0] == '\0' || value == NULL) return SIM_INTERP_ERR_PARAMETER;
|
|
if (name[0] == '_' && name[2] == '\0') {
|
|
int axis = axis_index((char)toupper((unsigned char)name[1]));
|
|
if (axis >= 0) {
|
|
*value = *axis_pointer((sim_interpreter_state *)&parser->meta->owner->state, axis);
|
|
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
}
|
|
for (index = 0; index < NAMED_PARAMETER_CAPACITY; ++index) {
|
|
if (parser->meta->named[index].used && strcmp(parser->meta->named[index].name, name) == 0) { *value = parser->meta->named[index].value; return SIM_INTERP_OK; }
|
|
}
|
|
if (!create) { *value = 0.0; return SIM_INTERP_OK; }
|
|
for (index = 0; index < NAMED_PARAMETER_CAPACITY; ++index) if (!parser->meta->named[index].used) {
|
|
strncpy(parser->meta->named[index].name, name, NAMED_PARAMETER_LENGTH - 1u);
|
|
parser->meta->named[index].name[NAMED_PARAMETER_LENGTH - 1u] = '\0';
|
|
parser->meta->named[index].used = 1; parser->meta->named[index].value = 0.0; *value = 0.0; return SIM_INTERP_OK;
|
|
}
|
|
return SIM_INTERP_ERR_PARAMETER;
|
|
}
|
|
|
|
static int parse_expression(expression_parser *parser, double *value);
|
|
|
|
static int parse_parameter_reference(expression_parser *parser, double *value, int *named_slot) {
|
|
unsigned long index = 0;
|
|
char name[NAMED_PARAMETER_LENGTH] = {0};
|
|
size_t length = 0;
|
|
int result;
|
|
skip_space(parser);
|
|
if (parser->cursor >= parser->end || *parser->cursor != '#') return SIM_INTERP_ERR_SYNTAX;
|
|
++parser->cursor;
|
|
if (parser->cursor < parser->end && *parser->cursor == '<') {
|
|
++parser->cursor;
|
|
while (parser->cursor < parser->end && *parser->cursor != '>') {
|
|
if (length + 1u < sizeof(name)) name[length++] = (char)tolower((unsigned char)*parser->cursor);
|
|
++parser->cursor;
|
|
}
|
|
if (parser->cursor >= parser->end || *parser->cursor != '>' || length == 0) return SIM_INTERP_ERR_PARAMETER;
|
|
++parser->cursor; name[length] = '\0';
|
|
result = named_lookup(parser, name, value, 1);
|
|
if (named_slot != NULL) *named_slot = (int)named_hash(name);
|
|
return result;
|
|
}
|
|
if (parser->cursor < parser->end && *parser->cursor == '[') {
|
|
double expression_value;
|
|
++parser->cursor; result = parse_expression(parser, &expression_value); skip_space(parser);
|
|
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_PARAMETER;
|
|
++parser->cursor;
|
|
if (!isfinite(expression_value) || expression_value < 1.0 || expression_value > (double)SIM_INTERPRETER_PARAMETER_MAX || floor(expression_value) != expression_value) return SIM_INTERP_ERR_PARAMETER;
|
|
index = (unsigned long)expression_value;
|
|
} else {
|
|
const char *start = parser->cursor;
|
|
while (parser->cursor < parser->end && isdigit((unsigned char)*parser->cursor)) { if (index <= 1000000UL) index = index * 10UL + (unsigned long)(*parser->cursor - '0'); ++parser->cursor; }
|
|
if (parser->cursor == start || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER;
|
|
}
|
|
if (named_slot != NULL) *named_slot = 0;
|
|
return parameter_lookup(parser, (uint32_t)index, value);
|
|
}
|
|
|
|
static int parse_primary(expression_parser *parser, double *value) {
|
|
char function_name[16] = {0};
|
|
const char *start;
|
|
int result;
|
|
skip_space(parser);
|
|
if (parser->cursor >= parser->end) return SIM_INTERP_ERR_EXPRESSION;
|
|
if (*parser->cursor == '#') return parse_parameter_reference(parser, value, NULL);
|
|
if (*parser->cursor == '[') {
|
|
++parser->cursor; result = parse_expression(parser, value); skip_space(parser);
|
|
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_EXPRESSION;
|
|
++parser->cursor; return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
if (isalpha((unsigned char)*parser->cursor)) {
|
|
result = identifier(parser, function_name, sizeof(function_name)); if (result != SIM_INTERP_OK) return result;
|
|
skip_space(parser);
|
|
if (strcmp(function_name, "PI") == 0) { *value = 3.14159265358979323846; return SIM_INTERP_OK; }
|
|
if (strcmp(function_name, "E") == 0) { *value = 2.71828182845904523536; return SIM_INTERP_OK; }
|
|
if (strcmp(function_name, "NAN") == 0 || strcmp(function_name, "INF") == 0 || strcmp(function_name, "INFINITY") == 0) return SIM_INTERP_ERR_SYNTAX;
|
|
if (strcmp(function_name, "MOD") == 0) return SIM_INTERP_ERR_EXPRESSION;
|
|
if (parser->cursor < parser->end && *parser->cursor == '[') {
|
|
++parser->cursor; result = parse_expression(parser, value); skip_space(parser);
|
|
if (result != SIM_INTERP_OK || parser->cursor >= parser->end || *parser->cursor != ']') return SIM_INTERP_ERR_EXPRESSION;
|
|
++parser->cursor;
|
|
} else { result = parse_primary(parser, value); if (result != SIM_INTERP_OK) return result; }
|
|
if (!isfinite(*value)) return SIM_INTERP_ERR_NONFINITE;
|
|
if (strcmp(function_name, "ABS") == 0) *value = fabs(*value);
|
|
else if (strcmp(function_name, "SQRT") == 0) { if (*value < 0.0) return SIM_INTERP_ERR_RANGE; *value = sqrt(*value); }
|
|
else if (strcmp(function_name, "SIN") == 0) *value = sin(*value * 3.14159265358979323846 / 180.0);
|
|
else if (strcmp(function_name, "COS") == 0) *value = cos(*value * 3.14159265358979323846 / 180.0);
|
|
else if (strcmp(function_name, "TAN") == 0) *value = tan(*value * 3.14159265358979323846 / 180.0);
|
|
else if (strcmp(function_name, "ASIN") == 0) { if (fabs(*value) > 1.0) return SIM_INTERP_ERR_RANGE; *value = asin(*value) * 180.0 / 3.14159265358979323846; }
|
|
else if (strcmp(function_name, "ACOS") == 0) { if (fabs(*value) > 1.0) return SIM_INTERP_ERR_RANGE; *value = acos(*value) * 180.0 / 3.14159265358979323846; }
|
|
else if (strcmp(function_name, "ATAN") == 0) *value = atan(*value) * 180.0 / 3.14159265358979323846;
|
|
else if (strcmp(function_name, "FIX") == 0) *value = floor(*value);
|
|
else if (strcmp(function_name, "FUP") == 0) *value = ceil(*value);
|
|
else if (strcmp(function_name, "ROUND") == 0) *value = nearbyint(*value);
|
|
else if (strcmp(function_name, "EXP") == 0) *value = exp(*value);
|
|
else if (strcmp(function_name, "LN") == 0) { if (*value <= 0.0) return SIM_INTERP_ERR_RANGE; *value = log(*value); }
|
|
else return SIM_INTERP_ERR_UNKNOWN_CODE;
|
|
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
start = parser->cursor; result = parse_decimal(&parser->cursor, parser->end, value); if (result != SIM_INTERP_OK) parser->cursor = start;
|
|
return result;
|
|
}
|
|
|
|
static int parse_unary(expression_parser *parser, double *value) {
|
|
skip_space(parser);
|
|
if (parser->cursor < parser->end && (*parser->cursor == '+' || *parser->cursor == '-')) {
|
|
char sign = *parser->cursor++; int result = parse_unary(parser, value);
|
|
if (result == SIM_INTERP_OK && sign == '-') *value = -*value; return result;
|
|
}
|
|
return parse_primary(parser, value);
|
|
}
|
|
|
|
static int parse_power(expression_parser *parser, double *value) {
|
|
int result = parse_unary(parser, value); skip_space(parser); if (result != SIM_INTERP_OK) return result;
|
|
if (parser->cursor + 1 < parser->end && parser->cursor[0] == '*' && parser->cursor[1] == '*') {
|
|
double right; parser->cursor += 2; result = parse_power(parser, &right); if (result != SIM_INTERP_OK) return result; *value = pow(*value, right);
|
|
}
|
|
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
|
|
static int parse_product(expression_parser *parser, double *value) {
|
|
int result = parse_power(parser, value); if (result != SIM_INTERP_OK) return result;
|
|
for (;;) {
|
|
double right; char operator_name[8] = {0}; const char *save;
|
|
skip_space(parser);
|
|
if (parser->cursor >= parser->end || (*parser->cursor != '*' && *parser->cursor != '/')) {
|
|
save = parser->cursor;
|
|
if (parser->cursor < parser->end && isalpha((unsigned char)*parser->cursor)) { identifier(parser, operator_name, sizeof(operator_name)); if (strcmp(operator_name, "MOD") != 0) parser->cursor = save; }
|
|
if (strcmp(operator_name, "MOD") != 0) break;
|
|
}
|
|
if (operator_name[0] != '\0') result = parse_power(parser, &right);
|
|
else {
|
|
char operator_character = *parser->cursor++; result = parse_power(parser, &right);
|
|
if (result == SIM_INTERP_OK) { if (operator_character == '/' && right == 0.0) result = SIM_INTERP_ERR_RANGE; else if (operator_character == '*') *value *= right; else *value /= right; }
|
|
}
|
|
if (strcmp(operator_name, "MOD") == 0 && result == SIM_INTERP_OK) { if (right == 0.0) result = SIM_INTERP_ERR_RANGE; else *value = fmod(*value, right); }
|
|
if (result != SIM_INTERP_OK) return result;
|
|
}
|
|
return isfinite(*value) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
|
|
static int parse_expression(expression_parser *parser, double *value) {
|
|
int result = parse_product(parser, value); if (result != SIM_INTERP_OK) return result;
|
|
for (;;) {
|
|
char operator_character; double right; skip_space(parser);
|
|
if (parser->cursor >= parser->end || (*parser->cursor != '+' && *parser->cursor != '-')) break;
|
|
operator_character = *parser->cursor++; result = parse_product(parser, &right); if (result != SIM_INTERP_OK) return result;
|
|
if (operator_character == '+') *value += right; else *value -= right; if (!isfinite(*value)) return SIM_INTERP_ERR_NONFINITE;
|
|
}
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
static int apply_g(sim_interpreter_state *state, interpreter_meta *meta, double code, int *groups) {
|
|
if (code_is(code, 0)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 0; return SIM_INTERP_OK; }
|
|
if (code_is(code, 1)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 1; return SIM_INTERP_OK; }
|
|
if (code_is(code, 2) || code_is(code, 3)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 17) || code_is(code, 18) || code_is(code, 19)) { if (++groups[1] > 1) return SIM_INTERP_ERR_STATE; state->plane = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 20) || code_is(code, 21)) { if (++groups[2] > 1) return SIM_INTERP_ERR_STATE; state->units = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 90) || code_is(code, 91)) { if (++groups[3] > 1) return SIM_INTERP_ERR_STATE; state->distance_mode = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 93) || code_is(code, 94)) { if (++groups[6] > 1) return SIM_INTERP_ERR_STATE; if (meta) meta->feed_mode = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 40) || code_is(code, 41) || code_is(code, 42)) { if (++groups[4] > 1) return SIM_INTERP_ERR_STATE; if (meta) meta->cutter_comp = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 43) || code_is(code, 49)) { if (++groups[5] > 1) return SIM_INTERP_ERR_STATE; if (meta && code_is(code, 49)) meta->tool_length = 0.0; return SIM_INTERP_OK; }
|
|
if (code_is(code, 80) || (code >= 81.0 && code <= 89.0 && floor(code) == code)) { if (meta) meta->cycle_mode = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 61) || code_is_value(code, 61.1) || code_is(code, 64)) { if (++groups[7] > 1) return SIM_INTERP_ERR_STATE; return SIM_INTERP_OK; }
|
|
if (code_is(code, 54) || code_is(code, 55) || code_is(code, 56) || code_is(code, 57) || code_is(code, 58) || code_is(code, 59)) { if (meta) meta->coordinate_system = (int)code - 53; return SIM_INTERP_OK; }
|
|
if (code_is_value(code, 59.1) || code_is_value(code, 59.2) || code_is_value(code, 59.3)) { if (meta) meta->coordinate_system = 6 + (int)lround((code - 59.0) * 10.0); return SIM_INTERP_OK; }
|
|
if (code_is(code, 53)) return SIM_INTERP_OK;
|
|
if (code_is(code, 10)) return SIM_INTERP_OK;
|
|
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
|
}
|
|
|
|
static int apply_m(sim_interpreter_state *state, interpreter_meta *meta, double code, int *groups) {
|
|
if (code_is(code, 1)) return SIM_INTERP_OK;
|
|
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) { if (++groups[4] > 1) return SIM_INTERP_ERR_STATE; state->spindle_mode = (int)code; return SIM_INTERP_OK; }
|
|
if (code_is(code, 2) || code_is(code, 30)) { if (++groups[5] > 1) return SIM_INTERP_ERR_STATE; state->program_end = 1; return SIM_INTERP_OK; }
|
|
if (code_is(code, 6)) { if (meta) meta->selected_tool = meta->selected_tool < 0 ? 0 : meta->selected_tool; return SIM_INTERP_OK; }
|
|
if (code_is(code, 7)) { if (meta) meta->coolant_mode = 7; return SIM_INTERP_OK; }
|
|
if (code_is(code, 8)) { if (meta) meta->coolant_mode = 8; return SIM_INTERP_OK; }
|
|
if (code_is(code, 9)) { if (meta) meta->coolant_mode = 0; return SIM_INTERP_OK; }
|
|
if (code_is(code, 19)) return SIM_INTERP_OK;
|
|
if (code_is(code, 428) || code_is(code, 429) || code_is(code, 430)) return SIM_INTERP_OK;
|
|
if (code_is(code, 48) || code_is(code, 49) || code_is(code, 50) || code_is(code, 51) || code_is(code, 52) || code_is(code, 53)) return SIM_INTERP_OK;
|
|
if (code >= 62.0 && code <= 68.0 && floor(code) == code) return SIM_INTERP_OK;
|
|
if (code_is(code, 70) || code_is(code, 71) || code_is(code, 72) || code_is(code, 73)) return SIM_INTERP_OK;
|
|
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
|
}
|
|
|
|
static uint64_t state_hash(const sim_interpreter *interpreter) {
|
|
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7 + sizeof(uint64_t)];
|
|
size_t offset = 0, index; uint64_t parameter_hash = 0;
|
|
const double *values = &interpreter->state.x; interpreter_meta *meta = meta_for(interpreter, 0);
|
|
for (index = 0; index < 11; ++index) { memcpy(bytes + offset, values + index, sizeof(double)); offset += sizeof(double); }
|
|
memcpy(bytes + offset, &interpreter->state.line, sizeof(uint32_t)); offset += sizeof(uint32_t);
|
|
memcpy(bytes + offset, &interpreter->state.units, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.distance_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.plane, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.motion_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.spindle_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.program_end, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.status, sizeof(int)); offset += sizeof(int);
|
|
if (meta != NULL) {
|
|
parameter_hash = fnv1a64((const unsigned char *)meta->parameters, sizeof(meta->parameters));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)meta->coordinate_offsets, sizeof(meta->coordinate_offsets));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->coordinate_system, sizeof(meta->coordinate_system));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->cutter_comp, sizeof(meta->cutter_comp));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->cycle_mode, sizeof(meta->cycle_mode));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->feed_mode, sizeof(meta->feed_mode));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->coolant_mode, sizeof(meta->coolant_mode));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->selected_tool, sizeof(meta->selected_tool));
|
|
parameter_hash ^= fnv1a64((const unsigned char *)&meta->tool_length, sizeof(meta->tool_length));
|
|
}
|
|
memcpy(bytes + offset, ¶meter_hash, sizeof(parameter_hash)); offset += sizeof(parameter_hash);
|
|
return fnv1a64(bytes, offset);
|
|
}
|
|
|
|
static uint64_t modal_hash(const sim_interpreter *interpreter) {
|
|
unsigned char bytes[sizeof(int) * 11 + sizeof(double) * 3]; size_t offset = 0;
|
|
memcpy(bytes + offset, &interpreter->state.units, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.distance_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.plane, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.motion_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.spindle_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &interpreter->state.feed, sizeof(double)); offset += sizeof(double);
|
|
memcpy(bytes + offset, &interpreter->state.speed, sizeof(double)); offset += sizeof(double);
|
|
{
|
|
interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta != NULL) {
|
|
memcpy(bytes + offset, &meta->coordinate_system, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->cutter_comp, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->cycle_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->feed_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->coolant_mode, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->selected_tool, sizeof(int)); offset += sizeof(int);
|
|
memcpy(bytes + offset, &meta->tool_length, sizeof(double)); offset += sizeof(double);
|
|
}
|
|
}
|
|
return fnv1a64(bytes, offset);
|
|
}
|
|
|
|
static int make_canonical(sim_interpreter *interpreter) {
|
|
int written; const sim_interpreter_state *s = &interpreter->state;
|
|
written = snprintf(interpreter->canonical, sizeof(interpreter->canonical),
|
|
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"modal_hash\":%llu,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
|
|
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, (unsigned long long)modal_hash(interpreter), s->motion_mode, s->plane, s->program_end,
|
|
s->speed, s->spindle_mode, s->status, s->units, s->u, s->v, s->w, s->x, s->y, s->z);
|
|
if (written < 0 || (size_t)written >= sizeof(interpreter->canonical)) return SIM_INTERP_ERR_BUFFER;
|
|
interpreter->canonical_length = (uint32_t)written; return SIM_INTERP_OK;
|
|
}
|
|
|
|
uint32_t sim_interpreter_size(void) { return (uint32_t)sizeof(sim_interpreter); }
|
|
|
|
int sim_interpreter_reset(sim_interpreter *interpreter) {
|
|
interpreter_meta *meta;
|
|
if (interpreter == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
memset(interpreter, 0, sizeof(*interpreter)); meta = meta_for(interpreter, 1); if (meta == NULL) return SIM_INTERP_ERR_BUFFER; meta_reset(meta, interpreter);
|
|
interpreter->state.units = SIM_INTERP_UNITS_MM; interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE; interpreter->state.plane = 17; interpreter->state.motion_mode = 1; interpreter->state.spindle_mode = 5; interpreter->state.status = SIM_INTERP_OK; meta->feed_mode = 94;
|
|
return make_canonical(interpreter);
|
|
}
|
|
|
|
int sim_interpreter_init(sim_interpreter *interpreter) { return sim_interpreter_reset(interpreter); }
|
|
|
|
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length) {
|
|
sim_interpreter_state previous; sim_interpreter_state next; interpreter_meta *meta; expression_parser parser; const char *end;
|
|
int groups[8] = {0}, seen_axis[9] = {0}, seen_feed = 0, seen_speed = 0, seen_line = 0, seen_p = 0, seen_r = 0, seen_l = 0, seen_g10 = 0, seen_g53 = 0, result = SIM_INTERP_OK;
|
|
int seen_center[3] = {0};
|
|
double axis_values[9] = {0}, center_values[3] = {0}, feed_value = 0.0, speed_value = 0.0, p_value = 0.0, r_value = 0.0, l_value = 0.0;
|
|
if (interpreter == NULL || block == NULL || length == 0) return SIM_INTERP_ERR_ARGUMENT;
|
|
meta = meta_for(interpreter, 1); if (meta == NULL || !finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
|
|
if (interpreter->state.program_end) { set_error(interpreter, meta, SIM_INTERP_ERR_STATE, 1); return SIM_INTERP_ERR_STATE; }
|
|
previous = interpreter->state; next = interpreter->state; meta->has_motion = 0; parser.meta = meta; parser.cursor = block; parser.end = block + length; parser.pending_count = 0; end = parser.end;
|
|
while (parser.cursor < end) {
|
|
char letter; double value; int index; uint32_t column;
|
|
while (parser.cursor < end && isspace((unsigned char)*parser.cursor)) ++parser.cursor;
|
|
if (parser.cursor >= end || *parser.cursor == ';') break;
|
|
if (*parser.cursor == '%') { ++parser.cursor; continue; }
|
|
if (*parser.cursor == '(') {
|
|
int closed = 0; ++parser.cursor; while (parser.cursor < end) { if (*parser.cursor++ == ')') { closed = 1; break; } }
|
|
if (!closed) { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, (uint32_t)(parser.cursor - block)); return SIM_INTERP_ERR_SYNTAX; }
|
|
continue;
|
|
}
|
|
if (*parser.cursor == '#') {
|
|
const char *assignment_start = parser.cursor; int named_slot = 0; double ignored;
|
|
result = parse_parameter_reference(&parser, &ignored, &named_slot); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, (uint32_t)(assignment_start - block + 1)); return result; }
|
|
skip_space(&parser); if (parser.cursor >= end || *parser.cursor != '=') { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
|
|
++parser.cursor; result = parse_expression(&parser, &value); if (result != SIM_INTERP_OK || !isfinite(value)) { if (result == SIM_INTERP_OK) result = SIM_INTERP_ERR_NONFINITE; set_error(interpreter, meta, result, (uint32_t)(assignment_start - block + 1)); return result; }
|
|
if (named_slot != 0) {
|
|
size_t n; int assigned = 0;
|
|
for (n = 0; n < NAMED_PARAMETER_CAPACITY; ++n) if (parser.meta->named[n].used && named_hash(parser.meta->named[n].name) == (uint32_t)named_slot) { parser.meta->named[n].value = value; assigned = 1; break; }
|
|
if (!assigned) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
|
|
} else {
|
|
const char *ref = assignment_start + 1; unsigned long parameter_index = 0;
|
|
while (ref < end && isdigit((unsigned char)*ref)) { parameter_index = parameter_index * 10UL + (unsigned long)(*ref - '0'); ++ref; }
|
|
if (parameter_index < SIM_INTERPRETER_PARAMETER_MIN || parameter_index > SIM_INTERPRETER_PARAMETER_MAX || parser.pending_count >= PENDING_ASSIGNMENTS) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, (uint32_t)(assignment_start - block + 1)); return SIM_INTERP_ERR_PARAMETER; }
|
|
parser.pending_index[parser.pending_count] = (int)parameter_index; parser.pending_value[parser.pending_count++] = value;
|
|
}
|
|
continue;
|
|
}
|
|
column = (uint32_t)(parser.cursor - block + 1); letter = (char)toupper((unsigned char)*parser.cursor++);
|
|
if (letter < 'A' || letter > 'Z') { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, column); return SIM_INTERP_ERR_SYNTAX; }
|
|
result = parse_expression(&parser, &value); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, column); return result; }
|
|
switch (letter) {
|
|
case 'G': result = apply_g(&next, meta, value, groups); break;
|
|
case 'M': result = apply_m(&next, meta, value, groups); break;
|
|
case 'N': if (seen_line++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0 || value > 4294967295.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else next.line = (uint32_t)value; break;
|
|
case 'F': if (seen_feed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else feed_value = value; break;
|
|
case 'S': if (seen_speed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else speed_value = value; break;
|
|
case 'I': case 'J': case 'K': {
|
|
int center_index = letter == 'I' ? 0 : letter == 'J' ? 1 : 2;
|
|
if (seen_center[center_index]++) result = SIM_INTERP_ERR_STATE;
|
|
else center_values[center_index] = value;
|
|
break;
|
|
}
|
|
case 'L': if (seen_l++) result = SIM_INTERP_ERR_STATE; else l_value = value; break;
|
|
case 'Q': case 'D': case 'E': break;
|
|
case 'P': if (seen_p++) result = SIM_INTERP_ERR_STATE; else p_value = value; break;
|
|
case 'R': if (seen_r++) result = SIM_INTERP_ERR_STATE; else r_value = value; break;
|
|
case 'T': if (value < 0.0 || value > 1001.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else meta->selected_tool = (int)value; break;
|
|
case 'H': if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE; else meta->tool_length = value; break;
|
|
default:
|
|
index = axis_index(letter); if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE; else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE; else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE; if (result == SIM_INTERP_OK) axis_values[index] = value;
|
|
break;
|
|
}
|
|
if (letter == 'G' && code_is(value, 10)) seen_g10 = 1;
|
|
if (letter == 'G' && code_is(value, 53)) seen_g53 = 1;
|
|
if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, column); return result; }
|
|
}
|
|
skip_space(&parser); if (parser.cursor < end && *parser.cursor != ';') { set_error(interpreter, meta, SIM_INTERP_ERR_SYNTAX, (uint32_t)(parser.cursor - block + 1)); return SIM_INTERP_ERR_SYNTAX; }
|
|
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0); if (seen_speed) next.speed = speed_value;
|
|
{
|
|
size_t axis;
|
|
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) {
|
|
double axis_value = axis_values[axis]; double *destination = axis_pointer(&next, (int)axis);
|
|
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) axis_value *= 25.4;
|
|
if (seen_g10) {
|
|
int coordinate;
|
|
if (!seen_p || p_value < 0.0 || p_value > 9.0 || floor(p_value) != p_value || !seen_l || (!code_is(l_value, 2) && !code_is(l_value, 20))) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
|
|
coordinate = p_value == 0.0 ? (meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0) : (int)p_value - 1;
|
|
if (code_is(l_value, 20)) {
|
|
meta->coordinate_offsets[coordinate][axis] = *axis_pointer(&previous, (int)axis) - axis_value;
|
|
} else {
|
|
meta->coordinate_offsets[coordinate][axis] = axis_value;
|
|
}
|
|
} else if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += axis_value;
|
|
else *destination = axis_value + (seen_g53 ? 0.0 : meta->coordinate_offsets[meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0][axis]);
|
|
}
|
|
}
|
|
if (seen_g10 && !seen_axis[0] && !seen_axis[1] && !seen_axis[2] && !seen_axis[3] && !seen_axis[4] && !seen_axis[5] && !seen_axis[6] && !seen_axis[7] && !seen_axis[8]) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
|
|
if (meta->cycle_mode >= 81 && meta->cycle_mode <= 89 && seen_r && seen_axis[2]) {
|
|
double return_plane = r_value;
|
|
if (next.units == SIM_INTERP_UNITS_INCH) return_plane *= 25.4;
|
|
next.z = return_plane + meta->coordinate_offsets[meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0][2];
|
|
}
|
|
if (meta->tool_length != 0.0 && meta->cutter_comp == 43 && seen_axis[2]) next.z -= meta->tool_length;
|
|
if (!finite_state(&next)) { set_error(interpreter, meta, SIM_INTERP_ERR_NONFINITE, 1); return SIM_INTERP_ERR_NONFINITE; }
|
|
if (!seen_line) next.line = interpreter->state.line + 1u; next.status = SIM_INTERP_OK; interpreter->state = next;
|
|
{
|
|
size_t axis;
|
|
int has_axis_motion = 0;
|
|
int has_center = seen_center[0] || seen_center[1] || seen_center[2];
|
|
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) { has_axis_motion = 1; break; }
|
|
if (!seen_g10 && (has_axis_motion || has_center) && next.motion_mode >= 0 && next.motion_mode <= 3) {
|
|
sim_posemath_segment *segment = &meta->last_motion;
|
|
memset(segment, 0, sizeof(*segment));
|
|
if (meta->has_output_pose) segment->start = meta->last_output_pose;
|
|
else machine_pose_for_state(meta, &previous, seen_g53, &segment->start);
|
|
machine_pose_for_state(meta, &next, seen_g53, &segment->end);
|
|
segment->velocity = next.feed / 60.0;
|
|
segment->acceleration = 0.0;
|
|
segment->motion_type = next.motion_mode >= 2 ? SIM_POSEMATH_CIRCULAR : SIM_POSEMATH_LINEAR;
|
|
if (next.motion_mode >= 2 && seen_p && (p_value < 1.0 || p_value > 1000000.0 || floor(p_value) != p_value)) { set_error(interpreter, meta, SIM_INTERP_ERR_RANGE, 1); return SIM_INTERP_ERR_RANGE; }
|
|
segment->turn = (next.motion_mode == 2 ? -1 : 1) * (next.motion_mode >= 2 && seen_p ? (int)p_value : 1);
|
|
segment->center.x = segment->start.tran.x; segment->center.y = segment->start.tran.y; segment->center.z = segment->start.tran.z;
|
|
if (next.units == SIM_INTERP_UNITS_INCH) for (axis = 0; axis < 3; ++axis) center_values[axis] *= 25.4;
|
|
segment->center.x += center_values[0]; segment->center.y += center_values[1]; segment->center.z += center_values[2];
|
|
meta->last_output_pose = segment->end;
|
|
meta->has_output_pose = 1;
|
|
if (next.plane == 18) segment->normal.y = 1.0;
|
|
else if (next.plane == 19) segment->normal.x = 1.0;
|
|
else segment->normal.z = 1.0;
|
|
/* LinuxCNC applies inverse-time feed to G1/G2/G3. G0 remains a
|
|
* rapid move and does not require an F word while G93 is modal. */
|
|
if (meta->feed_mode == 93 && next.motion_mode != 0) {
|
|
const double dx = segment->end.tran.x - segment->start.tran.x;
|
|
const double dy = segment->end.tran.y - segment->start.tran.y;
|
|
const double dz = segment->end.tran.z - segment->start.tran.z;
|
|
const double da = segment->end.a - segment->start.a;
|
|
const double db = segment->end.b - segment->start.b;
|
|
const double dc = segment->end.c - segment->start.c;
|
|
const double du = segment->end.u - segment->start.u;
|
|
const double dv = segment->end.v - segment->start.v;
|
|
const double dw = segment->end.w - segment->start.w;
|
|
double path_length = sqrt(dx * dx + dy * dy + dz * dz + da * da + db * db + dc * dc + du * du + dv * dv + dw * dw);
|
|
if (next.motion_mode >= 2) {
|
|
const double sx = segment->start.tran.x - segment->center.x;
|
|
const double sy = segment->start.tran.y - segment->center.y;
|
|
const double sz = segment->start.tran.z - segment->center.z;
|
|
const double ex = segment->end.tran.x - segment->center.x;
|
|
const double ey = segment->end.tran.y - segment->center.y;
|
|
const double ez = segment->end.tran.z - segment->center.z;
|
|
const double radius = sqrt(sx * sx + sy * sy + sz * sz);
|
|
const double dot = sx * ex + sy * ey + sz * ez;
|
|
const double cross_normal = segment->normal.x * (sy * ez - sz * ey) + segment->normal.y * (sz * ex - sx * ez) + segment->normal.z * (sx * ey - sy * ex);
|
|
double angle = fabs(atan2(cross_normal, dot));
|
|
if (angle <= 1e-12) angle = 2.0 * 3.14159265358979323846;
|
|
angle += 2.0 * 3.14159265358979323846 * (fabs((double)segment->turn) - 1.0);
|
|
path_length = sqrt((radius * angle) * (radius * angle) + dz * dz + da * da + db * db + dc * dc + du * du + dv * dv + dw * dw);
|
|
}
|
|
if (!isfinite(path_length) || path_length <= 0.0 || next.feed <= 0.0) { set_error(interpreter, meta, SIM_INTERP_ERR_RANGE, 1); return SIM_INTERP_ERR_RANGE; }
|
|
segment->velocity = path_length * next.feed / 60.0;
|
|
}
|
|
meta->has_motion = 1;
|
|
}
|
|
}
|
|
{
|
|
size_t item; for (item = 0; item < parser.pending_count; ++item) meta->parameters[parser.pending_index[item]] = parser.pending_value[item];
|
|
}
|
|
interpreter->block_hash = fnv1a64((const unsigned char *)block, length); meta->error_column = 0; meta->error_code = SIM_INTERP_OK;
|
|
result = make_canonical(interpreter); if (result != SIM_INTERP_OK) { set_error(interpreter, meta, result, 1); return result; }
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) { if (interpreter == NULL || state == NULL) return SIM_INTERP_ERR_ARGUMENT; *state = interpreter->state; return SIM_INTERP_OK; }
|
|
|
|
int sim_interpreter_read_motion(const sim_interpreter *interpreter, sim_posemath_segment *segment) {
|
|
interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta == NULL || segment == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
if (!meta->has_motion) return SIM_INTERP_ERR_STATE;
|
|
*segment = meta->last_motion;
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
uint32_t sim_interpreter_motion_size(void) { return (uint32_t)sizeof(sim_posemath_segment); }
|
|
|
|
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity) {
|
|
if (interpreter == NULL) return 0; if (output == NULL || capacity == 0) return interpreter->canonical_length + 1u; if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1u; memcpy(output, interpreter->canonical, interpreter->canonical_length + 1u); return interpreter->canonical_length + 1u;
|
|
}
|
|
|
|
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : state_hash(interpreter); }
|
|
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : interpreter->block_hash; }
|
|
uint64_t sim_interpreter_modal_hash(const sim_interpreter *interpreter) { return interpreter == NULL ? 0 : modal_hash(interpreter); }
|
|
uint32_t sim_interpreter_error_column(const sim_interpreter *interpreter) { interpreter_meta *meta = meta_for(interpreter, 0); return meta == NULL ? 0 : meta->error_column; }
|
|
int32_t sim_interpreter_error_code(const sim_interpreter *interpreter) { interpreter_meta *meta = meta_for(interpreter, 0); return meta == NULL ? SIM_INTERP_ERR_ARGUMENT : meta->error_code; }
|
|
|
|
int sim_interpreter_parameter_set(sim_interpreter *interpreter, uint32_t index, double value) {
|
|
interpreter_meta *meta = meta_for(interpreter, 1); if (meta == NULL || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER; if (!isfinite(value)) return SIM_INTERP_ERR_NONFINITE; meta->parameters[index] = value; return SIM_INTERP_OK;
|
|
}
|
|
|
|
int sim_interpreter_parameter_get(const sim_interpreter *interpreter, uint32_t index, double *value) {
|
|
interpreter_meta *meta = meta_for(interpreter, 0); if (meta == NULL || value == NULL || index < SIM_INTERPRETER_PARAMETER_MIN || index > SIM_INTERPRETER_PARAMETER_MAX) return SIM_INTERP_ERR_PARAMETER; *value = meta->parameters[index]; return SIM_INTERP_OK;
|
|
}
|
|
|
|
int sim_interpreter_get_coordinates(const sim_interpreter *interpreter, sim_coordinate_context *context) {
|
|
interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta == NULL || context == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
memcpy(context->offsets, meta->coordinate_offsets, sizeof(context->offsets));
|
|
context->system = meta->coordinate_system > 0 ? meta->coordinate_system : 1;
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
int sim_interpreter_set_coordinates(sim_interpreter *interpreter, const sim_coordinate_context *context) {
|
|
interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta == NULL || context == NULL || context->system < 1 || context->system > 9) return SIM_INTERP_ERR_ARGUMENT;
|
|
memcpy(meta->coordinate_offsets, context->offsets, sizeof(meta->coordinate_offsets));
|
|
meta->coordinate_system = context->system;
|
|
return SIM_INTERP_OK;
|
|
}
|
|
|
|
int sim_interpreter_sync_position(sim_interpreter *interpreter, const sim_posemath_pose *position) {
|
|
interpreter_meta *meta = meta_for(interpreter, 0);
|
|
if (meta == NULL || position == NULL) return SIM_INTERP_ERR_ARGUMENT;
|
|
memcpy(&interpreter->state.x, &position->tran.x, 9 * sizeof(double));
|
|
meta->last_output_pose = *position;
|
|
meta->has_output_pose = 1;
|
|
meta->has_motion = 0;
|
|
interpreter->state.status = SIM_INTERP_OK;
|
|
return SIM_INTERP_OK;
|
|
}
|