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

Implement WebAssembly runtime and kinematics coverage

Browse Source
This commit is contained in:
wangdequan
2026-08-21 06:04:48 +08:00
parent 5dd614c335
commit 554b72dae8
112 changed files with 47148 additions and 807 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
wasm-core/src/sim_motion_queue.c Normal file
View File

@@ -0,0 +1,154 @@
#include "sim_motion_queue.h"
#include <math.h>
#include <stdio.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 valid_config(double cycle_time, double velocity_limit) {
return isfinite(cycle_time) && isfinite(velocity_limit) && cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9;
}
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;
return isfinite(value) && value > 0.0 ? sqrt(value) : 0.0;
}
static int poses_match(const sim_posemath_pose *left, const sim_posemath_pose *right) {
const double *a = &left->tran.x;
const double *b = &right->tran.x;
size_t index;
for (index = 0; index < 9; ++index) if (fabs(a[index] - b[index]) > 1e-12) return 0;
return 1;
}
static void zero_state(sim_motion_state *state) {
memset(state, 0, sizeof(*state));
state->status = SIM_MOTION_OK;
}
int sim_motion_reset(sim_motion_queue *queue) {
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
zero_state(&queue->state);
return SIM_MOTION_OK;
}
int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_limit) {
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
if (!valid_config(cycle_time, velocity_limit)) return SIM_MOTION_ERR_RANGE;
queue->cycle_time = cycle_time; queue->velocity_limit = velocity_limit;
return sim_motion_reset(queue);
}
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
uint32_t index;
int result;
if (queue == 0 || command == 0) return SIM_MOTION_ERR_ARGUMENT;
result = sim_posemath_segment_validate(&command->segment);
if (result == SIM_POSEMATH_ERR_MOTION_TYPE) return SIM_MOTION_ERR_MOTION_TYPE;
if (result != SIM_POSEMATH_OK) return result == SIM_POSEMATH_ERR_NONFINITE ? SIM_MOTION_ERR_NONFINITE : SIM_MOTION_ERR_GEOMETRY;
if (queue->count >= SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_CAPACITY;
if (command->segment.velocity > queue->velocity_limit) return SIM_MOTION_ERR_RANGE;
if (queue->count > 0) {
index = (queue->head + queue->count - 1) % SIM_MOTION_QUEUE_CAPACITY;
if (!poses_match(&queue->commands[index].segment.end, &command->segment.start)) return SIM_MOTION_ERR_STATE;
} else if (queue->state.tick > 0 && !poses_match(&queue->state.position, &command->segment.start)) {
return SIM_MOTION_ERR_STATE;
}
index = (queue->head + queue->count) % SIM_MOTION_QUEUE_CAPACITY;
queue->commands[index] = *command;
queue->commands[index].id = queue->next_id++;
if (id_out != 0) *id_out = queue->commands[index].id;
queue->count++;
return SIM_MOTION_OK;
}
int sim_motion_step(sim_motion_queue *queue) {
sim_motion_command *command;
double length;
double delta;
int result;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
command = &queue->commands[queue->head];
length = segment_length(&command->segment);
if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->state.active_id = command->id; queue->state.active_tag = command->tag; queue->state.motion_type = command->segment.motion_type;
delta = command->segment.velocity * queue->cycle_time / length;
if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
queue->state.progress += delta;
if (queue->state.progress >= 1.0) {
queue->state.progress = 1.0;
result = sim_posemath_segment_point(&command->segment, 1.0, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->head = (queue->head + 1) % SIM_MOTION_QUEUE_CAPACITY;
queue->count--;
queue->state.progress = 0.0;
queue->state.active_id = 0;
queue->state.active_tag = 0;
queue->state.motion_type = 0;
} else {
result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
}
queue->state.velocity = command->segment.velocity;
queue->tick++;
queue->state.tick = queue->tick;
queue->state.queue_depth = queue->count;
queue->state.status = SIM_MOTION_OK;
return SIM_MOTION_OK;
}
int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state) {
if (queue == 0 || state == 0) return SIM_MOTION_ERR_ARGUMENT;
*state = queue->state;
state->queue_depth = queue->count;
return SIM_MOTION_OK;
}
uint32_t sim_motion_queue_depth(const sim_motion_queue *queue) {
return queue == 0 ? 0 : queue->count;
}
uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
sim_motion_state state;
unsigned char bytes[sizeof(sim_motion_state)];
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
memset(bytes, 0, sizeof(bytes));
memcpy(bytes, &state.position, sizeof(state.position));
memcpy(bytes + sizeof(state.position), &state.velocity, sizeof(double));
memcpy(bytes + sizeof(state.position) + sizeof(double), &state.progress, sizeof(double));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2, &state.tick, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t), &state.queue_depth, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 2, &state.active_id, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 3, &state.active_tag, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4, &state.motion_type, sizeof(int));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int), &state.status, sizeof(int));
return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
}
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
sim_motion_state state;
char discard[1];
char *destination = output == 0 ? discard : output;
uint32_t destination_capacity = output == 0 ? 1 : capacity;
int written;
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
state.progress, state.queue_depth, state.status, state.tick, state.velocity);
if (written < 0) return 0;
if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
return (uint32_t)written + 1;
}