99 lines
5.1 KiB
C
99 lines
5.1 KiB
C
#include "sim_kinematics.h"
|
|
|
|
#include <string.h>
|
|
|
|
static uint64_t user_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_static_name(const char *name) {
|
|
size_t index;
|
|
if (name == 0 || name[0] == '\0') return 0;
|
|
for (index = 0; index < SIM_KIN_USER_NAME_CAPACITY && name[index] != '\0'; ++index) {
|
|
unsigned char value = (unsigned char)name[index];
|
|
if (value < 0x21 || value > 0x7e || value == '/' || value == '\\') return 0;
|
|
}
|
|
return index < SIM_KIN_USER_NAME_CAPACITY;
|
|
}
|
|
|
|
int sim_kinematics_user_init(sim_kinematics_user *user,
|
|
const sim_kinematics_user_descriptor *descriptor) {
|
|
sim_kinematics_config config;
|
|
uint32_t required = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
|
|
SIM_KIN_USER_CAP_STATIC;
|
|
int result;
|
|
if (user == 0 || descriptor == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (descriptor->abi_version != SIM_KIN_USER_ABI_VERSION) return SIM_KIN_ERR_ABI_VERSION;
|
|
if ((descriptor->capabilities & required) != required ||
|
|
(descriptor->capabilities & ~required) != 0) return SIM_KIN_ERR_CAPABILITY;
|
|
if (!valid_static_name(descriptor->name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
|
|
config = descriptor->config;
|
|
if (descriptor->implementation == SIM_KIN_USER_IDENTITY) config.type = SIM_KIN_TRIVKINS;
|
|
else if (descriptor->implementation == SIM_KIN_USER_GENSER) config.type = SIM_KIN_GENSER;
|
|
else return SIM_KIN_ERR_MISSING_MODULE;
|
|
result = sim_kinematics_init(&user->module, &config);
|
|
if (result != SIM_KIN_OK) return result;
|
|
user->descriptor = *descriptor;
|
|
user->descriptor.config = user->module.config;
|
|
user->forward_calls = 0;
|
|
user->inverse_calls = 0;
|
|
user->initialized = 1;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
int sim_kinematics_user_bind_switch(sim_kinematics_user *user,
|
|
sim_kinematics_switch *switcher, int mode,
|
|
uint8_t fwd_iterates) {
|
|
if (user == 0 || switcher == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
|
|
return sim_kinematics_switch_register(switcher, mode, &user->module, fwd_iterates);
|
|
}
|
|
|
|
int sim_kinematics_user_forward(sim_kinematics_user *user, const double *joints,
|
|
uint32_t joint_count, sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info) {
|
|
if (user == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
|
|
user->forward_calls += 1;
|
|
return sim_kinematics_forward_ex(&user->module, joints, joint_count, 0, pose, info);
|
|
}
|
|
|
|
int sim_kinematics_user_inverse(sim_kinematics_user *user,
|
|
const sim_posemath_pose *pose, double *joints,
|
|
uint32_t joint_count,
|
|
sim_kinematics_result_info *info) {
|
|
if (user == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
|
|
user->inverse_calls += 1;
|
|
return sim_kinematics_inverse_ex(&user->module, pose, joint_count, 0, joints, info);
|
|
}
|
|
|
|
int sim_kinematics_user_reject_dynamic(const char *module_path) {
|
|
return module_path == 0 ? SIM_KIN_ERR_ARGUMENT : SIM_KIN_ERR_DYNAMIC_MODULE;
|
|
}
|
|
|
|
uint64_t sim_kinematics_user_state_hash(const sim_kinematics_user *user,
|
|
const double *joints, uint32_t joint_count,
|
|
const sim_posemath_pose *pose) {
|
|
unsigned char bytes[sizeof(uint32_t) * 5 + sizeof(int) + SIM_KIN_USER_NAME_CAPACITY + sizeof(uint64_t)];
|
|
uint64_t module_hash;
|
|
size_t offset = 0;
|
|
if (user == 0 || joints == 0 || pose == 0 || !user->initialized) return 0;
|
|
module_hash = sim_kinematics_state_hash(&user->module, joints, joint_count, pose);
|
|
memcpy(bytes + offset, &user->descriptor.abi_version, sizeof(user->descriptor.abi_version)); offset += sizeof(user->descriptor.abi_version);
|
|
memcpy(bytes + offset, &user->descriptor.capabilities, sizeof(user->descriptor.capabilities)); offset += sizeof(user->descriptor.capabilities);
|
|
memcpy(bytes + offset, &user->descriptor.implementation, sizeof(user->descriptor.implementation)); offset += sizeof(user->descriptor.implementation);
|
|
memcpy(bytes + offset, user->descriptor.name, sizeof(user->descriptor.name)); offset += sizeof(user->descriptor.name);
|
|
memcpy(bytes + offset, &user->initialized, sizeof(user->initialized)); offset += sizeof(user->initialized);
|
|
memcpy(bytes + offset, &user->forward_calls, sizeof(user->forward_calls)); offset += sizeof(user->forward_calls);
|
|
memcpy(bytes + offset, &user->inverse_calls, sizeof(user->inverse_calls)); offset += sizeof(user->inverse_calls);
|
|
memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
|
|
return user_fnv1a64(bytes, offset);
|
|
}
|