wangdequan/linuxcncWebWASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Complete XYZAC LinuxCNC WASM parity and smooth 3D preview

Browse Source
This commit is contained in:
wangdequan
2026-08-28 06:24:50 +08:00
parent e8fe323aab
commit 1a7995fa9b
181 changed files with 36854 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wasm-core/src/sim_device_io.c
View File

@@ -8,6 +8,8 @@ static int valid_device(const sim_device_io *device) {
return device != 0 && device->state.encoder_count > 0U && device->state.encoder_count <= SIM_DEVICE_MAX_ENCODERS && isfinite(device->cycle_time) && device->cycle_time > 0.0;
}
uint32_t sim_device_io_size(void) { return (uint32_t)sizeof(sim_device_io); }
static int finite_value(double value) { return isfinite(value); }
static void set_error(sim_device_io *device, int error) {

22
wasm-core/src/sim_hal.c
View File

@@ -174,6 +174,17 @@ int32_t sim_hal_set_pin(const char *name, sim_hal_value value) {
return SIM_HAL_OK;
}
int32_t sim_hal_set_pin_scalar(const char *name, uint32_t type, double value) {
sim_hal_value scalar = {0};
scalar.type = type;
if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
else return SIM_HAL_ERR_TYPE;
return sim_hal_set_pin(name, scalar);
}
int32_t sim_hal_get_pin(const char *name, sim_hal_value *out) {
int index = find_pin(name);
if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;
@@ -191,6 +202,17 @@ int32_t sim_hal_set_param(const char *name, sim_hal_value value) {
return SIM_HAL_OK;
}
int32_t sim_hal_set_param_scalar(const char *name, uint32_t type, double value) {
sim_hal_value scalar = {0};
scalar.type = type;
if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
else return SIM_HAL_ERR_TYPE;
return sim_hal_set_param(name, scalar);
}
int32_t sim_hal_get_param(const char *name, sim_hal_value *out) {
int index = find_param(name);
if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;

107
wasm-core/src/sim_interpreter.c
View File

@@ -9,7 +9,7 @@
/* 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 4u
#define META_SLOTS 32u
#define NAMED_PARAMETER_CAPACITY 16u
#define NAMED_PARAMETER_LENGTH 32u
#define PENDING_ASSIGNMENTS 32u
@@ -28,11 +28,15 @@ typedef struct interpreter_meta {
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;
@@ -92,6 +96,20 @@ static int axis_index(char letter) {
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) {
int coordinate = meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0;
const double *offset = meta->coordinate_offsets[coordinate];
pose->tran.x = state->x + (machine_coordinates ? 0.0 : offset[0]);
pose->tran.y = state->y + (machine_coordinates ? 0.0 : offset[1]);
pose->tran.z = state->z + (machine_coordinates ? 0.0 : offset[2]);
pose->a = state->a + (machine_coordinates ? 0.0 : offset[3]);
pose->b = state->b + (machine_coordinates ? 0.0 : offset[4]);
pose->c = state->c + (machine_coordinates ? 0.0 : offset[5]);
pose->u = state->u + (machine_coordinates ? 0.0 : offset[6]);
pose->v = state->v + (machine_coordinates ? 0.0 : offset[7]);
pose->w = state->w + (machine_coordinates ? 0.0 : offset[8]);
}
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; }
@@ -166,6 +184,13 @@ static int parameter_lookup(const expression_parser *parser, uint32_t index, dou
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; }
}
@@ -317,10 +342,12 @@ static int apply_g(sim_interpreter_state *state, interpreter_meta *meta, double
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, 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(code, 53)) return SIM_INTERP_OK;
if (code_is(code, 10)) return SIM_INTERP_OK;
return SIM_INTERP_ERR_UNKNOWN_CODE;
}
@@ -333,6 +360,7 @@ static int apply_m(sim_interpreter_state *state, interpreter_meta *meta, double
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;
@@ -358,6 +386,7 @@ static uint64_t state_hash(const sim_interpreter *interpreter) {
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));
@@ -367,7 +396,7 @@ static uint64_t state_hash(const sim_interpreter *interpreter) {
}
static uint64_t modal_hash(const sim_interpreter *interpreter) {
unsigned char bytes[sizeof(int) * 10 + sizeof(double) * 3]; size_t offset = 0;
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);
@@ -381,6 +410,7 @@ static uint64_t modal_hash(const sim_interpreter *interpreter) {
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);
@@ -405,7 +435,7 @@ 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;
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);
}
@@ -413,9 +443,9 @@ int sim_interpreter_init(sim_interpreter *interpreter) { return sim_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[6] = {0}, seen_axis[9] = {0}, seen_feed = 0, seen_speed = 0, seen_line = 0, seen_p = 0, seen_r = 0, seen_g10 = 0, result = SIM_INTERP_OK;
int groups[7] = {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;
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; }
@@ -462,7 +492,8 @@ int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uin
else center_values[center_index] = value;
break;
}
case 'L': case 'Q': case 'D': case 'E': 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;
@@ -472,6 +503,7 @@ int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uin
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; }
@@ -482,10 +514,18 @@ int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uin
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) {
if (!seen_p || p_value < 1.0 || p_value > 9.0 || floor(p_value) != p_value) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
meta->coordinate_offsets[(int)p_value - 1][axis] = axis_value;
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;
if (p_value == 0.0) meta->coordinate_l20 = 1;
*destination = 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 + meta->coordinate_offsets[meta->coordinate_system > 0 ? meta->coordinate_system - 1 : 0][axis];
else *destination = axis_value + (seen_g53 || meta->coordinate_l20 ? 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]) { set_error(interpreter, meta, SIM_INTERP_ERR_PARAMETER, 1); return SIM_INTERP_ERR_PARAMETER; }
@@ -502,25 +542,54 @@ int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uin
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 ((has_axis_motion || has_center) && next.motion_mode >= 0 && next.motion_mode <= 3) {
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));
segment->start.tran.x = previous.x; segment->start.tran.y = previous.y; segment->start.tran.z = previous.z;
segment->start.a = previous.a; segment->start.b = previous.b; segment->start.c = previous.c;
segment->start.u = previous.u; segment->start.v = previous.v; segment->start.w = previous.w;
segment->end.tran.x = next.x; segment->end.tran.y = next.y; segment->end.tran.z = next.z;
segment->end.a = next.a; segment->end.b = next.b; segment->end.c = next.c;
segment->end.u = next.u; segment->end.v = next.v; segment->end.w = next.w;
segment->velocity = next.feed;
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;
segment->turn = next.motion_mode == 2 ? -1 : 1;
segment->center.x = previous.x; segment->center.y = previous.y; segment->center.z = previous.z;
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;
if (meta->feed_mode == 93) {
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;
}
}

2
wasm-core/src/sim_io.c
View File

@@ -34,6 +34,8 @@ static int valid_io(const sim_io *io) {
io->state.spindle_count <= SIM_IO_MAX_SPINDLES;
}
uint32_t sim_io_size(void) { return (uint32_t)sizeof(sim_io); }
static int valid_spindle(const sim_io *io, uint32_t spindle) {
return valid_io(io) && spindle < io->state.spindle_count &&
io->state.spindles[spindle].registered;

8
wasm-core/src/sim_motion_queue.c
View File

@@ -53,7 +53,13 @@ static double segment_length(const sim_posemath_segment *segment) {
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;
double value = dx * dx + dy * dy + dz * dz;
double value = dx * dx + dy * dy + dz * dz +
(segment->end.a - segment->start.a) * (segment->end.a - segment->start.a) +
(segment->end.b - segment->start.b) * (segment->end.b - segment->start.b) +
(segment->end.c - segment->start.c) * (segment->end.c - segment->start.c) +
(segment->end.u - segment->start.u) * (segment->end.u - segment->start.u) +
(segment->end.v - segment->start.v) * (segment->end.v - segment->start.v) +
(segment->end.w - segment->start.w) * (segment->end.w - segment->start.w);
if (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
const double sx = segment->start.tran.x - segment->center.x;
const double sy = segment->start.tran.y - segment->center.y;

8
wasm-core/src/sim_posemath.c
View File

@@ -117,7 +117,13 @@ int sim_posemath_segment_validate(const sim_posemath_segment *segment) {
if (segment->velocity < 0.0 || segment->acceleration < 0.0 || segment->velocity > 1e9 || segment->acceleration > 1e12) return SIM_POSEMATH_ERR_RANGE;
length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
(segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z) +
(segment->end.a - segment->start.a) * (segment->end.a - segment->start.a) +
(segment->end.b - segment->start.b) * (segment->end.b - segment->start.b) +
(segment->end.c - segment->start.c) * (segment->end.c - segment->start.c) +
(segment->end.u - segment->start.u) * (segment->end.u - segment->start.u) +
(segment->end.v - segment->start.v) * (segment->end.v - segment->start.v) +
(segment->end.w - segment->start.w) * (segment->end.w - segment->start.w);
if (!isfinite(length_sq) || (length_sq <= 1e-24 && segment->motion_type != SIM_POSEMATH_CIRCULAR)) return SIM_POSEMATH_ERR_DEGENERATE;
if (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
normal_sq = segment->normal.x * segment->normal.x + segment->normal.y * segment->normal.y + segment->normal.z * segment->normal.z;

410
wasm-core/src/sim_program.c Normal file
View File

@@ -0,0 +1,410 @@
#include "sim_program.h"
#include <ctype.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
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 ^= bytes[index]; hash *= UINT64_C(1099511628211); }
return hash;
}
static int control_line(const char *line) {
const char *cursor = line;
while (*cursor != '\0' && isspace((unsigned char)*cursor)) ++cursor;
if (*cursor == 'O' || *cursor == 'o') return 1;
if ((cursor[0] == 'M' || cursor[0] == 'm') && (cursor[1] == '8' || cursor[1] == '9')) return 1;
if ((cursor[0] == 'M' || cursor[0] == 'm') && (cursor[1] == '9' || cursor[1] == '8')) return 1;
return 0;
}
static uint32_t line_length(const char *line, uint32_t maximum) {
uint32_t length = 0;
while (length < maximum && line[length] != '\0' && line[length] != '\n' && line[length] != '\r') ++length;
return length;
}
static int span_equal(const char *value, size_t length, const char *expected) {
size_t index;
if (strlen(expected) != length) return 0;
for (index = 0; index < length; ++index)
if (tolower((unsigned char)value[index]) != tolower((unsigned char)expected[index])) return 0;
return 1;
}
static int parse_oword(const char *line, uint32_t length, char *name, size_t name_capacity,
const char **verb, size_t *verb_length, const char **arguments) {
const char *cursor = line;
const char *end = line + length;
size_t name_length = 0;
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
if (cursor >= end || tolower((unsigned char)*cursor++) != 'o' || cursor >= end || *cursor++ != '<') return 0;
while (cursor < end && *cursor != '>') {
if (name_length + 1u >= name_capacity) return 0;
name[name_length++] = (char)tolower((unsigned char)*cursor++);
}
if (cursor >= end || *cursor++ != '>' || name_length == 0) return 0;
name[name_length] = '\0';
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
*verb = cursor;
while (cursor < end && isalpha((unsigned char)*cursor)) ++cursor;
*verb_length = (size_t)(cursor - *verb);
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
*arguments = cursor;
return *verb_length > 0;
}
int32_t sim_program_requires_subroutines(const char *source, uint32_t length) {
uint32_t offset = 0;
if (source == NULL || length == 0 || length >= SIM_PROGRAM_SOURCE_CAPACITY) return SIM_PROGRAM_ERR_ARGUMENT;
while (offset < length) {
uint32_t available = length - offset;
uint32_t line_size = line_length(source + offset, available);
char name[SIM_PROGRAM_SUBROUTINE_NAME_CAPACITY] = {0};
const char *verb = NULL, *arguments = NULL;
size_t verb_length = 0;
if (parse_oword(source + offset, line_size, name, sizeof(name), &verb, &verb_length, &arguments) && span_equal(verb, verb_length, "call")) return 1;
offset += line_size;
while (offset < length && (source[offset] == '\n' || source[offset] == '\r')) ++offset;
}
return 0;
}
static void reindex_source(sim_program *program) {
uint32_t index;
program->line_count = program->source_length == 0 ? 0 : 1;
program->line_offsets[0] = 0;
for (index = 0; index < program->source_length; ++index) if (program->source[index] == '\n' && index + 1u < program->source_length) {
if (program->line_count < SIM_PROGRAM_LINE_CAPACITY) program->line_offsets[program->line_count++] = index + 1u;
}
program->line_offsets[program->line_count] = program->source_length;
}
static int append_source(sim_program *program, const char *source, uint32_t length) {
uint32_t separator = program->source_length > 0 && program->source[program->source_length - 1u] != '\n' ? 1u : 0u;
if (source == NULL || length == 0 || program->source_length + separator + length >= SIM_PROGRAM_SOURCE_CAPACITY) return SIM_PROGRAM_ERR_RANGE;
if (separator) program->source[program->source_length++] = '\n';
memcpy(program->source + program->source_length, source, length);
program->source_length += length;
program->source[program->source_length] = '\0';
reindex_source(program);
return SIM_PROGRAM_OK;
}
static int scan_subroutines(sim_program *program) {
uint32_t line;
program->subroutine_count = 0;
for (line = program->main_line_count; line < program->line_count; ++line) {
const char *start = program->source + program->line_offsets[line];
uint32_t maximum = program->line_offsets[line + 1u] - program->line_offsets[line];
uint32_t length = line_length(start, maximum);
char name[SIM_PROGRAM_SUBROUTINE_NAME_CAPACITY] = {0};
const char *verb = NULL, *arguments = NULL;
size_t verb_length = 0;
uint32_t end_line;
if (!parse_oword(start, length, name, sizeof(name), &verb, &verb_length, &arguments) || !span_equal(verb, verb_length, "sub")) continue;
for (end_line = line + 1u; end_line < program->line_count; ++end_line) {
const char *end_start = program->source + program->line_offsets[end_line];
uint32_t end_maximum = program->line_offsets[end_line + 1u] - program->line_offsets[end_line];
uint32_t end_length = line_length(end_start, end_maximum);
char end_name[SIM_PROGRAM_SUBROUTINE_NAME_CAPACITY] = {0};
const char *end_verb = NULL, *end_arguments = NULL;
size_t end_verb_length = 0;
if (parse_oword(end_start, end_length, end_name, sizeof(end_name), &end_verb, &end_verb_length, &end_arguments) && strcmp(name, end_name) == 0 && span_equal(end_verb, end_verb_length, "endsub")) break;
}
if (end_line >= program->line_count || program->subroutine_count >= SIM_PROGRAM_SUBROUTINE_CAPACITY) return SIM_PROGRAM_ERR_CONTROL;
strncpy(program->subroutines[program->subroutine_count].name, name, SIM_PROGRAM_SUBROUTINE_NAME_CAPACITY - 1u);
program->subroutines[program->subroutine_count].body_line = line + 1u;
program->subroutines[program->subroutine_count].end_line = end_line;
program->subroutine_count += 1u;
line = end_line;
}
return SIM_PROGRAM_OK;
}
static const sim_program_subroutine *find_subroutine(const sim_program *program, const char *name) {
uint32_t index;
for (index = 0; index < program->subroutine_count; ++index)
if (strcmp(program->subroutines[index].name, name) == 0) return &program->subroutines[index];
return NULL;
}
static int evaluate_call_arguments(sim_program *program, const char *cursor, const char *end,
double *values, uint32_t *count) {
uint32_t item = 0;
while (cursor < end) {
const char *start;
const char *finish;
int depth = 0;
char assignment[512];
int written;
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
if (cursor >= end || *cursor == ';' || *cursor == '(') break;
if (item >= SIM_PROGRAM_CALL_PARAMETER_CAPACITY) return SIM_PROGRAM_ERR_CONTROL;
start = cursor;
if (*cursor == '[') {
do {
if (*cursor == '[') depth += 1;
else if (*cursor == ']') depth -= 1;
++cursor;
} while (cursor < end && depth > 0);
if (depth != 0) return SIM_PROGRAM_ERR_CONTROL;
finish = cursor;
} else {
while (cursor < end && !isspace((unsigned char)*cursor) && *cursor != ';' && *cursor != '(') ++cursor;
finish = cursor;
}
written = snprintf(assignment, sizeof(assignment), "#5000=%.*s", (int)(finish - start), start);
if (written <= 0 || (size_t)written >= sizeof(assignment) || sim_interpreter_execute(&program->interpreter, assignment, (uint32_t)written) != SIM_INTERP_OK || sim_interpreter_parameter_get(&program->interpreter, 5000, &values[item]) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
item += 1u;
}
*count = item;
return SIM_PROGRAM_OK;
}
static int begin_call(sim_program *program, const char *name, const char *arguments, const char *end) {
const sim_program_subroutine *subroutine = find_subroutine(program, name);
sim_program_call_frame *frame;
double values[SIM_PROGRAM_CALL_PARAMETER_CAPACITY] = {0};
uint32_t count = 0, index;
int result;
if (subroutine == NULL || program->call_depth >= SIM_PROGRAM_CALL_DEPTH) return SIM_PROGRAM_ERR_CONTROL;
result = evaluate_call_arguments(program, arguments, end, values, &count);
if (result != SIM_PROGRAM_OK) return result;
frame = &program->call_stack[program->call_depth++];
frame->return_line = program->current_line;
for (index = 0; index < SIM_PROGRAM_CALL_PARAMETER_CAPACITY; ++index) {
if (sim_interpreter_parameter_get(&program->interpreter, index + 1u, &frame->parameters[index]) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
if (sim_interpreter_parameter_set(&program->interpreter, index + 1u, index < count ? values[index] : 0.0) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
}
program->current_line = subroutine->body_line;
return SIM_PROGRAM_OK;
}
static int end_call(sim_program *program) {
sim_program_call_frame *frame;
uint32_t index;
if (program->call_depth == 0) return SIM_PROGRAM_ERR_CONTROL;
frame = &program->call_stack[--program->call_depth];
for (index = 0; index < SIM_PROGRAM_CALL_PARAMETER_CAPACITY; ++index)
if (sim_interpreter_parameter_set(&program->interpreter, index + 1u, frame->parameters[index]) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
program->current_line = frame->return_line;
return SIM_PROGRAM_OK;
}
static int switch_mcode(const char *block, uint32_t length) {
const char *cursor = block;
const char *end = block + length;
while (cursor < end) {
if (*cursor == ';') break;
if (*cursor == '(') { while (cursor < end && *cursor++ != ')') {} continue; }
if (toupper((unsigned char)*cursor) == 'M') {
char *number_end = NULL;
long code = strtol(cursor + 1, &number_end, 10);
if (number_end != cursor + 1 && (code == 428 || code == 429 || code == 430)) return (int)code;
}
++cursor;
}
return 0;
}
static void reset_runtime(sim_program *program) {
sim_interpreter_reset(&program->interpreter);
program->source_length = 0;
program->line_count = 0;
program->main_line_count = 0;
program->current_line = 0;
program->status = SIM_PROGRAM_IDLE;
program->last_error = 0;
program->error_line = 0;
program->has_motion = 0;
program->subroutine_count = 0;
program->call_depth = 0;
program->has_control = 0;
program->control_code = 0;
program->rapid_velocity = 35.0;
memset(&program->motion, 0, sizeof(program->motion));
memset(program->subroutines, 0, sizeof(program->subroutines));
memset(program->call_stack, 0, sizeof(program->call_stack));
}
uint32_t sim_program_size(void) { return (uint32_t)sizeof(sim_program); }
int32_t sim_program_init(sim_program *program) {
if (program == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
memset(program, 0, sizeof(*program));
if (sim_interpreter_init(&program->interpreter) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
program->status = SIM_PROGRAM_IDLE;
return SIM_PROGRAM_OK;
}
int32_t sim_program_reset(sim_program *program) {
if (program == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
if (sim_interpreter_reset(&program->interpreter) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
reset_runtime(program);
return SIM_PROGRAM_OK;
}
int32_t sim_program_load(sim_program *program, const char *source, uint32_t length) {
if (program == NULL || source == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
if (length == 0 || length >= SIM_PROGRAM_SOURCE_CAPACITY) return SIM_PROGRAM_ERR_RANGE;
if (sim_program_reset(program) != SIM_PROGRAM_OK) return SIM_PROGRAM_ERR_INTERPRETER;
if (append_source(program, source, length) != SIM_PROGRAM_OK) return SIM_PROGRAM_ERR_RANGE;
program->main_line_count = program->line_count;
program->status = SIM_PROGRAM_LOADED;
return SIM_PROGRAM_OK;
}
int32_t sim_program_add_subroutine(sim_program *program, const char *source, uint32_t length) {
int result;
if (program == NULL || source == NULL || program->status != SIM_PROGRAM_LOADED) return SIM_PROGRAM_ERR_STATE;
result = append_source(program, source, length);
if (result != SIM_PROGRAM_OK) return result;
return scan_subroutines(program);
}
int32_t sim_program_set_rapid_velocity(sim_program *program, double velocity) {
if (program == NULL || !isfinite(velocity) || velocity <= 0.0 || velocity > 1e9) return SIM_PROGRAM_ERR_RANGE;
program->rapid_velocity = velocity;
return SIM_PROGRAM_OK;
}
int32_t sim_program_start(sim_program *program) {
if (program == NULL || program->status == SIM_PROGRAM_IDLE) return SIM_PROGRAM_ERR_STATE;
if (program->status == SIM_PROGRAM_COMPLETED || program->status == SIM_PROGRAM_ABORTED) return SIM_PROGRAM_ERR_STATE;
program->status = SIM_PROGRAM_RUNNING;
return SIM_PROGRAM_OK;
}
int32_t sim_program_pause(sim_program *program) {
if (program == NULL || program->status != SIM_PROGRAM_RUNNING) return SIM_PROGRAM_ERR_STATE;
program->status = SIM_PROGRAM_PAUSED;
return SIM_PROGRAM_OK;
}
int32_t sim_program_resume(sim_program *program) {
if (program == NULL || program->status != SIM_PROGRAM_PAUSED) return SIM_PROGRAM_ERR_STATE;
program->status = SIM_PROGRAM_RUNNING;
return SIM_PROGRAM_OK;
}
int32_t sim_program_abort(sim_program *program) {
if (program == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
program->status = SIM_PROGRAM_ABORTED;
program->has_motion = 0;
return SIM_PROGRAM_OK;
}
int32_t sim_program_execute_block(sim_program *program, const char *block, uint32_t length) {
sim_posemath_segment segment;
int result;
if (program == NULL || block == NULL || length == 0) return SIM_PROGRAM_ERR_ARGUMENT;
if (control_line(block)) return SIM_PROGRAM_ERR_CONTROL;
program->has_control = 0;
program->control_code = switch_mcode(block, length);
if (program->control_code != 0) program->has_control = 1;
result = sim_interpreter_execute(&program->interpreter, block, length);
if (result != SIM_INTERP_OK) { program->last_error = result; return SIM_PROGRAM_ERR_INTERPRETER; }
memset(&segment, 0, sizeof(segment));
if (sim_interpreter_read_motion(&program->interpreter, &segment) == SIM_INTERP_OK && segment.motion_type != 0) {
if (program->interpreter.state.motion_mode == 0) segment.velocity = program->rapid_velocity;
program->motion = segment; program->has_motion = 1;
} else program->has_motion = 0;
return SIM_PROGRAM_OK;
}
int32_t sim_program_step(sim_program *program) {
uint32_t line;
if (program == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
if (program->status == SIM_PROGRAM_PAUSED) return SIM_PROGRAM_ERR_STATE;
if (program->status != SIM_PROGRAM_RUNNING) return SIM_PROGRAM_ERR_STATE;
program->has_motion = 0;
program->has_control = 0;
program->control_code = 0;
while (program->current_line < program->line_count) {
const char *line_start = program->source + program->line_offsets[program->current_line];
uint32_t maximum = program->line_offsets[program->current_line + 1u] - program->line_offsets[program->current_line];
uint32_t length = line_length(line_start, maximum);
program->current_line += 1;
line = program->current_line;
while (length > 0 && isspace((unsigned char)line_start[length - 1u])) --length;
if (length == 0 || line_start[0] == ';') continue;
{
char name[SIM_PROGRAM_SUBROUTINE_NAME_CAPACITY] = {0};
const char *verb = NULL, *arguments = NULL;
size_t verb_length = 0;
if (parse_oword(line_start, length, name, sizeof(name), &verb, &verb_length, &arguments)) {
int control_result;
if (span_equal(verb, verb_length, "call")) control_result = begin_call(program, name, arguments, line_start + length);
else if (span_equal(verb, verb_length, "endsub")) control_result = end_call(program);
else if (span_equal(verb, verb_length, "sub")) {
const sim_program_subroutine *declared = find_subroutine(program, name);
program->current_line = declared ? declared->end_line + 1u : program->line_count;
continue;
}
else control_result = SIM_PROGRAM_ERR_CONTROL;
if (control_result != SIM_PROGRAM_OK) { program->last_error = control_result; program->error_line = line; program->status = SIM_PROGRAM_ERROR; return control_result; }
return SIM_PROGRAM_OK;
}
}
if (control_line(line_start)) { program->last_error = SIM_PROGRAM_ERR_CONTROL; program->error_line = line; program->status = SIM_PROGRAM_ERROR; return SIM_PROGRAM_ERR_CONTROL; }
if (sim_program_execute_block(program, line_start, length) != SIM_PROGRAM_OK) { program->error_line = line; program->status = SIM_PROGRAM_ERROR; return SIM_PROGRAM_ERR_INTERPRETER; }
if (program->interpreter.state.program_end || (program->call_depth == 0 && program->current_line >= program->main_line_count)) program->status = SIM_PROGRAM_COMPLETED;
return SIM_PROGRAM_OK;
}
program->status = SIM_PROGRAM_COMPLETED;
return SIM_PROGRAM_OK;
}
int32_t sim_program_seek(sim_program *program, uint32_t line) {
if (program == NULL || line > program->line_count) return SIM_PROGRAM_ERR_RANGE;
if (sim_interpreter_reset(&program->interpreter) != SIM_INTERP_OK) return SIM_PROGRAM_ERR_INTERPRETER;
program->current_line = 0; program->status = SIM_PROGRAM_PAUSED; program->has_motion = 0;
while (program->current_line < line) {
int result = sim_program_step(program);
if (result != SIM_PROGRAM_OK && result != SIM_PROGRAM_ERR_STATE) return result;
if (program->status == SIM_PROGRAM_COMPLETED) break;
program->status = SIM_PROGRAM_PAUSED;
if (program->current_line >= line) break;
program->status = SIM_PROGRAM_RUNNING;
}
program->status = SIM_PROGRAM_PAUSED;
return SIM_PROGRAM_OK;
}
int32_t sim_program_read_motion(const sim_program *program, sim_posemath_segment *segment) {
if (program == NULL || segment == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
if (!program->has_motion) return SIM_PROGRAM_ERR_STATE;
*segment = program->motion;
return SIM_PROGRAM_OK;
}
int32_t sim_program_clear_motion(sim_program *program) {
if (program == NULL) return SIM_PROGRAM_ERR_ARGUMENT;
program->has_motion = 0;
memset(&program->motion, 0, sizeof(program->motion));
return SIM_PROGRAM_OK;
}
uint32_t sim_program_canonical(const sim_program *program, char *output, uint32_t capacity) {
char discard[1];
char *destination = output == NULL ? discard : output;
uint32_t destination_capacity = output == NULL ? 1 : capacity;
int written;
if (program == NULL) return 0;
written = snprintf(destination, destination_capacity,
"{\"call_depth\":%u,\"control_code\":%d,\"current_line\":%u,\"error_line\":%u,\"has_control\":%u,\"has_motion\":%u,\"last_error\":%d,\"line_count\":%u,\"main_line_count\":%u,\"motion_mode\":%d,\"rapid_velocity\":%.17g,\"schema\":\"sim_program.v1\",\"status\":%u,\"subroutine_count\":%u,\"source_length\":%u,\"interpreter_state_hash\":\"%llu\"}",
program->call_depth, program->control_code, program->current_line, program->error_line, program->has_control, program->has_motion, program->last_error, program->line_count, program->main_line_count, program->interpreter.state.motion_mode, program->rapid_velocity, program->status, program->subroutine_count, program->source_length,
(unsigned long long)sim_interpreter_state_hash(&program->interpreter));
return written < 0 ? 0 : (uint32_t)written + 1u;
}
uint64_t sim_program_state_hash(const sim_program *program) {
char canonical[SIM_PROGRAM_CANONICAL_CAPACITY];
uint32_t length = sim_program_canonical(program, canonical, sizeof(canonical));
return length == 0 ? 0 : fnv1a64((const unsigned char *)canonical, length - 1u);
}

2
wasm-core/src/sim_task.c
View File

@@ -68,7 +68,7 @@ static int transition_allowed(uint32_t from, uint32_t to) {
if (from == SIM_TASK_PAUSED) return to == SIM_TASK_RUNNING || to == SIM_TASK_STOPPED || to == SIM_TASK_ESTOP || to == SIM_TASK_FAULT;
if (from == SIM_TASK_ESTOP) return to == SIM_TASK_STOPPED || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_FAULT) return to == SIM_TASK_STOPPED || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_STOPPED) return to == SIM_TASK_READY || to == SIM_TASK_DESTROYED;
if (from == SIM_TASK_STOPPED) return to == SIM_TASK_READY || to == SIM_TASK_ESTOP || to == SIM_TASK_DESTROYED;
return 0;
}